Guide Navigation

Quick Tips

• Click on any section to jump directly
• Hover over terms for explanations
• Check implementation boxes for code
• Use the progress tracker above

Advanced Visual SLAM Learning Guide

A comprehensive, scientifically-grounded guide to mastering Visual SLAM theory and implementation with expanded explanations and professional mathematical notation.

📚

13 Chapters

Part I: Fundamental Knowledge

Chapters 1–5: Building the mathematical and conceptual foundation

The SLAM Context and Core Framework

Theory Foundation

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.

Core SLAM Problem

Visual SLAM addresses the fundamental challenge of simultaneously estimating a robot's 6-DOF pose (localization) while incrementally building a consistent map of its environment (mapping) using primarily visual sensors. This creates a chicken-and-egg problem where an accurate map requires precise localization, and precise localization requires an accurate map.

Mathematical Formulation

Motion Model: $$ \mathbf{x}_{k}=f(\mathbf{x}_{k-1},\mathbf{u}_{k},\mathbf{w}_{k}) $$ Observation Model: $$ \mathbf{z}_{k,j}=h(\mathbf{y}_{j},\mathbf{x}_{k},\mathbf{v}_{k,j}) $$

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.

Historical Context: Early SLAM implementations used Extended Kalman Filters (EKF), but modern approaches favor optimization techniques due to EKF's linearization errors and quadratic complexity.

Implementation Focus

Setting up the development environment with C++, CMake, and understanding the basic project structure for SLAM implementations.

// 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;
};

Pose Representation in 3D Space

Geometry Math

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\)).

Transformation Matrix (\(SE(3)\))

Combines the 3 DoF rotation and 3 DoF translation (\(\mathbf{t}\)) into a single \(4\times4\) matrix using homogeneous coordinates:

\mathbf{T} = \begin{pmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix}

This matrix representation allows efficient composition of multiple transformations and application to points using simple matrix multiplication.

Rotation Representations

Representation	DoF	Advantages	Disadvantages
Rotation Matrix	3	Intuitive, direct composition	9 parameters with 6 constraints
Euler Angles	3	Minimal, intuitive	Gimbal lock, non-unique
Rotation Vector	3	Minimal, no singularities	Non-intuitive, complex interpolation
Quaternions	4	No singularities, efficient composition	4 parameters (1 redundant), non-intuitive

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

Implementation Focus

Using Eigen's specialized classes for efficient pose manipulation and transformation composition.

// 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;

Lie Group and Lie Algebra for Optimization

Advanced Math

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.

Lie Theory in SLAM

Lie theory provides the mathematical framework to perform calculus on manifolds like SO(3) and SE(3), which is essential for optimizing pose variables in SLAM without violating the manifold structure.

Exponential Map: $$ \mathbf{T} = \exp(\boldsymbol{\xi}^{\wedge}) $$ Logarithmic Map: $$ \boldsymbol{\xi} = \log(\mathbf{T})^{\vee} $$

Derivatives and Perturbation Model

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

\frac{\partial (\mathbf{T} \mathbf{p})}{\partial \delta\boldsymbol{\xi}} = - (\mathbf{T} \mathbf{p})^{\odot} = \begin{pmatrix} \mathbf{I} & -(\mathbf{T} \mathbf{p})^{\wedge} \\ \mathbf{0}^T & \mathbf{0}^T \end{pmatrix}

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.

Baker-Campbell-Hausdorff (BCH) Formula: Describes how Lie Group multiplication relates to Lie Algebra addition: \(\ln(\exp(\mathbf{A})\exp(\mathbf{B})) \approx \mathbf{A} + \mathbf{B} + \frac{1}{2}[\mathbf{A}, \mathbf{B}] + \dots\)

Implementation Focus

Utilizing Sophus to compute \(\exp(\cdot)\), \(\log(\cdot)\), and apply the pose update \(\mathbf{T}_{\text{new}} = \exp(\delta\boldsymbol{\xi}^{\wedge})\mathbf{T}_{\text{current}}\).

// 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);

The Observation Model: Cameras and Pixels

Computer Vision Geometry

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}\).

Camera Projection Pipeline

The process of projecting a 3D world point to a 2D image pixel involves multiple coordinate transformations:

World coordinates → Camera coordinates (using extrinsics)
Camera coordinates → Normalized coordinates (perspective division)
Normalized coordinates → Pixel coordinates (using intrinsics)

Projection Formula: $$ Z\mathbf{p}_{uv} = \mathbf{K}\mathbf{P}_c $$ Intrinsic Matrix: $$ \mathbf{K} = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} $$

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.

Radial Distortion Model: $$ \mathbf{x}_{\text{distorted}} = \mathbf{x} (1 + k_1 r^2 + k_2 r^4 + k_3 r^6) $$ Stereo Depth: $$ \mathbf{Z} = \frac{f b}{d} $$

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).

Camera Calibration: The process of estimating intrinsic parameters (focal length, principal point) and distortion coefficients is essential for accurate SLAM.

Implementation Focus

Using OpenCV to manage camera intrinsics, perform camera calibration, and implement projection and distortion correction.

// 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);

Nonlinear Optimization for State Estimation

Optimization Math

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.

\min_{\mathbf{x}} J(\mathbf{x}) = \sum_k \mathbf{e}_{u,k}^T \mathbf{R}_k^{-1} \mathbf{e}_{u,k} + \sum_{k,j} \mathbf{e}_{z,k,j}^T \mathbf{Q}_{k,j}^{-1} \mathbf{e}_{z,k,j}

Optimization Methods

Method	Key Idea	Advantages
Gauss-Newton	Linearize and solve normal equations	Fast convergence near optimum
Levenberg-Marquardt	Damped version of Gauss-Newton	More robust, handles poor initial guesses
Gradient Descent	Follow negative gradient direction	Simple, guaranteed convergence

Solving the Optimization

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

Normal Equation: $$ \mathbf{H}\Delta\mathbf{x} = \mathbf{g} $$ Levenberg-Marquardt: $$ (\mathbf{H} + \lambda \mathbf{I})\Delta\mathbf{x} = \mathbf{g} $$

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

Hessian Sparsity: In SLAM problems, the Hessian matrix exhibits a characteristic sparse block structure that can be exploited for efficient computation.

Implementation Focus

Hand-coding the iterative solution (Gauss-Newton) and leveraging external libraries like Ceres and g2o for robust optimization.

// 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

Feature-Based Visual Odometry

Implementation Computer Vision

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).

Motion Estimation Methods

2D-2D (Epipolar Geometry)

For monocular cameras, uses essential matrix to recover R,t up to scale with constraint \(\mathbf{x}_2^T \mathbf{E} \mathbf{x}_1 = 0\).

3D-2D (PnP)

Perspective-n-Point, solves for camera pose given 3D points and 2D projections by minimizing reprojection error.

3D-3D (ICP)

Iterative Closest Point, aligns two 3D point clouds by minimizing point-to-point or point-to-plane distances.

Mathematical Formulation

Essential Matrix Constraint: $$ \mathbf{x}_2^T \mathbf{E} \mathbf{x}_1 = 0 $$ Reprojection Error: $$ \mathbf{e}_{\text{reproj}} = \mathbf{p}_{\text{observed}} - \mathbf{p}_{\text{projected}}(\mathbf{T}) $$

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.

Triangulation Contradiction: Depth estimation is fragile when translation is small, making \(\mathbf{P}\) estimation uncertain due to numerical instability in the triangulation equations.

Implementation Focus

Using OpenCV for feature detection, description, and matching, then implementing PnP solvers with RANSAC for robust pose estimation.

// 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);

Direct Visual Odometry

Implementation Optimization

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.

Direct vs Feature-Based Methods

Direct Methods

Use pixel intensities directly
No feature extraction needed
Work in textureless regions
Sensitive to lighting changes

Feature-Based Methods

Use distinctive image features
Robust to lighting changes
Fail in textureless regions
Feature extraction overhead

Mathematical Formulation

Photometric Error: $$ e_i = I_1(\mathbf{p}_{1,i}) - I_2(\mathbf{p}_{2,i}(\mathbf{T})) $$ Jacobian Chain Rule: $$ \frac{\partial e}{\partial \delta\boldsymbol{\xi}} = - \frac{\partial I_2}{\partial \mathbf{u}} \frac{\partial \mathbf{u}}{\partial \mathbf{q}} \frac{\partial \mathbf{q}}{\partial \delta\boldsymbol{\xi}} $$

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.

Lucas-Kanade Optical Flow: Solves for 2D shift \(\mathbf{u}\) using multiple pixels in a patch: \(\mathbf{A}^T \mathbf{A} \mathbf{u} = - \mathbf{A}^T \mathbf{b}\).

Implementation Focus

Implementing single-layer and multi-layer Direct Pose Estimation, utilizing parallel processing for speed on CPU.

// 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;
    }
}

Backend Optimization Structure

Backend Optimization

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.

SLAM Backend Evolution

Approach	Key Idea	Advantages
EKF-SLAM	Recursive Bayesian filtering	Theoretically sound, online operation
Bundle Adjustment	Batch nonlinear optimization	High accuracy, handles loop closures
Pose Graph	Marginalize landmarks, optimize poses only	Efficient for large-scale mapping

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.

Schur Complement: $$ \mathbf{S} = \mathbf{B} - \mathbf{E} \mathbf{C}^{-1} \mathbf{E}^T $$

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.

EKF vs. Optimization: EKF suffers from non-linearity errors and its state dimensionality grows with O(N) poses and O(M) landmarks. Optimization handles non-linearity via iterative relinearization and manages complexity via sparsity.

Implementation Focus

Using Ceres and g2o for BA (with LM and robust kernels like Huber) and implementing a g2o-based pose graph optimizer.

// 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);

Loop Closure Detection

Recognition Machine Learning

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.

Bag-of-Words Model

The Bag-of-Words approach treats images as collections of visual words from a predefined vocabulary, enabling efficient place recognition through text retrieval techniques adapted for visual data.

TF-IDF Weighting: $$ \eta_i = \text{TF}_i \times \text{IDF}_i $$ Inverse Document Frequency: $$ \text{IDF}_i = \log\left(\frac{N}{n_i}\right) $$

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.

TF-IDF Explanation: Term Frequency measures how often a word appears in the current document. Inverse Document Frequency measures the discriminative power of the word across the whole vocabulary.

Loop Closure Pipeline

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

Implementation Focus

Using the DBoW3 library to train a visual vocabulary, transform feature descriptors into BoW vectors, and query for similar images within a database.

// 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);
    }
}

Mapping and Dense Reconstruction

Mapping 3D Vision

Volumetric and Probabilistic Maps

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

Map Representations in SLAM

Sparse Maps

Only distinctive features, efficient for localization but limited for navigation

Dense Maps

All surfaces modeled, useful for navigation but computationally expensive

Semantic Maps

Objects labeled with categories, enables high-level reasoning and interaction

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.

Bayesian Depth Update: $$ \mu_{\text{fuse}} = \frac{\sigma^2_{\text{obs}}\mu + \sigma^2\mu_{\text{obs}}}{\sigma^2 + \sigma^2_{\text{obs}}}, \quad \sigma^2_{\text{fuse}} = \frac{\sigma^2\sigma^2_{\text{obs}}}{\sigma^2 + \sigma^2_{\text{obs}}} $$ OctoMap Log-Odds Update: $$ L(\mathbf{n}|\mathbf{z}_t) = L(\mathbf{n}|\mathbf{z}_{t-1}) + L(\mathbf{n}|\mathbf{z}_t) $$

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.

Implementation Focus

Using PCL for 3D point cloud manipulation and OctoMap for occupancy grid creation.

// 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);

System Integration and Future Trends

Advanced Future

Advanced Systems and Sensor Fusion

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

Modern SLAM Systems

ORB-SLAM3

Multi-map Atlas system
Visual-inertial capabilities
Robust place recognition
Open-source implementation

Kimera

Metric-semantic mapping
Real-time mesh reconstruction
Semantic labeling
Geometric and semantic SLAM

Emerging Trends and Challenges

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

VIO (Visual-Inertial Odometry): Fusing high-frequency IMU data with visual data provides robustness to fast motion and reduces drift through complementary sensor characteristics.

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

Implementation Focus

Building a stable SLAM system requires robust engineering practices beyond individual algorithms, including data management, multi-threading, and proper keyframe selection.

// 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