Visual SLAM

Visual SLAM Overview

Visual SLAM (Simultaneous Localization and Mapping) enables a robot or camera to build a map of an unknown environment while simultaneously tracking its position within it. Using camera observations, SLAM systems extract visual features, match them across frames, estimate camera motion, and construct a 3D map of the environment. The system must handle loop closures, drift accumulation, and real-time processing constraints.

Theory: Feature-Based SLAM

Feature-based SLAM extracts distinctive keypoints from images and matches them across frames to estimate camera motion. ORB features combine FAST corners with rotated BRIEF descriptors, providing rotation invariance and computational efficiency. Feature matching establishes correspondences between frames, which are used to compute the essential matrix encoding the relative camera pose.

Bundle adjustment jointly optimizes camera poses and 3D point positions by minimizing reprojection error. The Schur complement trick exploits the sparse structure of the problem, marginalizing 3D points to solve only for camera parameters in a reduced system. This reduces computational complexity from cubic to linear in the number of cameras.

Loop closure detection identifies previously visited locations to correct accumulated drift. Place recognition uses visual bag-of-words or learned descriptors to find candidate matches. When a loop is detected, pose graph optimization adjusts the entire trajectory to enforce global consistency.

Mathematical Foundations

The essential matrix relates corresponding points in two views:

Where each parameter means:

• is the normalized image point in the first frame (homogeneous coordinates)
• is the corresponding normalized image point in the second frame
• is the 3x3 essential matrix with rank 2
• The equation constrains epipolar geometry between the two views

Bundle adjustment minimizes reprojection error:

Where each parameter means:

• are the rotation and translation of camera
• is the 3D position of point
• is the observed 2D measurement of point in camera
• is the projection function from 3D to 2D
• is a robust cost function (Huber or Cauchy)
• is the measurement covariance matrix

Pose graph optimization enforces loop closure constraints:

Where each parameter means:

• is the pose (rotation and translation) of keyframe
• is the set of edges in the pose graph
• is the error between predicted and measured relative pose
• is the information matrix (inverse covariance) of the constraint
• The optimization distributes error across all connected poses

Architecture Design

Implementation

import numpy as np
import cv2


class ORBFeatureSLAM:
    def __init__(self):
        self.orb = cv2.ORB_create(nfeatures=1000)
        self.bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        self.keyframes = []
        self.map_points = []
        self.camera_matrix = np.array([[525, 0, 319.5], [0, 525, 239.5], [0, 0, 1]])
        self.current_pose = np.eye(4)

    def extract_features(self, frame):
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) if len(frame.shape) == 3 else frame
        keypoints, descriptors = self.orb.detectAndCompute(gray, None)
        return keypoints, descriptors

    def match_features(self, desc1, desc2):
        matches = self.bf.match(desc1, desc2)
        matches = sorted(matches, key=lambda x: x.distance)
        good_matches = [m for m in matches if m.distance < 50]
        return good_matches

    def estimate_pose(self, kp1, kp2, matches):
        pts1 = np.float32([kp1[m.queryIdx].pt for m in matches])
        pts2 = np.float32([kp2[m.trainIdx].pt for m in matches])
        E, mask = cv2.findEssentialMat(pts1, pts2, self.camera_matrix, method=cv2.RANSAC)
        _, R, t, mask = cv2.recoverPose(E, pts1, pts2, self.camera_matrix)
        pose = np.eye(4)
        pose[:3, :3] = R
        pose[:3, 3] = t.flatten()
        return pose

    def triangulate_points(self, pose1, pose2, kp1, kp2, matches):
        pts1 = np.float32([kp1[m.queryIdx].pt for m in matches]).T
        pts2 = np.float32([kp2[m.trainIdx].pt for m in matches]).T
        P1 = self.camera_matrix @ pose1[:3]
        P2 = self.camera_matrix @ pose2[:3]
        points_4d = cv2.triangulatePoints(P1, P2, pts1, pts2)
        points_3d = points_4d[:3] / points_4d[3]
        return points_3d.T

    def process_frame(self, frame):
        kp, desc = self.extract_features(frame)
        if len(self.keyframes) == 0:
            self.keyframes.append({'kp': kp, 'desc': desc, 'pose': np.eye(4)})
            return None, None
        prev = self.keyframes[-1]
        matches = self.match_features(prev['desc'], desc)
        if len(matches) < 10:
            return None, None
        pose = self.estimate_pose(prev['kp'], kp, matches)
        self.current_pose = self.current_pose @ pose
        points_3d = self.triangulate_points(prev['pose'], self.current_pose, prev['kp'], kp, matches)
        self.keyframes.append({'kp': kp, 'desc': desc, 'pose': self.current_pose.copy()})
        return self.current_pose, points_3d

Comparison Table

Common Challenges

1. Scale Drift: Monocular SLAM cannot recover absolute scale leading to drift over long trajectories
2. Motion Blur: Fast camera movement degrades feature quality and matching accuracy
3. Texture-less Scenes: Featureless regions like white walls prevent reliable feature extraction
4. Dynamic Objects: Moving objects violate static world assumptions and corrupt map building
5. Loop Closure False Positives: Incorrect place recognition can cause catastrophic map corruption

Visual Odometry and Incremental Mapping

Visual odometry estimates camera motion incrementally between consecutive frames, providing a baseline for SLAM systems. The 2-point algorithm with RANSAC computes the essential matrix using the minimum number of correspondences, enabling robust estimation in the presence of outliers. The 5-point algorithm handles non-planar scenes while the 8-point algorithm provides a linear solution for calibrated cameras.

Incremental mapping builds the 3D map progressively by triangulating new points from successfully tracked keyframes. The keyframe selection criteria ensure sufficient baseline for accurate triangulation while avoiding redundant frames. The typical interval is 10-20 frames or when the camera has moved 10% of the scene depth.

Map maintenance involves culling low-quality points that fail reprojection tests, merging redundant keyframes, and periodically running global bundle adjustment. The culling rules remove points observed by fewer than 3 keyframes or with reprojection error greater than 3 pixels. Global BA runs every 10-20 keyframes to correct accumulated drift.

Dense SLAM and Reconstruction

Dense SLAM systems reconstruct detailed 3D surfaces in real-time by processing every pixel. KinectFusion uses truncated signed distance functions (TSDF) to fuse depth maps from a Kinect sensor, producing dense meshes at 30 FPS. The GPU implementation processes 640x480 depth maps through raycasting and volume updating at interactive rates.

BundleFusion jointly optimizes camera poses and dense surface geometry using sparse-to-dense correspondences. The system alternates between spatial and temporal optimization, achieving globally consistent reconstruction even under challenging camera motion. The real-time performance is achieved through GPU-accelerated feature extraction and optimization.

Case Study: EuRoC MAV Dataset

EuRoC contains stereo images and IMU data from a micro aerial vehicle flying in two rooms and a machine hall at 20 FPS. ORB-SLAM3 achieves 0.015m ATE on the challenging Machine Hall sequence with aggressive motion. VINS-Mono achieves 0.018m ATE by tightly fusing visual features with IMU measurements for scale recovery. The dataset covers velocities from 0.3 to 1.8 m/s with rotational rates up to 180 deg/s. ORB features are extracted at 8 image pyramid levels with FAST threshold 20, producing approximately 1000 features per frame processed in 15ms. The machine hall sequence presents particular challenges with repetitive textures and metallic surfaces that create ambiguous feature matches, requiring robust outlier rejection through RANSAC and consistency checks.

Relocalization and Map Reuse

Relocalization recovers camera pose in a previously mapped environment, essential for loop closure and map reuse. The DBoW2 place recognition system converts visual features into bag-of-words representations for efficient database lookup. The vocabulary tree quantizes ORB descriptors into visual words, enabling fast similarity search across keyframes.

The relocalization pipeline detects candidate keyframes using vocabulary lookup, verifies matches through geometric verification with PnP RANSAC, and refines the pose using local bundle adjustment. The success rate depends on viewpoint variation and environmental changes, with typical rates of 85% for viewpoint changes less than 45 degrees.

Global localization using Monte Carlo localization maintains a particle filter over possible camera poses in the map. Each particle represents a hypothesized pose weighted by observation likelihood. This approach handles multi-modal pose distributions and provides uncertainty estimates essential for safe navigation.

Key Takeaways

• Visual SLAM builds maps and tracks position simultaneously using camera observations only
• ORB features provide efficient rotation-invariant descriptors for real-time feature matching
• Bundle adjustment jointly optimizes camera poses and 3D points minimizing reprojection error
• Loop closure detection corrects accumulated drift through global pose graph optimization
• Multi-threading separates tracking, mapping, and loop closure for real-time performance
• IMU fusion provides scale information and improves robustness during fast motion
• Robust kernels and RANSAC handle outliers from dynamic objects and matching errors
• Relocalization enables map reuse through place recognition and pose recovery