Simultaneous Localization and Mapping (SLAM)

SLAM is fundamentally a probabilistic estimation problem. The system must jointly estimate the robot's pose and the map of the environment, both of which are corrupted by sensor noise and motion uncertainty. This is typically formulated using Bayesian filtering techniques.

• Extended Kalman Filter (EKF) SLAM: Linearizes motion and observation models to maintain a joint state vector of the robot pose and all landmark positions, with a covariance matrix representing uncertainty.
• Particle Filter (FastSLAM): Decomposes the problem by using a Rao-Blackwellized particle filter, where each particle represents a possible robot trajectory and maintains its own map of landmarks.
• Factor Graph Optimization (Modern SLAM): Represents the problem as a graph of nodes (poses and landmarks) and edges (constraints from odometry and observations), solved via nonlinear least squares optimization (e.g., iSAM2, g2o).

The choice of exteroceptive sensor fundamentally shapes the SLAM algorithm's characteristics. Modern systems often fuse multiple modalities to overcome individual sensor limitations.

• Visual SLAM (vSLAM): Uses monocular, stereo, or RGB-D cameras. ORB-SLAM3 is a state-of-the-art feature-based system using oriented FAST and rotated BRIEF (ORB) features for real-time operation. DTAM and LSD-SLAM are direct methods that operate on pixel intensities.
• LiDAR SLAM: Relies on laser range finders for high-precision geometric measurements. LOAM (LiDAR Odometry and Mapping) and its derivative LeGO-LOAM extract edge and planar features for lightweight, drift-minimized odometry.
• Visual-Inertial SLAM (VI-SLAM): Fuses camera data with an Inertial Measurement Unit (IMU) to provide robust state estimation during aggressive motion and in texture-deprived environments.

Data association, or the correspondence problem, is the critical task of correctly matching current sensor measurements to previously observed landmarks or map elements. Incorrect associations inject catastrophic errors into the estimation backend.

• Short-term Data Association: Matching features between consecutive frames for visual odometry, often using descriptor matching with RANSAC for outlier rejection.
• Long-term Data Association (Loop Closure Detection): Recognizing a previously visited location to correct accumulated drift. This is achieved using a Bag-of-Words (BoW) model like DBoW2, which compares image descriptors against a visual vocabulary database.
• Per-Aliasing: The failure mode where different physical locations generate identical sensor signatures, causing false-positive loop closures that corrupt the map.

The internal representation of the environment, or world model, dictates the system's spatial reasoning capabilities and computational complexity.

• Landmark-based Maps (Sparse): Represent the environment as a set of discrete, parameterized features (e.g., 3D points). This is computationally efficient but provides no information about free or unexplored space.
• Occupancy Grid Maps (Dense): Discretize the world into a grid of cells, each storing a probability of being occupied. OctoMap uses an octree data structure for efficient 3D volumetric representation.
• Surfel Maps: Represent surfaces as a cloud of disk-like elements (surfels) with position, normal, and radius, enabling high-fidelity dense reconstruction for manipulation.
• Semantic Maps: Augment geometric maps with object-level labels and relationships, enabling higher-level scene understanding beyond pure geometry.

Modern SLAM systems are architecturally decoupled into a real-time frontend and a globally consistent backend to manage computational load.

• Frontend (Visual/LiDAR Odometry): Processes raw sensor data at frame rate to extract features and compute a locally consistent, incremental motion estimate. It abstracts sensor data into a condensed set of constraints for the backend.
• Backend (Global Optimization): Performs computationally intensive, globally consistent optimization on a keyframe-limited subset of the data. It solves the bundle adjustment or pose graph optimization problem to eliminate drift upon loop closure detection.
• Keyframe Management: The frontend selectively inserts representative frames into the backend graph to maintain sparsity and bound optimization time, dropping redundant frames based on motion or visual overlap heuristics.

A fundamental challenge, especially in monocular visual SLAM, is the inherent scale ambiguity. A single camera cannot distinguish between a small, nearby object and a large, distant one.

• Monocular Scale Drift: Without an absolute scale reference, the map and trajectory are estimated up to an unknown global scale factor, which can drift over time.
• Initialization Strategies: Systems must perform a deliberate initialization routine to establish an initial map and scale. This often involves a known motion pattern (e.g., a pure translation of a known distance) or a planar scene assumption for homography decomposition.
• Metric Scale Recovery: Scale is recovered by integrating an IMU (in VI-SLAM), using a stereo camera with a known baseline, projecting a structured light pattern (RGB-D), or fusing with wheel odometry.

The computational backbone of SLAM that combines data from disparate sensors to overcome individual limitations. Visual-inertial odometry fuses camera images with IMU accelerometer and gyroscope data to provide robust short-term motion estimates. LiDAR-visual fusion aligns sparse depth points with dense visual features for precise 3D reconstruction. Modern frameworks employ factor graphs to represent sensor measurements as probabilistic constraints between robot poses and landmarks, enabling efficient nonlinear least-squares optimization.

A particle-filter-based approach to robot pose estimation that represents belief as a set of weighted hypotheses. Each particle encodes a possible robot pose, and the filter recursively applies:

• Motion update: Particles are propagated using the robot's odometry model with added noise
• Sensor update: Particle weights are adjusted based on how well predicted observations match actual sensor readings
• Resampling: Particles with low weights are replaced by copies of high-weight particles MCL naturally handles multi-modal distributions and the kidnapped robot problem, where the robot must recover from complete localization failure.

A volumetric map representation where each voxel stores the shortest distance to the nearest surface, with negative values inside obstacles. SDFs enable:

• Gradient-based collision checking: The distance gradient points away from obstacles, providing efficient repulsive forces for trajectory optimization
• Truncated SDF (TSDF): Only distances within a narrow band around surfaces are stored, dramatically reducing memory
• Continuous interpolation: Sub-voxel accuracy through trilinear interpolation of distance values Popular in dense SLAM systems like KinectFusion and Voxblox, SDFs provide both the map representation and a differentiable cost function for pose refinement.

The dominant back-end formulation for modern SLAM systems, representing the estimation problem as a bipartite graph. Variable nodes encode robot poses and landmark positions; factor nodes encode probabilistic constraints from:

• Odometry factors: Relative motion between consecutive poses
• Loop closure factors: Constraints between non-consecutive poses when a previously visited location is recognized
• Landmark factors: Bearing or range measurements to mapped features Optimization uses Gauss-Newton or Levenberg-Marquardt algorithms on the manifold of poses. Libraries like GTSAM and g2o provide efficient sparse solvers that exploit the problem's inherent sparsity pattern.

The mechanism that corrects accumulated drift by recognizing when the robot revisits a previously mapped area. Approaches include:

• Bag-of-Words (BoW): Visual features are quantized into a vocabulary tree; scene similarity is computed via histogram comparison of visual word occurrences
• Scan matching: Direct alignment of current LiDAR scans against the global map using Iterative Closest Point (ICP)
• Place recognition: Deep learning embeddings from networks like NetVLAD produce viewpoint-invariant descriptors Successful loop closures add constraints to the factor graph that distribute the accumulated error globally through pose graph optimization, producing a globally consistent map.

Two fundamental paradigms distinguished by map representation density:

• Sparse SLAM: Tracks a limited set of distinct feature points (e.g., ORB-SLAM tracks corners and blobs). Computationally efficient and suitable for localization, but produces maps unsuitable for obstacle avoidance or manipulation
• Dense SLAM: Reconstructs every pixel or voxel in the observed scene. Methods like DTAM and ElasticFusion produce surfel-based maps usable for physics simulation and grasping
• Semi-dense SLAM: A middle ground (e.g., LSD-SLAM) that reconstructs only pixels with sufficient intensity gradient, balancing computational cost with map utility