From Visual Odometry to SLAM, Measured Against Motion Capture

Two weeks of adding loop closure, a pose graph and relocalisation to a monocular C++ pipeline, scored against a public benchmark’s ground truth. One feature cut the error by two thirds. Two others were built, measured, and not claimed.
Author

Benedict Thekkel

Published

October 2, 2026

The previous post was about getting ros2_pi to run end to end: one webcam, ORB + PnP odometry, neural depth on CUDA, TSDF fusion into a live mesh. That is visual odometry. Every pose is estimated relative to recent frames, so error accumulates and nothing ever corrects it. Walk around a room and come back to the start, and the map does not close.

SLAM is the step that fixes that: recognise a place you have been before, and use it to correct everything in between. This post covers that step, from 2026-09-25 to 2026-10-02, and the rule I held to throughout: a feature is claimed only when the measurement separates it from the baseline.

First, a number to beat

“The mesh looks better” is not a measurement. So before adding anything, the pipeline was pointed at the TUM RGB-D fr1/desk sequence: 613 frames of a handheld camera over a desk, with ground truth from a 100 Hz motion-capture system. evo_ape aligns the estimated trajectory to ground truth and reports the absolute trajectory error (ATE).

Monocular depth has no absolute scale, so the alignment is Sim(3): rotation, translation and scale. Baseline visual odometry over seven runs: ATE 0.27 to 0.36 m. A rotation-only control, which ignores translation, cannot be aligned at all, which is a useful confirmation that the metric discriminates.

Building the benchmark found a bug before it measured anything. Rotating TUM’s own ground truth into the camera’s optical frame and scoring it against itself gave a relative error of 0.654 m. A trajectory compared with itself should score zero. Body frame versus optical frame: the first real run read 0.768 m in the wrong frame and 0.150 m in the right one. Every number after this depends on that being right.

Two features that were built and not claimed

A local map. Instead of tracking against the last keyframe only, track against a map of recent landmarks. Standard practice, and it should help. Measured: 0.46 to 0.50 m, worse than the 0.28 to 0.46 m baseline. The diagnosis was that Depth Anything’s scale differs by 15 to 21% between keyframes, so landmarks triangulated from one keyframe’s depth do not agree with the next one’s. A local map built on inconsistent scale averages disagreement.

Local bundle adjustment with g2o. The median looked good: 0.259 m against 0.429 m without it. But at three runs each, BA’s worst run (0.418) overlapped no-BA’s best (0.318). Worse, the baseline with unchanged code ranged 0.28 to 0.46 m one day and 0.44 to 0.47 m the next. The run-to-run spread was bigger than the effect.

Both are in the code, both default off, and neither is claimed. A median that a re-run could reverse is not a result. It is a reason to run more, and the issue stays open until the distributions separate.

Place recognition, without a vocabulary

The usual tool is a bag-of-words vocabulary such as DBoW2. It is not packaged for either ROS 2 distribution this runs on, so the first version is brute force, run on a low-priority thread:

  1. Exclude the obvious. Skip keyframes younger than 3 seconds, or sharing more than 5 tracked features with the query. Those are not revisits, they are the recent past.
  2. Match descriptors by Hamming distance with a 0.8 ratio test, and keep the top 3 candidate keyframes.
  3. RANSAC a relative pose, requiring at least 12 inliers.
  4. Guided matching: project the candidate’s landmarks into the query with that pose and search within 8 pixels. Accept at 30 or more.

Step 4 is what makes it trustworthy. On a real revisit, blind matching found 15 to 18 correspondences, which is in the range a false match can also reach. Guided matching found 127 to 134. The separation between a true revisit and a lookalike is in the geometry, not the descriptors.

Scored against motion capture: zero wrong-place closures across the evaluation runs. A wrong loop closure is far worse than a missed one, because the pose graph will faithfully bend the whole map to agree with it, so this is the number that matters most.

The pose graph

One vertex per keyframe, as an SE(3) pose. Two kinds of edge:

Edge Uncertainty Source
Odometry 7 deg/s, 0.15 m/s The tracker’s measured drift rate
Loop closure 3 deg, 0.06 m, Huber kernel The guided-match pose

The odometry sigmas are measured, not tuned. That matters because the optimiser’s whole job is to decide how much to trust each edge, and it can only do that if the numbers mean something. The Huber kernel limits the damage any single bad closure can do.

The graph is re-solved after each closure, in 0.7 to 0.9 ms. The correction is published the ROS way: the tracker keeps publishing odom -> base_link, and a real, moving map -> odom transform carries the correction, with the same timestamp. Before this, map -> odom was a static identity.

Keyframe ATE: 0.118 to 0.153 m with loop closure, against 0.361 to 0.533 m without. Three runs each, and the two ranges do not overlap: there is a 0.21 m gap between the worst run with it and the best run without. That is the one claimed in this post.

Using the correction, and knowing when you are lost

A corrected trajectory is only half the point if the mesh was built at the uncorrected poses. So the TSDF is rebuilt at the corrected keyframe poses. Against a reference rebuilt at motion-capture poses, the surface distance dropped from 0.66 to 0.90 m with odometry alone to 0.51 to 0.53 m corrected.

Then failure. Cover the lens, and a tracker that does not know it is lost will keep fusing garbage into the map. There is now an explicit LOST state, entered within 5 depth frames, during which nothing is fused. The control run, with the state disabled, integrated 514 to 622 million voxel updates of nonsense.

And recovery: from a saved map, the pipeline relocalises after 2 LOST frames to 0.18 m median, 0.25 m worst. The control is a recording from a different room: 819 relocalisation queries, zero accepted. The first time this gate passed, it passed over nothing: the test sequence was already LOST before the lens cap went on, so there was nothing to recover from. A pass with nothing to recover from proves nothing, and that run does not count.

What this is, and is not

Honest scope, as the repo states it:

  • Keyframe ATE, not online ATE. The number above is the graph’s final keyframe poses. The per-frame trajectory the TSDF actually integrates at, with the correction as it stood at each moment, is not yet scored. Until it is, loop closure ships off by default.
  • One benchmark sequence. fr1/desk is short and well textured.
  • Monocular depth is relative. Scale breathes by about 4%, anything past about 6 m is clipped, and a blank wall defeats it entirely.
  • The pose graph cannot catch a large false closure on its own. A 5 m false loop in the unit tests goes straight through. The defence is upstream, in step 4 of place recognition.
  • The mesh is a good room. It is not a survey.

Takeaway

Feature Result Claimed?
Local map Worse than baseline (scale disagreement) No
Local bundle adjustment Better median, overlapping runs No
Place recognition 0 wrong closures Yes
Pose graph ATE 0.12 to 0.15 m vs 0.36 to 0.53 m Yes
LOST + relocalisation 0.18 m median, 0 false accepts Yes

Two things I would carry to any perception project. Get a ground-truth benchmark before the feature, because without one, everything that makes the mesh look nicer looks like progress. And report the spread, not the median. The bundle adjustment number was a better median and a non-result, and the only thing that showed it was running the baseline twice.

Back to top