Visual SLAM from theory to implementation

Part I: Fundamental Knowledge

Chapters 1–5: Building the mathematical and conceptual foundation

1. The SLAM Context and Core Framework

Scientific Foundation

The SLAM problem is formally defined by two stochastic equations representing motion and observation models. The noise terms (\(\mathbf{w}_k\), \(\mathbf{v}_{k,j}\)) are typically modeled as Gaussian distributions (\(N(0, \mathbf{R}_k)\)) to enable probabilistic estimation techniques.

Mathematical Formulation

The motion equation models how the robot's state evolves over time based on control inputs, while the observation equation relates the landmark state and current pose to sensor measurements.

// Basic SLAM system structure
class SLAMSystem {
public:
    void processFrame(const cv::Mat& image, double timestamp);
    Eigen::Matrix4d getCurrentPose() const;
    // ...
private:
    Frontend frontend;
    Backend backend;
    Map map;
};

2. Pose Representation in 3D Space

Mathematical Foundation

Finding a robust, compact representation for rotation (\(R\)) and translation (\(T\)) is crucial. The Rotation Matrix (\(\mathbf{R} \in SO(3)\)) is redundant (9 elements) but only has 3 degrees of freedom (DoF), requiring 6 geometric constraints (\(R^T R = I\), \(\det(R)=1\)).

Rotation Representations

Quaternions are particularly useful for interpolation (slerp) and avoid the gimbal lock problem that plagues Euler angles.

// Create a rotation matrix from angle-axis representation
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));
Eigen::Matrix3d R = rotation_vector.toRotationMatrix();
// Create a transformation matrix
Eigen::Vector3d t(1.0, 2.0, 3.0);
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(R);
T.translate(t);
// Apply transformation to a point
Eigen::Vector3d p_world(1, 0, 0);
Eigen::Vector3d p_camera = T * p_world;

3. Lie Group and Lie Algebra for Optimization

Theoretical Foundation

The Lie Algebra (\(\mathfrak{se}(3)\)) is the vector space tangent to the Lie Group (\(SE(3)\)) at the identity, allowing optimization increments (\(\delta\boldsymbol{\xi}\)) to be treated as simple vector additions without violating geometric constraints.

Derivatives and Perturbation Model

The Jacobian for pose optimization is calculated by left-multiplying a perturbation \(\Delta \mathbf{T} = \exp(\delta\boldsymbol{\xi}^{\wedge})\):

This Jacobian tells us how a point transformed by T changes when we make a small change to the pose parameters in the Lie algebra.

// Example Sophus usage for Lie group operations
Sophus::SE3d T; // Current pose
Eigen::Vector6d update; // Update in Lie algebra
// Apply update using exponential map
Sophus::SE3d T_new = Sophus::SE3d::exp(update) * T;
// Convert back to Lie algebra for optimization
Eigen::Vector6d lie_alg = T_new.log();
// Perturbation model for derivatives
Eigen::Matrix J = Sophus::SE3d::JacobianR(lie_alg);

4. The Observation Model: Cameras and Pixels

Camera Projection Model

The transformation from a 3D point \(\mathbf{P}_w\) in the world frame to a 2D pixel \(\mathbf{p}_{uv}\) is given by the pinhole model, applied after the pose transform \(\mathbf{T}_{CW}\).

Lens Distortion and Stereo Vision

Real cameras exhibit lens distortion that must be corrected for accurate geometric computations. For stereo systems, depth can be calculated from disparity.

Lens distortion causes straight lines in the world to appear curved in images. Stereo vision triangulates depth by finding corresponding points in left and right images and using their horizontal displacement (disparity).

// OpenCV camera projection example
cv::Mat cameraMatrix = (cv::Mat_(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
cv::Mat distCoeffs = (cv::Mat_(5,1) << k1, k2, p1, p2, k3);
// Project 3D point to 2D
std::vector objectPoints;
std::vector imagePoints;
cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
// Undistort image
cv::Mat undistorted;
cv::undistort(image, undistorted, cameraMatrix, distCoeffs);

5. Nonlinear Optimization for State Estimation

Least-Squares Optimization

The system seeks to minimize the weighted sum of squared errors, equivalent to finding the Maximum A Posteriori (MAP) estimate under Gaussian assumptions.

Solving the Optimization

The normal equation \(\mathbf{H}\Delta\mathbf{x} = \mathbf{g}\) provides the solution to the linearized optimization problem at each iteration.

The damping parameter \(\lambda\) adapts during optimization to balance between fast convergence (small \(\lambda\), Gauss-Newton behavior) and stability (large \(\lambda\), gradient descent behavior).

// Ceres Solver example for bundle adjustment
ceres::Problem problem;
for (int i = 0; i < observations.size(); ++i) {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction(
            new ReprojectionError(observations[i]));
    problem.AddResidualBlock(cost_function, NULL, camera, point);
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

Part II: SLAM Technologies

Chapters 6–13: Implementing the theoretical foundation in real-world systems

6. Feature-Based Visual Odometry

Feature Processing

Using efficient features like ORB (Oriented FAST Keypoints + Rotated BRIEF Descriptors). Matching uses brute-force comparison or accelerated methods like FLANN based on distance (e.g., Hamming distance for binary descriptors).

Mathematical Formulation

The essential matrix E encapsulates the epipolar geometry between two views and can be decomposed to recover the relative pose. PnP is the most common method in feature-based VO as it leverages 3D map points to estimate new camera poses.

// ORB feature extraction and matching
cv::Ptr orb = cv::ORB::create(500);
std::vector kp1, kp2;
cv::Mat desc1, desc2;
orb->detectAndCompute(img1, cv::noArray(), kp1, desc1);
orb->detectAndCompute(img2, cv::noArray(), kp2, desc2);
// Feature matching
cv::BFMatcher matcher(cv::NORM_HAMMING);
std::vector matches;
matcher.match(desc1, desc2, matches);
// PnP with RANSAC
std::vector objectPoints;
std::vector imagePoints;
// ... populate points from matches
cv::Mat rvec, tvec;
cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, 
                   distCoeffs, rvec, tvec, false, 100, 8.0, 0.99);

7. Direct Visual Odometry

Photometric Optimization

Direct methods minimize brightness error \(e_i = I_1(\mathbf{p}_{1,i}) - I_2(\mathbf{p}_{2,i}(\mathbf{T}))\) over selected pixels. The large motions require Multi-Layer Pyramids for coarse-to-fine estimation.

Mathematical Formulation

This chain rule derivation connects changes in pixel intensity to changes in camera pose through the image formation process. The Constant Grayscale Assumption is crucial but can be violated by lighting changes or non-Lambertian surfaces.

// Direct method implementation (simplified)
void directPoseEstimation(
    const cv::Mat& img1, 
    const cv::Mat& img2,
    const std::vector& px_ref,
    const std::vector& pts_ref,
    Sophus::SE3d& T21) {
    // Parameters
    const int iterations = 10;
    double cost = 0, lastCost = 0;
    for (int iter = 0; iter < iterations; iter++) {
        Eigen::Matrix H = Eigen::Matrix::Zero();
        Eigen::Matrix g = Eigen::Matrix::Zero();
        cost = 0;
        // Compute photometric error and Jacobians
        for (size_t i = 0; i < px_ref.size(); i++) {
            // ... compute error and Jacobian for each point
            // H += J.transpose() * J;
            // g += -J.transpose() * error;
        }
        // Solve H * dx = g
        Eigen::Matrix dx = H.ldlt().solve(g);
        T21 = Sophus::SE3d::exp(dx) * T21;
        // Check convergence
        if (isnan(dx[0])) break;
        if (iter > 0 && cost >= lastCost) break;
        lastCost = cost;
    }
}

8. Backend Optimization Structure

Bundle Adjustment Structure

Bundle Adjustment (BA) refines both camera poses and 3D point positions by minimizing reprojection error. The Hessian matrix in BA exhibits a characteristic sparse block structure that can be exploited for efficient computation.

Schur Complement and Marginalization

The Schur complement trick exploits the block structure of the Hessian to dramatically reduce the problem size by eliminating landmark variables.

This reduces the system to only pose variables, which is much smaller than the full BA problem. Pose graphs take this further by explicitly marginalizing landmarks to create constraints between pose nodes only.

// g2o pose graph optimization example
typedef g2o::BlockSolver> BlockSolverType;
typedef g2o::LinearSolverEigen LinearSolverType;
auto solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique(g2o::make_unique())
);
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
// Add pose vertices
for (size_t i = 0; i < poses.size(); ++i) {
    g2o::VertexSE3* v = new g2o::VertexSE3();
    v->setId(i);
    v->setEstimate(poses[i]);
    optimizer.addVertex(v);
}
// Add edges (constraints between poses)
for (const auto& constraint : constraints) {
    g2o::EdgeSE3* e = new g2o::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.initializeOptimization();
optimizer.optimize(10);

9. Loop Closure Detection

Appearance-Based Loop Detection

Bag-of-Words (BoW) models use a dictionary generated by hierarchical clustering (e.g., K-d Tree). High Precision is prioritized, as false loops lead to catastrophic map collapse.

Loop Validation and Metrics

Loop validation often uses time-consistency checks and geometric verification to prevent singular false positive detections. Multiple consecutive detections or geometric verification are used to confirm loop closures.

Loop Closure Pipeline

1. Feature extraction from current frame
2. BoW vector generation
3. Database query for similar images
4. Geometric verification (essential matrix check)
5. Pose graph optimization with new constraint

// DBoW3 loop closure detection example
#include 
// Create vocabulary (offline)
DBoW3::Vocabulary vocab;
vocab.create(features); // features from training images
// Create database
DBoW3::Database database(vocab, false, 0);
// Add images to database
for (size_t i = 0; i < image_features.size(); ++i) {
    database.add(image_features[i]);
}
// Query for loop closures
DBoW3::QueryResults results;
database.query(current_features, results, 4); // top 4 matches
// Check for valid loop closure
if (!results.empty() && results[0].Score > 0.1) {
    // Perform geometric verification
    if (geometricVerification(current_frame, loop_frame)) {
        // Add loop closure constraint to pose graph
        addLoopClosureConstraint(current_frame_id, loop_frame_id, transform);
    }
}

10. Mapping and Dense Reconstruction

Volumetric and Probabilistic Maps

Different mapping approaches balance computational efficiency with representation accuracy, from sparse feature maps to dense surface reconstructions.

Depth Estimation and Fusion

Monocular depth estimation iteratively fuses depth observations into probabilistic representations, while volumetric approaches like OctoMap and TSDF provide efficient 3D representations.

The Bayesian update combines new depth measurements with prior knowledge, gradually refining depth estimates. OctoMap uses an efficient octree data structure to represent 3D space at multiple resolutions.

// OctoMap example
#include 
// Create octree
octomap::OcTree tree(0.05);  // 5cm resolution
// Insert point cloud
for (const auto& point : point_cloud) {
    // Update occupancy of leaf node at point
    tree.updateNode(point.x, point.y, point.z, true);
}
// Prune to remove unnecessary nodes
tree.prune();
// Query occupancy at specific location
octomap::OcTreeNode* node = tree.search(1.0, 2.0, 0.5);
if (node && tree.isNodeOccupied(node)) {
    // Location is occupied
}
// Save map
tree.writeBinary("map.bt");
// PCL point cloud processing
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
pcl::fromPCLPointCloud2(pcl_cloud, *cloud);
// Voxel grid filtering for downsampling
pcl::VoxelGrid voxel_grid;
voxel_grid.setInputCloud(cloud);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f); // 1cm resolution
voxel_grid.filter(*filtered_cloud);

11. System Integration and Future Trends

Advanced Systems and Sensor Fusion

Modern SLAM systems integrate multiple sensors and leverage advanced computational techniques to improve robustness, accuracy, and scalability.

Emerging Trends and Challenges

The field of Visual SLAM continues to evolve with new techniques addressing long-standing challenges and expanding capabilities.

Future Directions

• Semantic SLAM: Integrating object recognition and scene understanding
• Long-term autonomy: Handling environmental changes over extended periods
• Multi-robot SLAM: Collaborative mapping across multiple agents
• Deep Learning: End-to-end learning of SLAM components

// Keyframe management in SLAM system
class KeyFrame {
public:
    KeyFrame(const cv::Mat& image, double timestamp, 
             const Sophus::SE3d& pose);
    void addMapPoint(MapPoint* mp, size_t idx);
    bool isBad() const { return mbBad; }
    void setBadFlag();
    // Getters for pose, features, etc.
    Sophus::SE3d getPose() const { return mPose; }
private:
    Sophus::SE3d mPose;
    cv::Mat mImage;
    std::vector mvpMapPoints;
    bool mbBad = false;
    double mTimestamp;
};
// Map management
class Map {
public:
    void addKeyFrame(KeyFrame* pKF);
    void addMapPoint(MapPoint* pMP);
    void eraseKeyFrame(KeyFrame* pKF);
    void eraseMapPoint(MapPoint* pMP);
    std::vector getAllKeyFrames();
    std::vector getAllMapPoints();
private:
    std::set mspKeyFrames;
    std::set mspMapPoints;
    std::mutex mMutexMap;
};

Implementation Resources

Essential libraries, learning materials, and open-source systems

Essential Libraries

• Eigen

  C++ template library for linear algebra, matrices, vectors, numerical solvers

• OpenCV

  Computer vision and image processing with extensive algorithms

• g2o

  General graph optimization framework for SLAM problems

• Ceres Solver

  Nonlinear least squares minimizer with automatic differentiation

• PCL

  Point Cloud Library for 3D processing and visualization

Learning Materials

• "Introduction to Visual SLAM"

  Gao Xiang, Tao Zhang - Primary textbook with theory and practice

• "Multiple View Geometry"

  Hartley & Zisserman - Computer vision classic with mathematical rigor

• "Probabilistic Robotics"

  Thrun et al. - Robotics fundamentals with probabilistic approaches

• OpenCV Tutorials

  Official documentation and examples for practical implementation

Open Source Systems

• ORB-SLAM3

  Most popular feature-based SLAM with multi-map capabilities

• OpenVSLAM

  General visual SLAM framework with various camera models

• Kimera

  Semantic SLAM with metric-semantic mapping and mesh reconstruction

• VINS-Mono

  Visual-inertial odometry with tight coupling of camera and IMU