SLAM and Mapping

Simultaneous localization and mapping estimates the vehicle’s trajectory and a map of its surroundings from the same measurements, closing loops when a place is revisited so that the correction propagates backward through the whole trajectory. Almost none of it has flown. The exceptions are the ISS free flyers, and even there the map is built on the ground and the onboard problem is reduced to localization against it [1][2].

Why the flown vehicles did not run SLAM

The obstruction is arithmetic, not doctrine. A pose graph grows with time and with the number of vehicles, loop closure candidates grow faster still, and each candidate costs a scan or image registration. On four robots covering 6 km of the Kentucky Underground limestone mine, only 0.5 percent of the loop closure candidates could be computed within the available budget [4].

Mars rover flight computers have never had that budget. The Yutu-2 team states the case directly: given the limited online processing capability, precise navigation localization was performed at the teleoperation center rather than onboard [8]. Perseverance’s Rover Compute Element is a pair of 133 MHz RAD750s, and even the single global map match its successor performs was moved off it onto the helicopter base station’s Snapdragon board [10].

What flew instead was a ground-side photogrammetric pipeline plus a periodic frame reset. Yutu-2’s dead reckoning runs onboard and gives real-time position and attitude, while visual positioning runs at the Beijing teleoperation center in near real time and is more accurate: the two agree to within centimeters to decimeters over the 10 m or less between neighboring waypoints, and disagreed by more than 1 m when the rover slipped badly, with the visual solution taken as correct [8]. The visual solution establishes an observation equation from ASIFT feature points and solves it by bundle adjustment, with an imaging strategy designed around the scale and rotation change between waypoints seen from a 1.5 m camera height: image at raised pitch from the previous site, then select the image at the current site with the smallest angle to it. Digital elevation and orthophoto models at 0.02 m resolution are produced routinely at each waypoint from Navcam and Hazcam imagery, over a 0.03 m descent-image orthophoto model of the landing area [8]. Absolute localization covers the equivalent Mars practice.

The onboard attempts, and what they cost

Maximum likelihood range map matching was implemented onboard the Rocky 7 Mars rover prototype and evaluated against real Sojourner data [6]. A local occupancy map from rover stereo is matched against a global occupancy map built earlier from lander or rover imagery, with orientation assumed known from a gyrocompass, accelerometers or a sun sensor, so only a two-dimensional translation is searched. The search is hierarchical over a discretized pose space with recursive pruning of cells that cannot contain the maximum, and the depth dimension is removed by high-pass filtering rather than searched. Position error was much smaller than a meter, against the roughly 91 m average error of the mountain-bearing localization systems of the period [6]. The published limitation is the input data, not the algorithm: Sojourner stereo range was poor outside the central part of the image because of fish-eye lens miscalibration, and only a handful of positions had enough downlinked data to test.

The same group’s multi-resolution mapping combined surface, descent and orbital imagery into one framework, refining descent camera motion by tracking features under an epipolar constraint with Levenberg-Marquardt, then recovering depth by correlating onto virtual planar surfaces with sum-of-squared-differences and subpixel interpolation [7]. On Silver Lake field data, 8 descent images spanning altitudes of 1086 m down to 8 m, and a localization test using 11 descent images against 14 rover stereo pairs at three rover stations, rover positions came within 1 m of the GPS estimate overall and below 5 cm about 5 m from the center of the descent sequence. Depth recovery is numerically unstable at the epipole, which for pure descent is the image center, because the correlation curves there are flat. Registration of descent imagery to orbital imagery was attempted with mutual information, which failed, and replaced with entropy alignment; the search remains a brute-force sweep over six degrees of freedom of camera pose. All of it ran on the ground.

What flies: Astrobee

Astrobee separates the two halves of SLAM. The sparse feature map of the module interior is built offline from imagery using SURF, which is expensive, and the onboard problem is matching incoming navigation images against it, described in free flyer guidance [1]. AstroLoc2 changed the matching half: DBoW2 selects candidate map images, covisibility graph queries add neighbors of the surviving candidates plus the highest-scoring map image from the previous iteration, and matching uses TEBLID features with 512 bit descriptors instead of BRISK, with CLAHE histogram equalization applied to navigation images to raise the feature count in dim areas of the station [2]. Matches are pruned by a Hamming distance threshold and Lowe’s ratio test, outliers removed by RANSAC essential matrix estimation, and the image registered by RANSAC perspective-n-point.

The ratio test carries a calibration that does not come from a spacecraft. Accepting a match only when the nearest descriptor is closer than 0.8 times the second nearest rejects 90 percent of false matches at a cost of under 5 percent of correct ones, a figure measured against a database of about 40,000 keypoints from 112 handheld and synthetically warped photographs of ordinary indoor and outdoor scenes under random rotation and scale change, up to 30 degrees of viewpoint tilt and 1 to 2 percent added image noise [11]. Correct matching stays above 50 percent only out to about 50 degrees of planar viewpoint tilt, and the descriptor is normalized against affine intensity change only, so a cast shadow or a saturated pixel falls outside what the invariance covers, a limit stated in the original characterization without being quantified. A module interior supplies the near-planar surfaces and the bounded viewpoint change the 0.8 threshold was tuned against; low sun angle regolith and shadowed terrain do not [11].

The number of database images queried is adjusted at runtime against the recent map matching success rate, increasing when the rate falls below a threshold and decreasing when it rises above one, and the keypoint detection threshold is scaled by the same rule with the scale factors driven toward unity as the keypoint count approaches its bounds [2]. The free flyer runs its entire estimator on one processor core, so the number of database images queried is itself a controlled variable [1][2].

Measured over a public dataset of 10 ISS activities, matching against the prebuilt map [2]:

Map matcher	ATE RMSE	Average success rate	Average runtime	Runtime standard deviation
AstroLoc2	0.193 m	0.55	0.708 s	0.173 s
AstroLoc	0.216 m	0.46	0.250 s	0.078 s

Source: [2], Table I. The adaptive matcher spends 2.4 times longer on the harder free-flying sequences than on the testing sequences, against 1.4 times for the fixed matcher, which is the intended behavior: map matching runs in its own process and the localizer holds position by relative odometry between updates, so a slower matcher with a higher success rate is preferred [2], the same division of labor between a fast relative estimator and a slow absolute fix that the Mars rovers use [10].

The cost of full SLAM, measured on the same data

The comparison that explains the architecture is the runtime table, all figures on a 3.4 GHz AMD Ryzen 9 5950X with 32 GB of RAM rather than on flight hardware [2]:

Localizer	Average runtime
AstroLoc2	45.37 ms
AstroLoc	52.37 ms
DSO	77.20 ms
SC-SfMLearner	77.56 ms
DROID-SLAM	136.64 ms
ORB-SLAM3	258.24 ms
DF-VO	540 ms

Source: [2], Table IV. ORB-SLAM3, which performs online bundle adjustment instead of using a prebuilt map, is 5.7 times slower than AstroLoc2 and more accurate by 2.2 percent. Astrobee’s own budget makes that trade one-sided: its single Snapdragon core runs roughly 10 times slower than an Intel i9-9980HK at 2.4 GHz and is shared with the rest of the robot’s software [1]. Adding time-of-flight depth is the same argument at smaller scale. Point-to-point correspondence estimation from time-of-flight intensity images takes about 12 ms, against about 110 ms for ICP on the same data, and the cheaper one gives lower error on two of three activities [2].

Loop closure as the expensive part

On the intravehicular datasets the odometry front end costs tens of milliseconds [2], while the candidate revisits to be tested grow quadratically with trajectory length and with the number of vehicles [4].

Detection at frame rate, on aerial imagery

A visual loop closure detector built for a future Mars Science Helicopter uses DBoW2 over a vocabulary trained on Mars simulation imagery, with candidate frames required to share a minimum number of words, island grouping over temporally adjacent candidates, and a geometric consistency check [3]. The 10^6 word vocabulary was trained on about 30,000 synthetic Mars images covering an 8 km by 8 km area. Two check configurations were compared over 1000 overlapping and 19,000 non-overlapping simulated images: brute-force matching with a five-point essential matrix in RANSAC gave the best recall at 100 percent precision, and direct-index matching with a rotational consistency check came second at sub-millisecond runtime, which was selected because the saved time buys more consistency checks [3].

Evaluated on real Ingenuity data, flights 7 to 19 stitched into one 3276 m trajectory at 7.2 to 13.4 m above ground level, the database held 882 keyframes at the end and overall recall was 41.3 percent with no false positives [3]. Recall was 72.9 percent, 51 of 70 possible closures, in the one region with strong texture where the image pairs were minutes apart, and 31.9 and 30.5 percent in two regions whose pairs were taken about 5 and 7 months apart, one of them after a sand storm. Restricted to pairs meeting the 65 percent image overlap the system needs at fixed altitude, overall recall is 67.9 percent [3]. Substituting the stock ORB-SLAM vocabulary at the same 10^6 word size produced histograms with 21 percent fewer distinct words and generated a false positive [3]. The Ingenuity figures are the only loop-closure recall numbers measured on imagery returned from another body [1].

Runtime per incoming navigation image averaged 24.3 ms on the simulated set and 19.4 ms on the Ingenuity set, the difference being that Martian terrain has less texture, both on a single thread of an AMD Ryzen 5 2600 [3]. The 30 Hz camera frame rate allows 33.3 ms. The target processor is a Snapdragon 820, within the same order of magnitude in single-thread rating, from which the authors project frame-rate operation. No measurement on the Snapdragon itself is published.

Prioritizing which closures to compute

LAMP’s centralized multi-robot back end attaches a keyed lidar scan to each pose graph node and generates loop closure candidates from nodes within a Euclidean distance that scales with accumulated uncertainty [4]. Three prioritization modules then choose which candidates to register: a graph neural network predicting the uncertainty reduction from adding each closure, an observability score over the candidate scans, and a proximity heuristic, combined by round-robin so each module contributes equally. Feature-poor areas are dropped as candidates, which is what saves the computation.

Evaluated on four Team CoSTAR datasets, two Huskies over 2.5 km combined in a coal mine, two Huskies and a Spot over 1.5 km in an abandoned nuclear power plant, three Spots and a Husky over 1.2 km on the DARPA Subterranean Challenge Finals course, and four Huskies over 6 km in a limestone mine, prioritization gave a 51 percent mean reduction in median error against odometry alone and 75 percent against the unprioritized baseline [4]. Experiments ran both on a laptop with a 12-core Intel i7-8750H and on a 64-core AMD Ryzen Threadripper 3990X.

Beacons instead of scan matching

Dropping radio beacons converts loop detection from a search into a measurement. Range-aided loop closure uses ultra-wideband range measurements from beacons dropped by the robot to nominate candidates, then computes the relative transform geometrically, so the expensive registration runs only on candidates the radio already agrees about [5]. On a Husky A200 with a VLP-16 lidar and an Intel NUC 7i7DNBE at 4 by 1.9 GHz with 32 GB of RAM, carrying Decawave MDEK1001 UWB beacons with centimeter-order two-way ranging, run on data from the DARPA Subterranean Challenge Urban Circuit:

Method	Map error, mean	Map error, sigma	Odometry error, mean	Loop closure compute time	Candidates	Accepted
No loop closure	0.661 m	0.847 m	4.34 m	none	none	none
Range factor only	1.15 m	1.56 m	7.21 m	not reported	not reported	not reported
Range-aided loop closure	0.434 m	0.458 m	3.18 m	16.1 s	48	12, 25.0 percent
Geometric lidar only	0.456 m	0.444 m	3.04 m	339.5 s	1145	35, 3.1 percent
Both in parallel	0.489 m	0.487 m	3.29 m	not reported	not reported	not reported

Source: [5], Table I and Figure 12. The range-aided method matches geometric-only accuracy at under 5 percent of its scan matching cost and an eight-fold lower outlier rate. The parallel combination was the configuration run live during the DARPA event. Range factors used alone, without geometric verification, are worse than no loop closure at all, because sparsely deployed beacons leave a binary ambiguity that multilateration cannot resolve when the trajectory near the beacon is close to straight.

Multi-vehicle mapping

CADRE is the first flight program to build and merge maps across several rovers onboard, using a distributed database, a multi-resolution traversability pyramid and pose graph optimization over visual-inertial odometry and ultra-wideband ranging [9]. Three rovers and a base station share state, pose graph nodes and edges, and local traversability maps through an SQLite database over Microhard mesh radios at 1 Mbps aggregate, on a Qualcomm Snapdragon 821 with 4 GB of RAM and 32 GB of flash per rover. Pose optimization runs on the leader rather than on each rover, so the system is a data fusion layer rather than distributed SLAM. That work is covered under multi-robot coordination.