Visual SLAM

The EKF SLAM section used the camera only to measure landmarks, and the robot’s motion came from its wheels. A visual SLAM system estimates the camera’s motion from the images themselves. It tracks points from frame to frame, recovers how the camera moved between frames, triangulates the points into a map, and fixes the accumulated drift when the camera returns to a place it has seen before. StellaVSLAM, which you run in the Visual SLAM assignment, is organized this way. This section builds each of those steps on a synthetic scene. Every pixel, pose and point has a known true value, so each source of error can be measured on its own. Real images add the work of detecting and matching features, which the assignment covers.

Features in visual SLAM and VIO

Visual SLAM and visual-inertial odometry (VIO) work with features too, but each part of the range, bearing and signature triple of the landmark model changes. A monocular camera does not measure range. A keypoint at pixel

(u, v)

fixes only the ray from the camera through the landmark. Multiplying by the inverse intrinsics,

K^{-1}(u, v, 1)^\top

, turns the pixel into a direction in 3D, a bearing in both azimuth and elevation. The landmarks are 3D points

p_j

, and the measurement model is the pinhole projection of the point into a camera with rotation

R

and position

t

z^i_t = \begin{bmatrix} u^i_t \\ v^i_t \end{bmatrix} = \pi\!\left(K R^\top (p_j - t)\right) + \varepsilon, \qquad \pi(a, b, c) = \left(\frac{a}{c}, \frac{b}{c}\right).

The division by depth in

\pi

is why one view gives no range. The residual

z^i_t - \hat z^i_t

is the reprojection error, in pixels. A stereo pair measures the disparity between its two images, which gives depth, and an RGB-D camera measures depth at each pixel. With a single camera, a new landmark is triangulated from two or more views taken far enough apart, and the map is known only up to scale. VIO adds an IMU, whose accelerometer fixes the metric scale and the direction of gravity. The signature becomes a descriptor computed from the image patch around the keypoint, such as a 256-bit ORB descriptor or a 128-dimensional SIFT descriptor. A descriptor resembles its landmark but does not name it. Two corners can look alike, and one corner looks different from another viewpoint. The correspondences are therefore unknown. They come from matching descriptors, or from tracking patches from frame to frame, and the wrong matches are removed geometrically, for example with RANSAC on the essential matrix. The rest of this section builds this pipeline. A frame also has hundreds of keypoints rather than a few landmarks, which changes how the estimators are built. Filter-based VIO such as the MSCKF keeps a window of past camera poses in its state but not the points. Each point is triangulated when its track ends and then constrains only the poses. Optimization-based systems such as ORB-SLAM adjust the keyframe poses and the points together to minimize the reprojection error, which is bundle adjustment. VINS-Mono does the same over a sliding window and adds IMU terms between keyframes.

	Range-bearing landmark model	Visual SLAM and VIO
Measurement	range and bearing $(r, \phi)$	pixel $(u, v)$ ; depth only from stereo or RGB-D
Landmark	2D point	3D point, triangulated; metric scale from stereo or an IMU
Signature	color, size, type or a marker ID	descriptor that resembles the landmark but does not name it
Correspondence	from the signature, or estimated	matched or tracked, then checked geometrically
Wrong correspondences	occasional	routine, removed with RANSAC or robust costs
Features per step	a few	hundreds

The scene

The camera moves around a circle of radius 1.5 m in a 6 m by 6 m room, 0.5 m above the floor and looking along its direction of travel. It records 60 keyframes on the way, about 16 cm apart, and a 61st that repeats the pose of the first, so the loop closes. The room holds 1500 points: half on the walls and half on furniture spread through the room, kept at least 0.5 m away from the path. The camera has the nominal intrinsics of the real D435i color camera from the course’s camera configuration,

f_x = f_y = 617

px and principal point (321, 240) in a 640 by 480 image, and the same optical frame as the VIO section:

z

forward,

x

right,

y

down. Each keyframe records the pixels of the points in its view with one pixel of noise. Matching is by point index, the stand-in for a feature descriptor. To imitate wrong descriptor matches, 5% of each keyframe’s pixels are replaced by a random pixel, so about one match in ten between two keyframes is wrong.

import cv2
import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation as Rot

FX = FY = 617.0
CX, CY, WIDTH, HEIGHT = 321.0, 240.0, 640, 480
K = np.array([[FX, 0, CX], [0, FY, CY], [0, 0, 1.0]])
SIGMA_PX = 1.0
N_KF = 60  # keyframes around the loop


def room_points(rng, n=1500, half=3.0, height=2.5):
    """Half the points on the four walls of a 6 m by 6 m room, half on furniture inside it."""
    n_wall = n // 2
    side = rng.integers(0, 4, n_wall)
    s_ = rng.uniform(-half, half, n_wall)
    z = rng.uniform(0.0, height, n_wall)
    x = np.select([side == 0, side == 1, side == 2], [np.full(n_wall, half), np.full(n_wall, -half), s_], s_)
    y = np.select([side == 0, side == 1, side == 2], [s_, s_, np.full(n_wall, half)], np.full(n_wall, -half))
    walls = np.stack([x, y, z], 1)
    inside = []
    while len(inside) < n - n_wall:  # furniture: anywhere in the room except within 0.5 m of the 1.5 m path
        q = np.r_[rng.uniform(-half, half, 2), rng.uniform(0.0, 1.8)]
        if abs(np.hypot(*q[:2]) - 1.5) > 0.5:
            inside.append(q)
    return np.vstack([walls, inside])


def camera_loop(radius=1.5, height=0.5, n=N_KF):
    """Camera-to-world poses (R_wc, t_wc) on a closed circle; the camera looks along the direction of travel.

    The list has n + 1 entries and the last pose repeats the first, so the loop is closed.
    """
    poses = []
    for k in range(n + 1):
        phi = 2 * np.pi * k / n
        heading = phi + np.pi / 2
        fwd = np.array([np.cos(heading), np.sin(heading), 0.0])
        right = np.array([np.sin(heading), -np.cos(heading), 0.0])
        down = np.array([0.0, 0.0, -1.0])
        R_wc = np.stack([right, down, fwd], 1)  # optical frame: x right, y down, z forward
        poses.append((R_wc, np.array([radius * np.cos(phi), radius * np.sin(phi), height])))
    return poses


def project(R_wc, t_wc, X):
    """Pixels of world points X (n x 3) and their depths."""
    Xc = (X - t_wc) @ R_wc
    return Xc[:, :2] / Xc[:, 2:] * [FX, FY] + [CX, CY], Xc[:, 2]


def observe(pose, pts, rng, outlier_frac=0.05):
    """Noisy pixels of the visible points, keyed by point index; a fraction are replaced by wrong pixels."""
    uv, depth = project(*pose, pts)
    vis = (depth > 0.3) & (uv[:, 0] >= 0) & (uv[:, 0] < WIDTH) & (uv[:, 1] >= 0) & (uv[:, 1] < HEIGHT)
    idx = np.flatnonzero(vis)
    obs = uv[idx] + rng.normal(0, SIGMA_PX, (len(idx), 2))
    bad = rng.random(len(idx)) < outlier_frac  # wrong matches: a random pixel instead of the true one
    obs[bad] = rng.uniform([0, 0], [WIDTH, HEIGHT], (bad.sum(), 2))
    return dict(zip(idx.tolist(), obs))


def relative_truth(pose_a, pose_b):
    """True motion from camera a to camera b: x_b = R x_a + t."""
    (Ra, ta), (Rb, tb) = pose_a, pose_b
    return Rb.T @ Ra, Rb.T @ (ta - tb)

rng = np.random.default_rng(0)
pts = room_points(rng)
truth = camera_loop()
obs = [observe(p, pts, rng) for p in truth]
P_true = np.array([t for _, t in truth])  # true camera positions, for evaluation only
print(f"{len(pts)} points, {len(truth) - 1} keyframes; each keyframe sees {np.mean([len(o) for o in obs]):.0f} points on average")

1500 points, 60 keyframes; each keyframe sees 157 points on average

Motion between two views

Two views of the same points satisfy the epipolar constraint from the stereo section. With calibrated cameras it involves the essential matrix,

\hat x_b^\top E\, \hat x_a = 0, \qquad E = [t]_\times R,

where

\hat x_a = K^{-1}(u_a, v_a, 1)^\top

are normalized image coordinates and

x_b = R x_a + t

moves points from camera

a

to camera

b

. Five matches determine

E

. A wrong match ruins any estimate it takes part in, so the estimate is wrapped in RANSAC, as in the homographies section. RANSAC repeatedly fits

E

to five random matches, counts the matches within 3 pixels of their epipolar line, and keeps the fit with the most.

E

yields four candidate motions, from two rotations and two signs of

t

. Only one of them puts the triangulated points in front of both cameras, and recoverPose picks that one. The translation comes out with unit length: the images do not contain the length of the baseline, because the same pictures result from a scene twice as large viewed by a camera that moved twice as far. With

R

and

t

fixed, each inlier match is triangulated into a 3D point in camera

a

‘s frame.

def relative_pose(obs_a, obs_b):
    """Essential matrix with RANSAC, then the rotation and unit translation from camera a to camera b."""
    common = sorted(set(obs_a) & set(obs_b))
    pa = np.array([obs_a[i] for i in common])
    pb = np.array([obs_b[i] for i in common])
    E, mask = cv2.findEssentialMat(pa, pb, K, method=cv2.RANSAC, prob=0.999, threshold=3.0)
    # recoverPose keeps the solution with points in front of both cameras and also drops the RANSAC
    # inliers that triangulate behind a camera; its mask defines the inliers
    _, R, t, pose_mask = cv2.recoverPose(E, pa, pb, K, mask=mask.copy())
    inliers = [i for i, m in zip(common, pose_mask.ravel()) if m]
    return R, t.ravel(), inliers


def triangulate(obs_a, obs_b, R, t, ids):
    """3D points in camera a's frame from two views related by x_b = R x_a + t."""
    P_a = K @ np.hstack([np.eye(3), np.zeros((3, 1))])
    P_b = K @ np.hstack([R, t.reshape(3, 1)])
    pa = np.array([obs_a[i] for i in ids]).T
    pb = np.array([obs_b[i] for i in ids]).T
    Xh = cv2.triangulatePoints(P_a, P_b, pa, pb)
    return dict(zip(ids, (Xh[:3] / Xh[3]).T))

R01, t01, inl01 = relative_pose(obs[0], obs[1])
R_true, t_true = relative_truth(truth[0], truth[1])
common = sorted(set(obs[0]) & set(obs[1]))
rot_err = np.degrees(np.arccos(np.clip((np.trace(R01.T @ R_true) - 1) / 2, -1, 1)))
print(f"keyframes 0 and 1 share {len(common)} points; {len(inl01)} pass RANSAC and lie in front of both cameras")
print(f"rotation error {rot_err:.2f} deg; estimated translation direction {t01.round(3)}, "
      f"true {(t_true / np.linalg.norm(t_true)).round(3)}")
print(f"true baseline {np.linalg.norm(t_true):.3f} m; the estimate has length {np.linalg.norm(t01):.0f} by construction")

keyframes 0 and 1 share 113 points; 97 pass RANSAC and lie in front of both cameras
rotation error 0.51 deg; estimated translation direction [-0.11   0.064 -0.992], true [-0.052  0.    -0.999]
true baseline 0.157 m; the estimate has length 1 by construction

Each line joins a point’s pixel in keyframe 0 to its pixel in keyframe 1. Most inlier lines run sideways by a similar amount: between the two keyframes the camera turns by 6 degrees, which shifts the whole image, and the forward motion adds a smaller spread away from the image center. The long lines are the wrong matches. RANSAC rejects most of them, while a wrong match that happens to lie near its epipolar line passes. The estimated rotation is half a degree off. Over a 16 cm baseline with one pixel of noise the translation direction is the least precise part of the estimate, about 5 degrees off here.

Visual odometry

Chaining the two-view estimates gives visual odometry. The first pair’s baseline defines the unit of length. Every later pair also returns a unit translation, so its length is rescaled to keep the map consistent: points seen by the pair before and by the current pair must come out at the same distance from the shared camera, and the median ratio of the two distances sets the scale. The function ate measures the result. It finds the similarity, a rotation, a translation and a scale, that best aligns the estimated camera positions with the true ones, and reports the remaining root-mean-square distance, the absolute trajectory error.

def visual_odometry(obs):
    """Chain two-view estimates into camera-to-world poses; the first baseline sets the unit of length."""
    R_wc, t_wc = np.eye(3), np.zeros(3)
    poses, inlier_ratios, inlier_sets = [(R_wc, t_wc)], [], []
    prev = None  # points of the previous pair, in the frame of the current camera
    for k in range(len(obs) - 1):
        R, t, inl = relative_pose(obs[k], obs[k + 1])
        inlier_ratios.append(len(inl) / len(set(obs[k]) & set(obs[k + 1])))
        inlier_sets.append(set(inl))
        X_a = triangulate(obs[k], obs[k + 1], R, t, inl)  # in camera k, with |t| = 1
        if prev is not None:  # fix the scale so that shared points have the same depth as before
            shared = [i for i in X_a if i in prev and X_a[i][2] > 0 and prev[i][2] > 0]
            scale = np.median([np.linalg.norm(prev[i]) / np.linalg.norm(X_a[i]) for i in shared])
        else:
            scale = 1.0
        t = t * scale
        prev = {i: R @ (X * scale) + t for i, X in X_a.items()}  # same points, now in camera k + 1
        R_wc_next = R_wc @ R.T
        t_wc = t_wc - R_wc_next @ t
        R_wc = R_wc_next
        poses.append((R_wc, t_wc))
    return poses, np.array(inlier_ratios), inlier_sets


def umeyama(X, Y):
    """Similarity (s, R, t) minimizing sum |Y - (s R X + t)|^2."""
    mx, my = X.mean(0), Y.mean(0)
    Xc, Yc = X - mx, Y - my
    U, D, Vt = np.linalg.svd(Yc.T @ Xc / len(X))
    S = np.eye(3)
    if np.linalg.det(U) * np.linalg.det(Vt) < 0:
        S[2, 2] = -1
    R = U @ S @ Vt
    s = np.trace(np.diag(D) @ S) / Xc.var(0).sum()
    return s, R, my - s * R @ mx


def ate(est_positions, true_positions):
    """Absolute trajectory error after the best similarity alignment, and the scale of that alignment."""
    s, R, t = umeyama(est_positions, true_positions)
    aligned = s * est_positions @ R.T + t
    return np.sqrt(np.mean(np.sum((aligned - true_positions) ** 2, 1))), s, aligned

vo, ratios, inlier_sets = visual_odometry(obs)
P_vo = np.array([t for _, t in vo])
ate_vo, s_vo, P_vo_aligned = ate(P_vo, P_true)
print(f"mean inlier ratio over the {len(ratios)} pairs: {ratios.mean():.2f}")
print(f"the chained trajectory is {1 / s_vo:.1f} times larger than the true one")
print(f"after the best similarity alignment: trajectory error {ate_vo:.3f} m, "
      f"gap between the loop's start and end {np.linalg.norm(P_vo_aligned[-1] - P_vo_aligned[0]):.3f} m")

mean inlier ratio over the 60 pairs: 0.84
the chained trajectory is 7.5 times larger than the true one
after the best similarity alignment: trajectory error 0.129 m, gap between the loop's start and end 0.492 m

RANSAC, followed by the check that each point lies in front of both cameras, keeps 84% of the matches on average, while 90% of the matches are correct. The chained trajectory comes out 7.5 times larger than the true one. Most of that factor is the choice of unit: the first baseline is 0.157 m, which alone would give a factor of 6.4. The rest is scale drift accumulated along the loop. A single camera cannot measure that factor. The D435i’s depth or its IMU can, which is why turtlebot-maze can run StellaVSLAM in RGB-D mode with the camera’s depth images. The scale also drifts. Each rescaling uses a few hundred noisy depths, and every later step inherits its error. After the best alignment the trajectory error is 0.129 m, and the end of the loop misses its start by 0.492 m.

Loop closure

At the last keyframe the camera is back at its starting pose. A real system recognizes this from the images, typically by comparing a bag-of-words summary of each keyframe’s descriptors. Here the shared point indices reveal it directly: most points in view were already mapped near the start. The function below relocalizes the last keyframe against that early map. Given known 3D points and their pixels, the camera pose follows from Perspective-n-Point (PnP), again wrapped in RANSAC. The same points, triangulated by the last pair of the chain, give the scale drift. Their distances from the camera in the chain’s units, compared with their distances in the units of the start map, show how far the unit of length has drifted around the loop.

def sim3_compose(a, b):
    """Compose similarities (s, R, t) acting as x -> s R x + t."""
    return (a[0] * b[0], a[1] @ b[1], a[0] * a[1] @ b[2] + a[2])


def sim3_inv(a):
    s, R, t = a
    return (1 / s, R.T, -R.T @ t / s)


def local_points(obs, poses, k, inliers):
    """Points triangulated by the pair (k, k + 1), in world coordinates of the pose chain."""
    (Ra, ta), (Rb, tb) = poses[k], poses[k + 1]
    R, t = Rb.T @ Ra, Rb.T @ (ta - tb)
    X_a = triangulate(obs[k], obs[k + 1], R, t, sorted(inliers))
    return {i: Ra @ X + ta for i, X in X_a.items() if X[2] > 0}


def close_loop(obs, poses, inlier_sets, k_loop):
    """Relocalize keyframe k_loop against the map built at the start, and measure the scale drift.

    Returns the loop edge as the similarity from camera 0 to camera k_loop.
    """
    early = {}
    for k in range(3):  # the map near the start of the loop
        early.update(local_points(obs, poses, k, inlier_sets[k]))
    ids = [i for i in obs[k_loop] if i in early]
    X = np.array([early[i] for i in ids])
    uv = np.array([obs[k_loop][i] for i in ids])
    ok, rvec, tvec, pnp_inl = cv2.solvePnPRansac(X, uv, K, None, reprojectionError=2.0)
    R_cw = cv2.Rodrigues(rvec)[0]
    R_wc, t_wc = R_cw.T, -R_cw.T @ tvec.ravel()  # where the start map says camera k_loop is
    # scale drift: the same points measured by the last pair of the chain
    late = local_points(obs, poses, k_loop - 1, inlier_sets[k_loop - 1])
    R_d, t_d = poses[k_loop]
    shared = [i for i in late if i in early]
    d_map = [np.linalg.norm(R_cw @ early[i] + tvec.ravel()) for i in shared]
    d_chain = [np.linalg.norm(R_d.T @ (late[i] - t_d)) for i in shared]
    s_loop = np.median(np.array(d_map) / np.array(d_chain))
    return (s_loop, R_wc, t_wc), len(ids), int(len(pnp_inl))

k_loop = len(obs) - 1
loop_edge, n_loop, n_loop_inl = close_loop(obs, vo, inlier_sets, k_loop)
print(f"keyframe {k_loop} sees {n_loop} points of the start map; PnP keeps {n_loop_inl}")
print(f"scale drift: a distance measured at the end of the loop comes out {1 / loop_edge[0]:.2f} times as large as at the start")

keyframe 60 sees 107 points of the start map; PnP keeps 37
scale drift: a distance measured at the end of the loop comes out 1.27 times as large as at the start

Only 37 of the 107 points pass PnP’s 2-pixel test. The start map was triangulated from pairs 16 cm apart, so many of its points, especially the distant ones, have poor depth. A real system refines the map with bundle adjustment: it adjusts the camera poses and the 3D points together to minimize the total reprojection error over every keyframe that sees each point. Chapter 44 of Torralba, Isola and Freeman’s Foundations of Computer Vision develops it.

Pose graph optimization

The loop measurement disagrees with the chain, and the error has to be spread over all the keyframes instead of being dumped on the last one. A pose graph does this. Each keyframe is a node with a pose and a scale, a similarity transform in Sim(3). Each consecutive pair of keyframes is joined by an edge that holds their relative motion from visual odometry, at unchanged scale. The loop closure adds one more edge, from keyframe 0 to the last keyframe, carrying the PnP pose and the measured scale drift. The optimizer moves the nodes to minimize the total disagreement with all the edges, keeping keyframe 0 fixed. Each odometry edge gives way a little, and the loop edge is satisfied. Sim(3) rather than the rigid motions of SE(3) is the choice for a single camera: the scale drift is a real error in the chain, and only a node with its own scale can absorb it.

def pose_graph(poses, loop_edge, k_loop):
    """Sim(3) pose graph: chain edges from visual odometry plus one loop edge; keyframe 0 is fixed."""
    S = [(1.0, R, t) for R, t in poses]
    edges = [(k, k + 1, sim3_compose(sim3_inv(S[k]), S[k + 1])) for k in range(len(S) - 1)]
    edges.append((0, k_loop, sim3_compose(sim3_inv(S[0]), loop_edge)))
    n = len(S) - 1

    def unpack(x):
        out = [S[0]]
        for k in range(n):
            v = x[7 * k:7 * k + 7]
            out.append((np.exp(v[6]), Rot.from_rotvec(v[:3]).as_matrix(), v[3:6]))
        return out

    def residuals(x):
        Sx = unpack(x)
        r = []
        for i, j, Z in edges:
            e = sim3_compose(sim3_inv(Z), sim3_compose(sim3_inv(Sx[i]), Sx[j]))
            r.append(np.r_[Rot.from_matrix(e[1]).as_rotvec(), e[2], np.log(e[0])])
        return np.concatenate(r)

    x0 = np.concatenate([np.r_[Rot.from_matrix(R).as_rotvec(), t, 0.0] for _, R, t in S[1:]])
    sol = least_squares(residuals, x0)
    return [(R, t) for _, R, t in unpack(sol.x)]

pg = pose_graph(vo, loop_edge, k_loop)
P_pg = np.array([t for _, t in pg])
ate_pg, s_pg, P_pg_aligned = ate(P_pg, P_true)
print(f"after the pose graph: trajectory error {ate_pg:.3f} m, "
      f"gap between the loop's start and end {np.linalg.norm(P_pg_aligned[-1] - P_pg_aligned[0]):.3f} m")

after the pose graph: trajectory error 0.039 m, gap between the loop's start and end 0.029 m

The left plot is the chained trajectory in the coordinates of the first camera, measured in units of the first baseline. It has the right shape at the wrong size, and it does not close. The right plot aligns each estimate to the truth. The pose graph pulls the loop shut, from a 0.492 m gap to 0.029 m, and the trajectory error drops from 0.129 m to 0.039 m.

The same steps in StellaVSLAM

StellaVSLAM, the maintained fork of OpenVSLAM, splits the work into three modules (Sumikura et al., 2019):

This section	StellaVSLAM module	What the module does
Two-view geometry, matching by index	Tracking	Extracts ORB features from every frame, matches them to the local map, optimizes the camera pose and decides when a frame becomes a keyframe. The two-view geometry of this section is used only to initialize the map; after that the pose comes from matching against map points, as in the PnP step below
Triangulation	Mapping	Triangulates new 3D points from keyframes and refines the nearby keyframes and points with local bundle adjustment
Loop detection, PnP, Sim(3) pose graph	Global optimization	Detects loops, runs pose graph optimization and global bundle adjustment, which removes trajectory drift and, for a monocular camera, scale drift

In the course’s turtlebot-maze repository, slam/slam_bridge.py takes the simulated D435i’s color images, and in RGB-D mode its depth images, and streams them to slam/run_slam.cc, which feeds them to StellaVSLAM and returns the pose of every frame. The assignment’s Task 1 builds the front end of this section on real KITTI images, where finding and matching the features is the hard part. Task 2 runs StellaVSLAM on video of your own room. Key references: (Sumikura et al., 2019; Corke, 2023)

References

Corke, P. (2023). Robotics, vision and control: Fundamental algorithms in python.
Sumikura, S., Shibuya, M., Sakurada, K. (2019). OpenVSLAM: A versatile visual SLAM framework. arXiv [cs.CV].

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Features in visual SLAM and VIO

The scene

Motion between two views

Visual odometry

Loop closure

Pose graph optimization

The same steps in StellaVSLAM

References

​Features in visual SLAM and VIO

​The scene

​Motion between two views

​Visual odometry

​Loop closure

​Pose graph optimization

​The same steps in StellaVSLAM

​References

Features in visual SLAM and VIO

The scene

Motion between two views

Visual odometry

Loop closure

Pose graph optimization

The same steps in StellaVSLAM

References