القائمة
SLAM (Simultaneous Localization and Mapping) — filtres particulaires, graph SLAM, EKF, FastSLAM, loop closure, ROS2 Cartographer, GTSAM
يُحدَّد Prompt الذي يراجع المصدر أولًا بشكل افتراضي. يمكنك التبديل إلى أمر مباشر أو تنزيل نسخة محلية.
اقرأ SKILL.md وأي ملفات مرافقة يعرضها SkillsMP قبل أن تقرر التثبيت.
استنادا إلى تصنيف SOC المهني
التثبيت باستخدام Codex أو Claude انسخ هذا Prompt والصقه في Codex أو Claude أو مساعد آخر ليراجع صفحة Skill ويثبّتها لك.
يتجاوز الأمر المباشر Prompt المخصّص للمراجعة. افحص المصدر قبل تشغيله.
npx skills add https://github.com/JohnNuwan/EVA_CORE --skill robot-slamيبقى الأمر في سطر واحد. مرّر أفقيًا لمراجعته كاملًا قبل النسخ.
تفضّل نسخة محلية؟ نزّل الملفات المتاحة حاليًا لدى SkillsMP.
عرض SKILL.md
Concevoir et maintenir un watchdog auto-correcteur pour services HTTP — checks de santé, auto-restart, état persistant, rapports et pièges bash.
Serveur de messagerie sécurisé auto-hébergé (Signal-like) avec Flask + WebSocket + AES-256-GCM + pont EVA
ADAM-SENTINEL — Veilleur technologique 24h/24h. Scanne 10 domaines, cree des rapports, met a jour les skills, alerte sur les CVE et breaking changes.
| name | robot-slam |
| description | SLAM (Simultaneous Localization and Mapping) — filtres particulaires, graph SLAM, EKF, FastSLAM, loop closure, ROS2 Cartographer, GTSAM |
Quand l'utilisateur demande de faire de la localisation de robot mobile, de la cartographie SLAM, du loop closure, de configurer Cartographer, GMapping, ou CORB-SLAM.
Estimer conjointement :
À partir de :
P(x₁:T, m | z₁:T, u₁:T) ← distribution conjointe
Mouvement : x_t = f(x_{t-1}, u_t) + w_t w_t ~ N(0, Q_t)
Observation : z_t = h(x_t, m) + v_t v_t ~ N(0, R_t)
| Approche | Représentation | Complexité | Robustesse | Usage |
|---|---|---|---|---|
| EKF-SLAM | Gaussienne | O(n²) | Faible | Petits espaces, peu de landmarks |
| FastSLAM | PF + EKF | O(n log n) | Moyenne | Robotique indoor |
| Graph SLAM | Graphe factoriel | O(n²)~O(n³) | Haute | Standard moderne |
| iSAM2 | Graphe incrémental | O(n) | Très haute | SLAM temps réel |
| Visual SLAM | ORB features | O(n) | Variable | Drones, voitures |
import numpy as np
class EKFSLAM:
"""
État : x = [x_r, y_r, θ_r, m₁_x, m₁_y, m₂_x, m₂_y, ...]^T
"""
def __init__(self, initial_pose):
# État initial : pose robot + 0 landmarks
self.mu = np.array([initial_pose[0], initial_pose[1], initial_pose[2]])
self.Sigma = np.eye(3) * 0.01 # covariance initiale
self.n_landmarks = 0
def predict(self, u, dt, Q):
"""u = [v, ω]^T (vitesse linéaire, angulaire)"""
# Modèle de vélocité (unicycle)
x, y, theta = self.mu[0], self.mu[1], self.mu[2]
v, omega = u[0], u[1]
if abs(omega) > 1e-6:
x_new = x + v/omega * (np.sin(theta + omega*dt) - np.sin(theta))
y_new = y + v/omega * (-np.cos(theta + omega*dt) + np.cos(theta))
theta_new = theta + omega*dt
else:
x_new = x + v*dt * np.cos(theta)
y_new = y + v*dt * np.sin(theta)
theta_new = theta
# Jacobienne G_t (3×3)
G = np.eye(3)
if abs(omega) > 1e-6:
G[0, 2] = v/omega * (np.cos(theta + omega*dt) - np.cos(theta))
G[, ] = v/omega * (np.sin(theta + omega*dt) - np.sin(theta))
:
G[, ] = -v*dt * np.sin(theta)
G[, ] = v*dt * np.cos(theta)
.mu[:] = [x_new, y_new, theta_new]
.Sigma[:, :] = G @ .Sigma[:, :] @ G.T + Q
():
x_r, y_r, theta = .mu[], .mu[], .mu[]
landmark_idx == -:
r, phi = z[], z[]
mx = x_r + r * np.cos(theta + phi)
my = y_r + r * np.sin(theta + phi)
.mu = np.append(.mu, [mx, my])
new_cov = R
.Sigma = np.block([
[.Sigma, np.zeros((.Sigma.shape[], ))],
[np.zeros((, .Sigma.shape[])), new_cov]
])
.n_landmarks +=
l_idx = + * landmark_idx
mx, my = .mu[l_idx], .mu[l_idx+]
dx = mx - x_r
dy = my - y_r
q = dx** + dy**
r_pred = np.sqrt(q)
phi_pred = np.arctan2(dy, dx) - theta
z_pred = np.array([r_pred, phi_pred])
innovation = z - z_pred
innovation[] = np.arctan2(np.sin(innovation[]), np.cos(innovation[]))
H = np.zeros((, (.mu)))
H[:, :] =
H[, ] = -dx/np.sqrt(q)
H[, ] = -dy/np.sqrt(q)
H[, ] = dy/q
H[, ] = -dx/q
H[, ] = -
H[, l_idx] = dx/np.sqrt(q)
H[, l_idx+] = dy/np.sqrt(q)
H[, l_idx] = -dy/q
H[, l_idx+] = dx/q
S = H @ .Sigma @ H.T + R
K = .Sigma @ H.T @ np.linalg.inv(S)
.mu = .mu + K @ innovation
.Sigma = (np.eye((.mu)) - K @ H) @ .Sigma
class FastSLAM2Particle:
"""Un particule FastSLAM 2.0 : pose robot + N EKF landmarks"""
def __init__(self, pose, weight=1.0):
self.pose = pose # [x, y, θ]
self.weight = weight
self.landmarks = {} # {id: (mu, Sigma)} # EKF 2×1 + 2×2
class FastSLAM2:
"""
FastSLAM 2.0 : Rao-Blackwellized PF
- M particules pour la pose
- Chaque particule a N EKF indépendants (1 par landmark)
Complexité : O(M log N) vs O(N²) pour EKF-SLAM
"""
def __init__(self, n_particles=100):
self.particles = [FastSLAM2Particle(np.zeros(3)) for _ in range(n_particles)]
self.n_particles = n_particles
def motion_update(self, u, dt, Q):
"""Propagation de chaque particule"""
for p in self.particles:
# Ajout bruit
v, omega = u[0] + np.random.normal(0, np.sqrt(Q[0,0])), \
u[1] + np.random.normal(0, np.sqrt(Q[1,1]))
# Modèle unicycle
if abs(omega) > :
p.pose[] += v/omega * (np.sin(p.pose[] + omega*dt) - np.sin(p.pose[]))
p.pose[] += v/omega * (-np.cos(p.pose[] + omega*dt) + np.cos(p.pose[]))
p.pose[] += omega * dt
:
p.pose[] += v*dt * np.cos(p.pose[])
p.pose[] += v*dt * np.sin(p.pose[])
():
p .particles:
landmark_id p.landmarks:
r, phi = z[], z[]
mx = p.pose[] + r * np.cos(p.pose[] + phi)
my = p.pose[] + r * np.sin(p.pose[] + phi)
p.landmarks[landmark_id] = (np.array([mx, my]), R)
p.weight *=
:
mu_l, Sigma_l = p.landmarks[landmark_id]
dx = mu_l[] - p.pose[]
dy = mu_l[] - p.pose[]
q = dx** + dy**
z_pred = np.array([np.sqrt(q), np.arctan2(dy, dx) - p.pose[]])
innovation = z - z_pred
innovation[] = np.arctan2(np.sin(innovation[]), np.cos(innovation[]))
H = np.array([
[dx/np.sqrt(q), dy/np.sqrt(q)],
[-dy/q, dx/q]
])
S = H @ Sigma_l @ H.T + R
K = Sigma_l @ H.T @ np.linalg.inv(S)
p.weight *= np.exp(- * innovation.T @ np.linalg.inv(S) @ innovation)
mu_l = mu_l + K @ innovation
Sigma_l = (np.eye() - K @ H) @ Sigma_l
p.landmarks[landmark_id] = (mu_l, Sigma_l)
():
weights = np.array([p.weight p .particles])
weights /= np.(weights)
new_particles = []
N = .n_particles
r = np.random.uniform(, /N)
c = weights[]
i =
j (N):
u = r + j/N
u > c:
i +=
c += weights[i]
new_particles.append(copy.deepcopy(.particles[i]))
new_particles[-].weight = /N
.particles = new_particles
θ* = argmin_θ Σ_t ||g(θ_t, u_t) - θ_{t-1}||²_Qt + Σ_t ||h(θ_t) - z_t||²_Rt
Où θ = [x₁, x₂, ..., x_T, m₁, m₂, ..., m_N] est le vecteur de tous les paramètres.
import gtsam # GTSAM 4.x
class GraphSLAM:
"""
GTSAM : Georgia Tech Smoothing and Mapping
Utilise iSAM2 (incremental) ou Levenberg-Marquardt (batch)
"""
def __init__(self):
self.graph = gtsam.NonlinearFactorGraph()
self.initial_estimate = gtsam.Values()
self.params = gtsam.LevenbergMarquardtParams()
self.isam = gtsam.ISAM2()
# Prior sur la première pose
self.prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.001]))
def add_odometry(self, pose_key_i, pose_key_j, odometry, noise):
"""Ajoute un facteur d'odométrie entre deux poses"""
# Odométrie : différence (dx, dy, dθ)
odom = gtsam.Pose2(odometry[0], odometry[1], odometry[2])
noise_model = gtsam.noiseModel.Diagonal.Sigmas(noise)
factor = gtsam.BetweenFactorPose2(pose_key_i, pose_key_j, odom, noise_model)
self.graph.add(factor)
def add_landmark_observation(self, pose_key, landmark_key, bearing, range_, noise):
"""Ajoute un facteur d'observation (bearing-range)"""
noise_model = gtsam.noiseModel.Diagonal.Sigmas(noise)
# BearingRangeFactor(pose_key, landmark_key, measured_bearing, measured_range, noise)
factor = gtsam.BearingRangeFactor2D(
pose_key, landmark_key,
gtsam.Rot2(np.cos(bearing), np.sin(bearing)),
range_, noise_model
)
self.graph.add(factor)
def add_loop_closure():
odom = gtsam.Pose2(relative_pose[], relative_pose[], relative_pose[])
noise_model = gtsam.noiseModel.Diagonal.Sigmas(noise)
factor = gtsam.BetweenFactorPose2(pose_key_i, pose_key_j, odom, noise_model)
.graph.add(factor)
():
initial_estimates :
initial_estimates = .initial_estimate
optimizer = gtsam.LevenbergMarquardtOptimizer(.graph,
initial_estimates,
.params)
result = optimizer.optimize()
result
():
.isam.update(new_factors, new_values)
result = .isam.calculateEstimate()
result
class LoopClosureDetector:
"""
Détection de boucle via scan matching ou descripteurs visuels
"""
def __init__(self, distance_threshold=1.0, angle_threshold=0.3):
self.distance_thresh = distance_threshold
self.angle_thresh = angle_threshold
self.pose_history = [] # [(x, y, θ, scan)]
def detect_loop(self, current_scan, current_pose):
"""Vérifie si le scan courant correspond à un historique"""
for i, (x_old, y_old, θ_old, scan_old) in enumerate(self.pose_history):
# Distance géométrique
dx = current_pose[0] - x_old
dy = current_pose[1] - y_old
dist = np.sqrt(dx**2 + dy**2)
if dist < self.distance_thresh:
# Scan matching (ICP ou correlative)
icp = ICPMatcher()
T_rel, score = icp.match(scan_old, current_scan,
initial_guess=np.array([dx, dy, 0]))
if score < threshold: # bon match
return i, T_rel, score
return None, None, None
class ICPMatcher:
"""Iterative Closest Point pour scan matching"""
():
T = initial_guess.copy()
_ (max_iter):
associations = .associate(scan_ref, scan_curr, T)
T_new = .estimate_transform(scan_ref, scan_curr, associations)
np.linalg.norm(T_new - T) < :
T = T_new
T, .compute_score(scan_ref, scan_curr, T)
-- cartographer.lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
sensor_identifier = "scan",
submaps = { -- 2D local SLAM
resolution = 0.05,
num_range_data = 100,
},
ceres_scan_matcher = {
occupied_space_weight = 1.,
translation_weight = 10.,
rotation_weight = 40.,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 20,
num_threads = 1,
},
},
ceres_pose = {
optimize_every_n_scans = 20,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 50,
num_threads = 1,
},
},
}
return options
# Cartographer 2D
ros2 launch cartographer_ros cartographer.launch.py \
configuration_basename:=cartographer.lua
# Occupancy grid → navigation
ros2 run cartographer_ros cartographer_occupancy_grid_node \
-resolution 0.05 \
-publish_period_sec 1.0
| Aspect | LiDAR SLAM | Visual SLAM |
|---|---|---|
| Capteur | LiDAR 2D/3D | Caméra monoculaire/stéréo |
| Précision | 1-5 cm | 5-50 cm (mono), 1-10 cm (stéréo) |
| Robustesse | Excellente (indépendant lumière) | Moyenne (lumière/texture) |
| Coût capteur | 500-5000€ | 50-500€ |
| Dérive | Faible | Plus élevée (sauf stéréo) |
| Temps réel | Oui (Cartographer, Karto) | Oui (ORB-SLAM3, DSO) |
| Cartographie | Grille d'occupation | Sparse feature map / dense |
# Monocular
ros2 run orbslam3 mono Vocabulary/ORBvoc.txt Monocular/TUM1.yaml
# Stereo
ros2 run orbslam3 stereo Vocabulary/ORBvoc.txt Stereo/EuRoC.yaml true
# RGB-D
ros2 run orbslam3 rgbd Vocabulary/ORBvoc.txt RGBD/TUM1.yaml
# IMU-stereo
ros2 run orbslam3 imu_stereo Vocabulary/ORBvoc.txt MyConfig.yaml
| Métrique | Description |
|---|---|
| ATE (Absolute Trajectory Error) | RMSE de la pose estimée vs vérité terrain |
| RPE (Relative Pose Error) | Erreur entre poses consécutives (dérive locale) |
| RMSE | Root Mean Square Error : √(1/N Σ |
| Consistency | χ² normalisé de la covariance estimée |
| Loop Closure Rate | % de boucles correctement détectées |
# ATE — évaluation de trajectoire
def compute_ate(estimated_poses, ground_truth_poses):
# Alignement (Horn's method) : Sim(3) alignment
T_aligned = sim3_alignment(estimated_poses, ground_truth_poses)
aligned_poses = [T_aligned @ p for p in estimated_poses]
errors = []
for p_est, p_gt in zip(aligned_poses, ground_truth_poses):
e = np.linalg.norm(p_est[:3, 3] - p_gt[:3, 3])
errors.append(e)
return {
'rmse': np.sqrt(np.mean(np.array(errors)**2)),
'mean': np.mean(errors),
'std': np.std(errors),
'max': np.max(errors),
'min': np.min(errors)
}