Simultaneous Localization and Mapping

Module: Computer Vision | Difficulty: Premium

Bundle Adjustment

where is the projection function and is a robust cost function.

Reprojection Error

Visual Odometry Pipeline

where is the relative transformation.

| System | Type | Latency | Map Quality | Loop Closure | |--------|------|---------|-------------|--------------| | ORB-SLAM3 | Feature | 30ms | High | Yes | | VINS-Mono | Feature | 25ms | Medium | Yes | | DSO | Direct | 15ms | Medium | No | | DPV-SLAM | Learned | 50ms | High | Yes | | Nice-SLAM | Neural | 100ms | Very High | Yes |

import numpy as np
from scipy.optimize import least_squares

def bundle_adjustment(points_3d, camera_params, observations,
                      point_indices, camera_indices):
    n_points = len(points_3d)
    n_cameras = len(camera_params)

    def residuals(params):
        cam_p = params[:n_cameras * 6].reshape(n_cameras, 6)
        pts = params[n_cameras * 6:].reshape(n_points, 3)
        res = []
        for obs, pt_idx, cam_idx in zip(
                observations, point_indices, camera_indices):
            rvec = cam_p[cam_idx, :3]
            tvec = cam_p[cam_idx, 3:]
            R = rotation_matrix(rvec)
            projected = R @ pts[pt_idx] + tvec
            projected = projected[:2] / projected[2]
            res.extend(obs - projected)
        return np.array(res)

    x0 = np.concatenate([camera_params.ravel(), points_3d.ravel()])
    result = least_squares(residuals, x0, method='lm')
    cam_result = result.x[:n_cameras * 6].reshape(n_cameras, 6)
    pts_result = result.x[n_cameras * 6:].reshape(n_points, 3)
    return cam_result, pts_result

def rotation_matrix(rvec):
    theta = np.linalg.norm(rvec)
    if theta < 1e-10:
        return np.eye(3)
    k = rvec / theta
    K = np.array([[0, -k[2], k[1]],
                  [k[2], 0, -k[0]],
                  [-k[1], k[0], 0]])
    return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * K @ K

Research Insight: Visual SLAM has been revolutionized by learned approaches that replace hand-crafted features with deep features and use neural fields for map representation. Nice-SLAM and DPV-SLAM represent scenes as neural implicit functions, enabling continuous map updates and photorealistic rendering. The key challenge is real-time performance: classical ORB-SLAM3 runs at 30fps while neural SLAM methods are often 10x slower. Hybrid approaches that combine classical geometry with learned features offer the best of both worlds.