Visual SLAM from theory to implementation

Visual SLAM (simultaneous localisation and mapping) lets a robot work out where it is and build a map at the same time, using cameras as its main sensor. It is a chicken-and-egg problem: an accurate map needs an accurate position, and an accurate position needs an accurate map.

This guide follows the path of the well-known textbook Introduction to Visual SLAM (Gao and Zhang): first the mathematics (chapters 1–5), then the parts of a real system (chapters 6–11). Each chapter gives the key idea, the essential equations and a short C++ sketch with the library you would use.

A complete visual SLAM system looks like this:

$$ \text{Camera} \;\rightarrow\; \underbrace{\text{Front end}}_{\text{features, tracking}} \;\rightarrow\; \underbrace{\text{Back end}}_{\text{optimisation}} \;\rightarrow\; \underbrace{\text{Loop closure}}_{\text{fix drift}} \;\rightarrow\; \text{Map} $$

Part I: Foundations

1. The SLAM problem

SLAM is described by two equations. The motion model says how the robot's state \(\mathbf{x}_k\) changes with the control input \(\mathbf{u}_k\); the observation model says what the sensor measures, \(\mathbf{z}_{k,j}\), when the robot at \(\mathbf{x}_k\) looks at landmark \(\mathbf{y}_j\):

$$ \mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \qquad \mathbf{z}_{k,j} = h(\mathbf{y}_j, \mathbf{x}_k) + \mathbf{v}_{k,j} $$

The noise terms are usually modelled as zero-mean Gaussians, \(\mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)\) and \(\mathbf{v}_{k,j} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{k,j})\). SLAM means estimating all \(\mathbf{x}_k\) and \(\mathbf{y}_j\) from the \(\mathbf{u}_k\) and \(\mathbf{z}_{k,j}\).

Filters or optimisation? Early systems used the extended Kalman filter (EKF). Modern systems use nonlinear optimisation, because the EKF linearises only once (so errors pile up) and its cost grows quadratically with the number of landmarks.

A SLAM program is usually organised like this:

class SLAMSystem {
public:
    void processFrame(const cv::Mat& image, double timestamp);
    Sophus::SE3d currentPose() const;
private:
    Frontend frontend_;   // features, matching, tracking
    Backend  backend_;    // optimisation
    Map      map_;        // keyframes and map points
};

2. Rotation and pose in 3D

A rotation is a matrix \(\mathbf{R}\) in the special orthogonal group:

$$ SO(3) = \{\, \mathbf{R} \in \mathbb{R}^{3\times3} \mid \mathbf{R}^\top\mathbf{R} = \mathbf{I},\ \det\mathbf{R} = 1 \,\} $$

It has 9 numbers but only 3 degrees of freedom. Rotation and translation \(\mathbf{t}\) together form a pose in the special Euclidean group \(SE(3)\), written as one 4 × 4 matrix:

$$ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix} \in SE(3), \qquad \tilde{\mathbf{p}}' = \mathbf{T}\,\tilde{\mathbf{p}} $$

where \(\tilde{\mathbf{p}} = [\,x, y, z, 1\,]^\top\) are homogeneous coordinates. Chaining poses is matrix multiplication: \(\mathbf{T}_{AC} = \mathbf{T}_{AB}\mathbf{T}_{BC}\).

With Eigen:

#include <Eigen/Geometry>

Eigen::AngleAxisd rot(M_PI / 4, Eigen::Vector3d::UnitZ());  // 45° about z
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rot);
T.pretranslate(Eigen::Vector3d(1.0, 2.0, 3.0));

Eigen::Vector3d p_world(1, 0, 0);
Eigen::Vector3d p_camera = T * p_world;        // rotate, then translate
Eigen::Quaterniond q(rot);                     // same rotation as a quaternion

3. Lie groups and Lie algebras

To optimise a pose we need to take small steps and derivatives. But adding two rotation matrices does not give a rotation matrix. The Lie algebra solves this: each pose \(\mathbf{T}\) corresponds to a 6-vector \(\boldsymbol{\xi} = [\boldsymbol{\rho}; \boldsymbol{\phi}]\) (translation part, rotation part), and we can add vectors freely. The exponential and logarithm maps convert between them:

$$ \mathbf{T} = \exp(\boldsymbol{\xi}^\wedge), \qquad \boldsymbol{\xi} = \log(\mathbf{T})^\vee $$

\(^\wedge\) turns a vector into a matrix and \(^\vee\) turns it back. For rotations, \(\boldsymbol{\phi}^\wedge\) is the skew-symmetric matrix with \(\boldsymbol{\phi}^\wedge \mathbf{a} = \boldsymbol{\phi} \times \mathbf{a}\).

The perturbation model. Instead of differentiating with respect to \(\boldsymbol{\xi}\) itself, we apply a small left perturbation \(\exp(\delta\boldsymbol{\xi}^\wedge)\,\mathbf{T}\) and differentiate with respect to \(\delta\boldsymbol{\xi}\). For a transformed point \(\mathbf{p}' = \mathbf{T}\mathbf{p}\) this gives a simple Jacobian:

$$ \frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta\boldsymbol{\xi}} = \begin{bmatrix} \mathbf{I} & -(\mathbf{T}\mathbf{p})^\wedge \end{bmatrix} \in \mathbb{R}^{3 \times 6} $$

This small matrix is used in almost every SLAM optimisation. Combining two exponentials is not simply adding the vectors; the BCH formula gives the correction:

$$ \log\!\big(\exp(\mathbf{A})\exp(\mathbf{B})\big) = \mathbf{A} + \mathbf{B} + \tfrac{1}{2}[\mathbf{A}, \mathbf{B}] + \cdots $$

With Sophus:

#include <sophus/se3.hpp>

Sophus::SE3d T;                                   // current pose
Eigen::Matrix<double, 6, 1> dxi;                  // update from the optimiser
dxi << 0.01, 0.0, 0.0, 0.0, 0.0, 0.005;           // (translation, rotation)

Sophus::SE3d T_new = Sophus::SE3d::exp(dxi) * T;  // left-multiplicative update
Eigen::Matrix<double, 6, 1> xi = T_new.log();     // back to the Lie algebra

4. Cameras: from 3D points to pixels

Projecting a world point \(\mathbf{P}_w\) to a pixel \((u, v)\) takes three steps:

$$ \underbrace{\mathbf{P}_c = \mathbf{T}_{cw}\,\mathbf{P}_w}_{\text{world} \to \text{camera}} \;\rightarrow\; \underbrace{\left(\tfrac{X_c}{Z_c},\ \tfrac{Y_c}{Z_c},\ 1\right)}_{\text{normalised plane}} \;\rightarrow\; \underbrace{Z_c \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K}\,\mathbf{P}_c}_{\text{pixels}}, \qquad \mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} $$

\(\mathbf{K}\) holds the intrinsics: focal lengths \(f_x, f_y\) and principal point \((c_x, c_y)\), all in pixels.

Lens distortion. Real lenses bend straight lines. The radial part depends on the distance \(r\) from the image centre in normalised coordinates:

$$ x_\text{dist} = x\,(1 + k_1 r^2 + k_2 r^4 + k_3 r^6), \qquad y_\text{dist} = y\,(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) $$

plus a small tangential part (\(p_1, p_2\)). Calibration estimates \(\mathbf{K}\) and these coefficients once per camera.

Stereo depth. Two cameras a baseline \(b\) apart see a point shifted horizontally by the disparity \(d\) pixels. Its depth is

$$ Z = \frac{f_x\, b}{d} $$

Example: \(f_x = 700\) px, \(b = 0.12\) m and \(d = 21\) px give \(Z = 4.0\) m. Note that the error grows quickly for distant points, where \(d\) is small.

With OpenCV:

cv::Mat K = (cv::Mat_<double>(3, 3) << fx, 0, cx,  0, fy, cy,  0, 0, 1);
cv::Mat dist = (cv::Mat_<double>(5, 1) << k1, k2, p1, p2, k3);

std::vector<cv::Point3f> object_points;   // 3D points
std::vector<cv::Point2f> image_points;    // their projections
cv::projectPoints(object_points, rvec, tvec, K, dist, image_points);

cv::Mat undistorted;
cv::undistort(image, undistorted, K, dist);

5. Nonlinear optimisation

With Gaussian noise, the most probable trajectory and map (the maximum a posteriori estimate) is the one that minimises the weighted sum of squared errors:

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

The errors are nonlinear in \(\mathbf{x}\), so we linearise around the current estimate with the Jacobian \(\mathbf{J}\) and solve for a step \(\Delta\mathbf{x}\) again and again:

$$ \underbrace{\mathbf{J}^\top\mathbf{J}}_{\mathbf{H}}\,\Delta\mathbf{x} = \underbrace{-\mathbf{J}^\top\mathbf{e}}_{\mathbf{g}} \qquad \text{(Gauss–Newton)} $$

$$ (\mathbf{H} + \lambda\mathbf{I})\,\Delta\mathbf{x} = \mathbf{g} \qquad \text{(Levenberg–Marquardt)} $$

A small \(\lambda\) behaves like Gauss–Newton, a large one like cautious gradient descent. In SLAM, \(\mathbf{H}\) is large but sparse, and good solvers exploit that.

With Ceres, you write the error once and let it compute the derivatives:

struct ReprojectionError {
    ReprojectionError(double u, double v) : u_(u), v_(v) {}
    template <typename T>
    bool operator()(const T* camera, const T* point, T* residual) const {
        T p[3];
        ceres::AngleAxisRotatePoint(camera, point, p);           // rotate
        p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5]; // translate
        residual[0] = T(fx) * p[0] / p[2] + T(cx) - T(u_);
        residual[1] = T(fy) * p[1] / p[2] + T(cy) - T(v_);
        return true;
    }
    double u_, v_;
};

ceres::Problem problem;
for (const auto& obs : observations) {
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6, 3>(
            new ReprojectionError(obs.u, obs.v)),
        new ceres::HuberLoss(1.0),                     // robust to outliers
        cameras[obs.cam].data(), points[obs.pt].data());
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

(fx, fy, cx, cy are the camera intrinsics, as constants.)

Part II: Building the system

6. Feature-based visual odometry

Visual odometry estimates the camera motion between frames. The feature-based way detects and matches ORB features (FAST corners plus rotated BRIEF descriptors, compared with the Hamming distance), then estimates the motion from the matches. The keypoints and matching post explains this step in detail.

The two key equations are the epipolar constraint between normalised image points, and the reprojection error that PnP minimises:

$$ \mathbf{x}_2^\top\,\mathbf{E}\,\mathbf{x}_1 = 0, \quad \mathbf{E} = \mathbf{t}^\wedge\mathbf{R}; \qquad \mathbf{e}_i = \mathbf{u}_i - \frac{1}{Z_i}\,\mathbf{K}\,(\mathbf{T}\,\mathbf{P}_i)_{1:3} $$

Triangulation needs translation. Depth comes from seeing a point from two different places. If the camera only rotates, or moves very little, the depth of new points is badly determined.

cv::Ptr<cv::ORB> orb = cv::ORB::create(1000);
std::vector<cv::KeyPoint> kp1, kp2;
cv::Mat desc1, desc2;
orb->detectAndCompute(img1, cv::noArray(), kp1, desc1);
orb->detectAndCompute(img2, cv::noArray(), kp2, desc2);

cv::BFMatcher matcher(cv::NORM_HAMMING);
std::vector<cv::DMatch> matches;
matcher.match(desc1, desc2, matches);

// 3D map points and their 2D observations in the new image
std::vector<cv::Point3f> pts3d;
std::vector<cv::Point2f> pts2d;
// ... fill from the matches ...
cv::Mat rvec, tvec;
std::vector<int> inliers;
cv::solvePnPRansac(pts3d, pts2d, K, dist, rvec, tvec,
                   false, 100, 8.0, 0.99, inliers);

7. Direct visual odometry

Direct methods skip features and compare pixel brightness directly. Assuming a point looks equally bright in both images (the photometric constancy assumption), they find the pose \(\mathbf{T}\) that minimises

$$ e_i = I_1(\mathbf{p}_{1,i}) - I_2\big(\mathbf{p}_{2,i}(\mathbf{T})\big), \qquad \frac{\partial e_i}{\partial \delta\boldsymbol{\xi}} = -\underbrace{\frac{\partial I_2}{\partial \mathbf{u}}}_{\text{image gradient}} \underbrace{\frac{\partial \mathbf{u}}{\partial \mathbf{q}}}_{\text{projection}} \underbrace{\frac{\partial \mathbf{q}}{\partial \delta\boldsymbol{\xi}}}_{\text{pose}} $$

where \(\mathbf{q}\) is the point in the second camera frame and \(\mathbf{u}\) its pixel. Large motions are handled with an image pyramid: solve on a small, blurred image first, then refine on larger ones.

The related Lucas–Kanade optical flow tracks a small patch by solving for its 2D shift \(\mathbf{d}\): \(\mathbf{A}^\top\mathbf{A}\,\mathbf{d} = -\mathbf{A}^\top\mathbf{b}\), with \(\mathbf{A}\) the image gradients and \(\mathbf{b}\) the brightness differences in the patch.

void directPoseEstimation(const cv::Mat& img1, const cv::Mat& img2,
                          const std::vector<Eigen::Vector2d>& px_ref,
                          const std::vector<double>& depth_ref,
                          Sophus::SE3d& T21) {
    double last_cost = 1e30;
    for (int iter = 0; iter < 10; ++iter) {
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
        Eigen::Matrix<double, 6, 1> g = Eigen::Matrix<double, 6, 1>::Zero();
        double cost = 0;
        for (size_t i = 0; i < px_ref.size(); ++i) {
            // project the point into img2, compute error e and Jacobian J (1×6)
            // H += J.transpose() * J;  g += -J.transpose() * e;  cost += e * e;
        }
        Eigen::Matrix<double, 6, 1> dx = H.ldlt().solve(g);
        if (!dx.allFinite() || cost >= last_cost) break;   // diverged or no progress
        T21 = Sophus::SE3d::exp(dx) * T21;
        last_cost = cost;
    }
}

8. The back end: bundle adjustment and pose graphs

Bundle adjustment (BA) refines all camera poses and all 3D points together by minimising the total reprojection error. Ordering the unknowns as poses \(\mathbf{x}_c\) then points \(\mathbf{x}_p\), the system has a special block structure:

$$ \begin{bmatrix} \mathbf{B} & \mathbf{E} \\ \mathbf{E}^\top & \mathbf{C} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{x}_c \\ \Delta\mathbf{x}_p \end{bmatrix} = \begin{bmatrix} \mathbf{v} \\ \mathbf{w} \end{bmatrix} $$

\(\mathbf{C}\) (points) is block-diagonal and therefore cheap to invert. The Schur complement eliminates the points first and leaves a much smaller system for the poses only:

$$ \big(\mathbf{B} - \mathbf{E}\,\mathbf{C}^{-1}\mathbf{E}^\top\big)\,\Delta\mathbf{x}_c = \mathbf{v} - \mathbf{E}\,\mathbf{C}^{-1}\mathbf{w} $$

Then each point is updated on its own. A pose graph goes further: it drops the points altogether and keeps only poses connected by relative-motion constraints, which is fast enough for very large maps.

With g2o:

using BlockSolver  = g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>>;
using LinearSolver = g2o::LinearSolverEigen<BlockSolver::PoseMatrixType>;

auto* algorithm = new g2o::OptimizationAlgorithmLevenberg(
    std::make_unique<BlockSolver>(std::make_unique<LinearSolver>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(algorithm);

for (size_t i = 0; i < poses.size(); ++i) {           // one vertex per pose
    auto* v = new g2o::VertexSE3();
    v->setId(static_cast<int>(i));
    v->setEstimate(poses[i]);
    if (i == 0) v->setFixed(true);                     // anchor the first pose
    optimizer.addVertex(v);
}
for (const auto& c : constraints) {                   // one edge per relative pose
    auto* e = new g2o::EdgeSE3();
    e->setVertex(0, optimizer.vertex(c.from));
    e->setVertex(1, optimizer.vertex(c.to));
    e->setMeasurement(c.transform);
    e->setInformation(c.information);
    optimizer.addEdge(e);
}
optimizer.initializeOptimization();
optimizer.optimize(20);

9. Loop closure

Odometry drifts a little with every step. When the robot comes back to a place it has seen before, recognising it adds one constraint that lets the optimiser spread the accumulated error over the whole path.

Bag of words. Feature descriptors are clustered offline into a vocabulary tree (hierarchical k-means), and each image becomes a histogram of "visual words". Words are weighted by TF-IDF: frequent in this image, rare in general means informative:

$$ \eta_i = \mathrm{TF}_i \times \mathrm{IDF}_i, \qquad \mathrm{TF}_i = \frac{n_i^{\text{image}}}{n^{\text{image}}}, \qquad \mathrm{IDF}_i = \log\frac{N}{N_i} $$

with \(N\) images in the training set, of which \(N_i\) contain word \(i\).

$$ \text{New keyframe} \;\rightarrow\; \text{BoW vector} \;\rightarrow\; \text{Query database} \;\rightarrow\; \text{Check consistency and geometry} \;\rightarrow\; \text{Add loop edge, optimise} $$

Precision matters more than recall. Missing a loop only means a bit more drift. Accepting a wrong loop can fold the whole map. That is why candidates must be confirmed over several frames and by a geometric check before they are used.

With DBoW3:

#include <DBoW3/DBoW3.h>

DBoW3::Vocabulary vocab("orb_vocabulary.yml.gz");   // trained offline
DBoW3::Database db(vocab, false, 0);
for (const cv::Mat& desc : keyframe_descriptors) db.add(desc);

DBoW3::QueryResults results;
db.query(current_descriptors, results, 4);           // 4 best candidates
if (!results.empty() && results[0].Score > 0.05) {
    // confirm with consecutive frames and a geometric check (PnP + RANSAC)
    // then add the loop edge to the pose graph and optimise
}

10. Mapping

Depth filtering. With one camera, the depth of a pixel is estimated many times and fused. If the current estimate is \(\mathcal{N}(\mu, \sigma^2)\) and a new measurement is \(\mathcal{N}(\mu_o, \sigma_o^2)\), the fused estimate is

$$ \mu_\text{fused} = \frac{\sigma_o^2\,\mu + \sigma^2\,\mu_o}{\sigma^2 + \sigma_o^2}, \qquad \sigma_\text{fused}^2 = \frac{\sigma^2\,\sigma_o^2}{\sigma^2 + \sigma_o^2} $$

The fused variance is always smaller than both: each measurement makes us more certain.

Occupancy (OctoMap). Space is split into cubes stored in an octree. Each cube keeps the log-odds \(L = \log\frac{p}{1-p}\) of being occupied, and every new measurement simply adds:

$$ L(n \mid z_{1:t}) = L(n \mid z_{1:t-1}) + L(n \mid z_t) $$

#include <octomap/octomap.h>
#include <pcl/filters/voxel_grid.h>

// Downsample the point cloud to 1 cm
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(cloud);
voxel.setLeafSize(0.01f, 0.01f, 0.01f);
voxel.filter(*filtered);

// Insert into a 5 cm occupancy map
octomap::OcTree tree(0.05);
for (const auto& p : filtered->points)
    tree.updateNode(octomap::point3d(p.x, p.y, p.z), true);
tree.updateInnerOccupancy();
tree.writeBinary("map.bt");

11. Putting it together, and what comes next

A practical SLAM system needs more than algorithms: keyframe selection (keep only frames that add new information), multithreading (tracking, mapping and loop closure in parallel), careful map management, and fusion with other sensors. Visual-inertial odometry (VIO) adds an IMU, which measures fast motions well where the camera blurs, and gives the true scale to a single camera.

Current research directions: semantic SLAM (understanding objects, not just points), dynamic environments with moving people, long-term mapping as places change, multi-robot mapping, and learned components, from deep features to neural scene representations.

class KeyFrame {
public:
    KeyFrame(const cv::Mat& image, double timestamp, const Sophus::SE3d& pose);
    Sophus::SE3d pose() const;
    void addMapPoint(MapPoint* mp, size_t index);
    bool isBad() const { return bad_; }
private:
    Sophus::SE3d pose_;
    std::vector<MapPoint*> map_points_;
    double timestamp_;
    bool bad_ = false;
};

class Map {
public:
    void addKeyFrame(KeyFrame* kf);
    void addMapPoint(MapPoint* mp);
    std::vector<KeyFrame*> keyFrames() const;
private:
    std::set<KeyFrame*> keyframes_;
    std::set<MapPoint*> map_points_;
    mutable std::mutex mutex_;   // tracking and mapping threads share the map
};

Resources

Books: Introduction to Visual SLAM: From Theory to Practice (Gao and Zhang), Multiple View Geometry in Computer Vision (Hartley and Zisserman), Probabilistic Robotics (Thrun, Burgard and Fox), State Estimation for Robotics (Barfoot).