🧠 SLAM Fundamentals
🎯 What is SLAM?
SLAM (Simultaneous Localization and Mapping) is the problem of building a map of an unknown environment while simultaneously keeping track of an agent's location within it. It is the core technology behind AR, VR, robotics, and autonomous vehicles. The "chicken-and-egg" problem: to map you need pose, to know pose you need a map.
The SLAM Pipeline
1. Sensing → Camera, IMU, LiDAR capture data. 2. Frontend → Feature extraction, tracking, odometry. 3. Backend → Optimization (bundle adjustment, pose graph). 4. Loop Closure → Recognize previously seen places. 5. Mapping → Build sparse/dense map. This loop runs continuously at 30-60Hz.
# SLAM pseudo-architecture
class SLAMSystem:
def __init__(self):
self.map = Map()
self.pose = Pose.identity()
def process_frame(self, image, imu_data):
# Frontend: feature tracking
features = self.extract_features(image)
self.pose = self.track_features(features, self.pose)
# Backend: optimization (runs async)
if self.needs_optimization():
self.optimize_map_and_pose()
# Loop closure
if self.detect_loop_closure():
self.correct_drift()
return self.pose📷 Sensors & Data Fusion
🎯 The sensory suite for SLAM
Cameras (mono, stereo, RGB-D), IMU (gyro + accelerometer), LiDAR, and wheel odometry. Each has strengths and weaknesses; fusion provides robustness.
Sensor Characteristics
Monocular camera: cheap, but scale ambiguity. Stereo camera: provides depth, but calibration-sensitive. IMU: high frequency (1000Hz), but drifts. LiDAR: accurate depth, but expensive and heavy. Fusion (e.g., VIO) combines camera + IMU for the best of both worlds.
# Sensor fusion with Extended Kalman Filter (EKF)
import numpy as np
class EKF:
def __init__(self):
self.state = np.zeros(16) # pose + velocity + biases
self.covariance = np.eye(16) * 0.1
def predict(self, imu_data, dt):
# IMU integration for prediction
self.state = self.motion_model(self.state, imu_data, dt)
self.covariance = self.F @ self.covariance @ self.F.T + self.Q
def update(self, camera_pose):
# Camera observation update
K = self.covariance @ self.H.T @ np.linalg.inv(self.H @ self.covariance @ self.H.T + self.R)
self.state += K @ (camera_pose - self.H @ self.state)
self.covariance = (np.eye(16) - K @ self.H) @ self.covariance🔍 Feature Detection & Matching
🎯 Finding correspondences
Features (corners, blobs, edges) are distinctive points in images. Matching them across frames enables motion estimation.
Feature Detectors & Descriptors
ORB (Oriented FAST and Rotated BRIEF) is the most popular for SLAM — fast and rotation-invariant. SIFT/SURF are more accurate but slower. SuperPoint and SuperGlue use deep learning for robust matching.
// ORB feature extraction and matching (OpenCV)
#include
Ptr orb = ORB::create(1000);
vector kp1, kp2;
Mat desc1, desc2;
orb->detectAndCompute(img1, noArray(), kp1, desc1);
orb->detectAndCompute(img2, noArray(), kp2, desc2);
// BFMatcher with Hamming distance
BFMatcher matcher(NORM_HAMMING);
vector matches;
matcher.match(desc1, desc2, matches);
// Filter good matches
sort(matches.begin(), matches.end());
matches.erase(matches.begin() + 100, matches.end()); 🚶 Visual Odometry
🎯 Estimating motion from images
Visual Odometry (VO) estimates camera motion between consecutive frames using feature correspondences. It's the frontend of SLAM.
VO Methods
Feature-based: Extract features, match, estimate essential/fundamental matrix, recover pose. Direct methods: Use pixel intensities directly (e.g., DSO, LSD-SLAM) — more accurate in textureless areas but sensitive to lighting. Hybrid: Combines both (e.g., SVO).
// Essential matrix and pose recovery
Mat E = findEssentialMat(pts1, pts2, focal, pp, RANSAC);
Mat R, t;
recoverPose(E, pts1, pts2, R, t, focal, pp);
// Triangulate 3D points
Mat pts3d;
triangulatePoints(P1, P2, pts1, pts2, pts3d);
// Convert from homogeneous
pts3d /= pts3d.row(3);🗺️ Mapping & Map Types
🎯 Representing the environment
Maps can be sparse (feature points), dense (point clouds, meshes), or semantic (labeled objects). Each serves different purposes.
Map Representations
Sparse maps: 3D landmarks, efficient for localization. Dense maps: Point clouds or voxels, useful for AR occlusion and path planning. Semantic maps: Augmented with object labels — enables intelligent AR interactions.
# Sparse map representation
class MapPoint:
def __init__(self, position, descriptor):
self.position = position # 3D world coordinates
self.descriptor = descriptor # ORB descriptor
self.observations = [] # keyframes that see this point
class Keyframe:
def __init__(self, pose, image, features):
self.pose = pose
self.image = image
self.features = features
self.map_points = [] # visible map points🔄 Loop Closure & Drift
🎯 Correcting accumulated error
Drift accumulates in odometry. Loop closure detects when the camera revisits a previously mapped area and corrects the entire trajectory.
Place Recognition
Bag of Words (BoW) is the classic approach — convert images to visual words and match. NetVLAD and DINOv2 use deep learning for robust place recognition. Loop closure triggers pose graph optimization to distribute the error.
# Loop closure detection with DBoW
import dbow
# Create vocabulary (offline)
voc = dbow.Vocabulary(k=10, L=6, weighting='tf-idf', scoring='L1')
voc.load('orb_vocabulary.yml')
# Online database
db = dbow.Database(voc)
# Add keyframe
db.add(image_descriptors)
# Query for loop closure
results = db.query(image_descriptors, max_results=5)
if results[0].score > threshold:
# Loop closure detected!
perform_pose_graph_optimization()📐 Bundle Adjustment & Pose Graph
🎯 The SLAM backend
Optimization refines camera poses and 3D point positions to minimize reprojection error. Bundle Adjustment (BA) for local, pose graph for global.
Non-linear Least Squares
Bundle Adjustment optimizes camera poses and 3D points jointly. Pose Graph Optimization optimizes only poses (edges = relative transforms) — faster and used for global consistency. g2o, Ceres Solver, GTSAM are popular libraries.
// Pose graph optimization with g2o
#include
#include
SparseOptimizer optimizer;
optimizer.setAlgorithm(new LevenbergMarquardtOptimization());
// Add vertices (poses)
for (auto& pose : poses) {
VertexSE3* v = new VertexSE3();
v->setId(pose.id);
v->setEstimate(pose.transform);
optimizer.addVertex(v);
}
// Add edges (odometry constraints)
for (auto& constraint : constraints) {
EdgeSE3* e = new EdgeSE3();
e->setVertex(0, optimizer.vertex(constraint.from));
e->setVertex(1, optimizer.vertex(constraint.to));
e->setMeasurement(constraint.transform);
e->setInformation(constraint.information);
optimizer.addEdge(e);
}
optimizer.optimize(10); 👁️ Visual SLAM (vSLAM)
🎯 SLAM with cameras only
Visual SLAM uses cameras as the primary sensor. It can be monocular, stereo, or RGB-D. ORB-SLAM3 is the state-of-the-art open-source system.
ORB-SLAM3 Architecture
Tracking thread: real-time pose estimation. Local mapping: local BA and map point management. Loop closing: place recognition and pose graph optimization. Atlas: multi-map representation for robustness.
// ORB-SLAM3 usage (C++)
#include
ORB_SLAM3::System SLAM(
"Vocabulary/ORBvoc.txt",
"Settings.yaml",
ORB_SLAM3::System::MONOCULAR,
true // use viewer
);
// Process frames
for (const auto& img : image_sequence) {
Sophus::SE3f pose = SLAM.TrackMonocular(img, timestamp);
// use pose for AR rendering
} 🧭 Visual-Inertial Odometry (VIO)
🎯 Camera + IMU fusion
VIO combines visual and inertial data for robust, metric-scale tracking. It's the core of ARKit, ARCore, and modern VR headsets.
Tightly vs Loosely Coupled
Loosely coupled: Visual and inertial processed separately, then fused (e.g., EKF). Tightly coupled: IMU pre-integration inside the optimization (e.g., VINS-Mono, OKVIS). Tightly coupled is more accurate and robust.
// VINS-Mono: IMU pre-integration
// Between keyframes, integrate IMU measurements
void integrateIMU(const vector& imu_data, double dt) {
// Update position, velocity, rotation
delta_p += delta_v * dt + 0.5 * (R * (accel - bias_a) + g) * dt * dt;
delta_v += (R * (accel - bias_a) + g) * dt;
delta_q = delta_q * Quaterniond(1, 0.5 * (gyro - bias_g) * dt);
// Covariance propagation
F = computeJacobian();
P = F * P * F.transpose() + Q;
} 🧱 Dense SLAM & Reconstruction
🎯 Full 3D reconstruction
Dense SLAM reconstructs complete surfaces, not just sparse points. Enables realistic AR occlusion, physics, and mesh-based interactions.
Dense Methods
TSDF (Truncated Signed Distance Function): Voxel-based fusion (KinectFusion). Surfel-based: ElasticFusion. Neural: NeRF-based SLAM (iMAP, NICE-SLAM) — photorealistic reconstruction. Gaussian Splatting: fast, high-quality rendering.
// TSDF fusion (KinectFusion style)
for each voxel in volume:
if (voxel.weight > 0):
// Project voxel to camera
float2 pixel = project(voxel.position, camera_pose);
float depth = depth_map[pixel];
float sdf = depth - voxel.distance_to_camera;
if (sdf > -truncation):
// Update TSDF
float tsdf = clamp(sdf / truncation, -1, 1);
voxel.distance = (voxel.distance * voxel.weight + tsdf) / (voxel.weight + 1);
voxel.weight += 1;☁️ AR Cloud & Multi-Session
🎯 Persistent, shared AR
AR Cloud stores maps in the cloud, enabling persistent AR experiences across sessions and shared multi-user AR.
Cloud Anchors & Maps
ARCore Cloud Anchors: Share anchors across devices. ARKit Location Anchors: Anchor to GPS coordinates. Visual Positioning System (VPS): Localize using pre-built maps (e.g., Google Geospatial, Niantic Lightship).
// ARCore Cloud Anchor hosting
using Google.XR.ARCoreExtensions;
public class CloudAnchorManager : MonoBehaviour {
public ARAnchorManager anchorManager;
public void HostAnchor(ARAnchor localAnchor) {
var cloudAnchor = anchorManager.HostCloudAnchorAsync(localAnchor, 1);
// wait for result
if (cloudAnchor.State == CloudAnchorState.Success) {
string cloudId = cloudAnchor.CloudAnchorId;
// share cloudId with other users
}
}
public void ResolveAnchor(string cloudId) {
var cloudAnchor = anchorManager.ResolveCloudAnchorAsync(cloudId);
// wait for result
}
}📍 Relocalization & Persistence
🎯 Finding yourself in a known map
Relocalization is the process of determining the camera pose within a previously built map — essential for persistent AR and multi-session experiences.
Relocalization Methods
Feature matching: Match current frame features to map features. PnP (Perspective-n-Point): Estimate pose from 3D-2D correspondences. Deep learning: PoseNet, DSAC++ regress pose directly from images.
// PnP relocalization
Mat rvec, tvec;
solvePnPRansac(
map_points_3d, // 3D points from map
image_points_2d, // corresponding 2D features
camera_matrix,
dist_coeffs,
rvec, tvec,
false, // use extrinsic guess?
100, // iterations
8.0, // reprojection error
0.99, // confidence
inliers // output inliers
);
// Convert to pose matrix
Mat R;
Rodrigues(rvec, R);⚠️ SLAM Challenges & Failure Modes
🎯 When SLAM breaks
SLAM fails in textureless environments, rapid motion, dynamic scenes, and lighting changes. Understanding failure modes is crucial for robust AR.
Common Failure Modes
Textureless walls: No features to track. Rapid motion: Motion blur, feature tracking lost. Dynamic objects: Moving people/cars confuse mapping. Lighting changes: Direct methods fail. Scale drift: Monocular SLAM loses metric scale. Solutions: IMU fusion, deep learning, multi-sensor.
# Robust SLAM with outlier rejection
def robust_pose_estimation(features_2d, map_points_3d):
# RANSAC for outlier rejection
best_inliers = []
for _ in range(max_iterations):
sample = random_sample(features_2d, map_points_3d, 4)
pose = estimate_pose(sample)
inliers = count_inliers(pose, features_2d, map_points_3d, threshold)
if len(inliers) > len(best_inliers):
best_inliers = inliers
# Refine with inliers only
refined_pose = optimize_pose(best_inliers)
return refined_pose, best_inliers🚀 Future: Neural SLAM & AI
🎯 Deep learning meets SLAM
Neural networks are replacing traditional SLAM components: feature extraction, matching, pose estimation, and mapping. Neural SLAM enables photorealistic reconstruction and semantic understanding.
Neural SLAM Systems
SuperPoint + SuperGlue: Learned features and matching. DROID-SLAM: Recurrent optical flow for dense SLAM. iMAP, NICE-SLAM: NeRF-based SLAM. Gaussian Splatting SLAM: Real-time photorealistic mapping. These systems are more robust to challenging conditions.
# DROID-SLAM inference (PyTorch)
import torch
from droid_slam import Droid
model = Droid().cuda()
model.eval()
# Process video
poses = []
for image in video_frames:
image = preprocess(image).cuda()
pose = model.track(image)
poses.append(pose)
# Global optimization
poses = model.global_ba(poses)