Week 12 — Simultaneous Localization and Mapping (SLAM) Day 7 of 7
The Week in One Paragraph
This week was about a robot answering two questions at the same time: where am I, and what does the world around me look like? Neither answer is available without the other. To build a map you need to know where the sensor was when each measurement was taken; to know where you are you need a map to compare against. SLAM is the joint estimation of both. We started with the probabilistic formulation — the pose trajectory $x_{1:t}$, the map $m$, and the measurement and motion models that tie them together, then looked at why estimating them jointly is harder than estimating either alone (the correlation between robot pose and landmark estimates, which naive approaches throw away). Day 2 covered the two classical answers: EKF-SLAM, which keeps a full covariance over pose and landmarks but scales as $O(n^2)$ in landmarks, and FastSLAM, which factorizes the posterior into a particle filter over trajectories and per-particle EKFs over landmarks, scaling as $O(M \log n)$. Day 3 moved to cameras with ORB-SLAM: FAST keypoints, oriented BRIEF descriptors, and a three-thread architecture (tracking, local mapping, loop closing) that made feature-based visual SLAM robust enough to run in real time. Day 4 moved to LiDAR with LOAM and LeGO-LOAM: edge and planar feature extraction, scan-to-scan and scan-to-map registration, and the ground-segmentation trick that lets LeGO-LOAM run on embedded hardware. Day 5 tackled loop closure — the place recognition step that detects revisits and lets the back-end correct accumulated drift. Day 6 made it concrete with a 2D pose-graph SLAM implementation in Python using g2o, where we built the graph, added odometry and loop-closure edges, and watched the optimizer pull the trajectory back into shape. The through-line: front-end gives you constraints, back-end gives you the estimate, and loop closure is what keeps the estimate from drifting into uselessness over long runs.
What Each Day Built
| Day | Topic | The one idea you must retain |
|---|---|---|
| 1 | SLAM Problem Formulation | SLAM is a joint posterior $p(x_{1:t}, m \mid z_{1:t}, u_{1:t})$; the hard part is the correlation between pose and map estimates, not the size of either alone. |
| 2 | EKF-SLAM and FastSLAM | EKF-SLAM is exact under linear-Gaussian assumptions but its covariance is $O(n^2)$ in landmarks; FastSLAM factorizes into particles over trajectories plus per-landmark EKFs and scales far better, at the cost of particle depletion. |
| 3 | Visual SLAM: ORB-SLAM | ORB features (FAST + oriented BRIEF, 256-bit binary descriptor) enable fast matching via Hamming distance; ORB-SLAM’s three-thread design separates tracking, mapping, and loop closing so each runs at its own rate. |
| 4 | LiDAR SLAM: LOAM and LeGO-LOAM | Extract edge and planar features from each scan, then register scan-to-scan for odometry and scan-to-map for refinement; LeGO-LOAM adds ground segmentation and clustering to cut computation. |
| 5 | Loop Closure Detection | Place recognition on a bag-of-words or descriptor database catches revisits; a verified loop-closure constraint is what lets the back-end remove drift that odometry alone cannot. |
| 6 | Python Practice: SLAM with g2o | A pose graph is nodes (poses) plus edges (relative constraints); least-squares optimization over all edges simultaneously distributes loop-closure error across the whole trajectory. |
Formula Cheat Sheet
| Equation | Symbols | Where it is used |
|---|---|---|
| $p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) \propto p(x_1), p(m) \prod_{k} p(x_k \mid x_{k-1}, u_k) \prod_{k} p(z_k \mid x_k, m)$ | $x$ pose, $m$ map, $z$ measurement, $u$ control input | Day 1 — the full SLAM posterior; every method this week is an approximation of this. |
| $\mu_{t} = \mu_{t-1} + F^{T} K (z_t - h(\mu_{t-1}))$, $P_t = (I - K H) P_{t-1}$ | $\mu$ state mean, $P$ covariance, $K$ Kalman gain, $H$ measurement Jacobian, $F$ state-transition Jacobian | Day 2 — EKF-SLAM update step, applied to the stacked pose-plus-landmark state vector. |
| $K = P_{t-1} H^{T} (H P_{t-1} H^{T} + R)^{-1}$ | $R$ measurement noise covariance | Day 2 — Kalman gain; note the $O(n^2)$ cost comes from the $n \times n$ landmark blocks in $P$. |
| $d(\mathbf{a}, \mathbf{b}) = \sum_{i} \text{XOR}(a_i, b_i)$ | $a_i, b_i$ bits of two ORB descriptors | Day 3 — Hamming distance for binary descriptor matching; fast because it is a popcount on 256-bit words. |
| $\mathbf{r}{ij} = \log\left( T{ij}^{-1}, T_i^{-1}, T_j \right)$ | $T_i, T_j$ poses, $T_{ij}$ measured relative transform, $\log$ maps SE(3) to se(3) | Day 4 and Day 6 — residual of a relative-pose edge in the pose graph; minimized over all edges. |
| $F(\mathbf{x}) = \sum_{\langle i,j \rangle \in C} \mathbf{e}{ij}^{T} \Omega{ij} \mathbf{e}_{ij}$ | $\mathbf{e}{ij}$ edge error, $\Omega{ij}$ information matrix, $C$ set of constraints | Day 6 — the g2o least-squares objective; $\Omega$ encodes how much you trust each constraint. |
| $\mathbf{x}^{*} = \arg\min_{\mathbf{x}} F(\mathbf{x})$ solved by $\Delta\mathbf{x} = -(J^{T}\Omega J)^{-1} J^{T}\Omega,\mathbf{e}$ | $J$ stacked Jacobian of all edge errors | Day 6 — Gauss-Newton step inside g2o; sparse $J$ is why large pose graphs are tractable. |
Method Selection Guide
- Small environment, few landmarks, need a principled uncertainty estimate? EKF-SLAM. The covariance is dense but small, and you get calibrated uncertainty out of the box. Do not use it past a few hundred landmarks — the $O(n^2)$ update will choke.
- Large landmark count, want to keep multiple trajectory hypotheses? FastSLAM. The particle filter handles data association ambiguity better than EKF-SLAM, but watch particle depletion and resample only when effective sample size drops.
- Camera-only robot, textured indoor or outdoor scene, need real-time on a CPU? ORB-SLAM. Binary descriptors and the three-thread design are the reason it runs without a GPU. It will fail in textureless corridors and on reflective surfaces — that is a sensor problem, not an algorithm problem.
- LiDAR available, geometrically structured environment (buildings, tunnels, warehouses)? LOAM. Edge and planar features are far more stable than raw points, and scan-to-map registration keeps drift low.
- LiDAR on an embedded platform with tight compute budget? LeGO-LOAM. Ground segmentation removes a large fraction of points before registration; the trade is sensitivity to non-flat terrain.
- Long-duration run where drift matters? Any front-end plus a loop-closure module (DBoW-style place recognition) plus a pose-graph back-end. Without loop closure, no front-end survives a kilometer of accumulated error.
- Need a working 2D graph SLAM result quickly? g2o with a pose graph. Build odometry edges, add loop edges, optimize. It is the shortest path from data to a corrected trajectory.
Common Pitfalls (Learned the Hard Way)
| Pitfall | Why it happens | Fix |
|---|---|---|
| EKF-SLAM covariance stops being positive-definite after long runs | Repeated linearization of a nonlinear measurement model accumulates numerical error in the $O(n^2)$ covariance | Use the Joseph form of the covariance update, or switch to a graph-based back-end once landmark count grows. |
| FastSLAM particles all collapse onto one trajectory | Resampling too often discards diversity; the filter loses the true hypothesis | Resample only when effective sample size $N_{eff} = 1/\sum w_i^2$ falls below a threshold (commonly $N/2$). |
| ORB-SLAM tracking lost in a white corridor | FAST finds no corners on textureless surfaces; descriptors cannot match | Add a second sensor (IMU or LiDAR) or rely on the relocalization thread; do not expect feature-based visual SLAM to handle blank walls. |
| LOAM drifts badly on open flat ground | Planar features dominate and provide no constraint in the direction of motion along the plane | Ensure enough vertical structure in view, or fuse with wheel odometry / IMU to constrain the degenerate direction. |
| Loop closure accepted between two similar-looking places | Perceptual aliasing — bag-of-words matches on appearance alone | Require geometric verification (PnP or ICP) on the candidate match before adding the edge; reject if inlier count is low. |
| g2o optimization diverges or produces a folded trajectory | Wrong information matrix scale, or an outlier loop edge with high weight | Set $\Omega$ from actual sensor noise, run robust kernel (Huber) on loop edges, and inspect residuals before optimizing. |
| Pose graph converges but map still looks wrong | Odometry edges were treated as ground truth with near-infinite information | Down-weight odometry edges relative to verified loop closures; the optimizer should be free to bend odometry. |
Self-Assessment
1. Why can’t you solve localization and mapping separately? Because the map is built from measurements taken at estimated poses, and the pose estimate depends on the map. Errors in one feed the other. The joint posterior in Day 1 captures this correlation; treating them independently (e.g., mapping with assumed-perfect poses) produces a map that is inconsistent with the actual trajectory.
2. What is the scaling difference between EKF-SLAM and FastSLAM, and why? EKF-SLAM maintains a covariance matrix over pose plus all $n$ landmarks, so the update is $O(n^2)$ per step. FastSLAM factorizes the posterior into $M$ particles, each carrying its own trajectory and a set of independent per-landmark EKFs, giving roughly $O(M \log n)$ — much better for large maps, at the cost of sampling error.
3. What does ORB stand for and why does it matter for real-time SLAM? Oriented FAST and Rotated BRIEF. FAST keypoints are cheap to detect, and BRIEF descriptors are 256-bit binary strings compared with Hamming distance (a popcount), which is far faster than floating-point descriptor distance. Rotation invariance comes from estimating keypoint orientation and steering the BRIEF sampling pattern accordingly.
4. What is the role of loop closure in a SLAM system? It detects when the robot revisits a previously mapped place, then adds a constraint between the current pose and the old one. That constraint tells the back-end that accumulated odometry drift is wrong, and the optimizer distributes the correction across the whole trajectory. Without loop closure, drift grows without bound.
5. In the g2o exercise, what are the nodes and edges, and what does the optimizer actually minimize? Nodes are robot poses (2D: $x, y, \theta$). Edges are relative-pose constraints: odometry edges between consecutive poses and loop-closure edges between revisited poses. The optimizer minimizes the sum of squared edge residuals weighted by their information matrices — the least-squares objective in the cheat sheet — adjusting all poses together so every constraint is satisfied as well as possible.
Preview: Week 13
Next week we move from geometric estimation to learned perception: Deep Learning for Computer Vision in Robotics. We will cover convolutional networks for detection and segmentation, how to train and deploy them on robot hardware, and where learned perception fits alongside the classical SLAM pipeline you built this week — including the growing use of learned features and depth estimation as front-ends for visual odometry.