Lifelong Mapping on Stretch4 with Rtabmap

We wanted Stretch 4 to navigate dynamic indoor environments autonomously, using a map that evolves as spaces change. In real environments, static maps go stale quickly, while uncontrolled real-time updates risk catastrophic map corruption if localization drifts.

To solve this, we rely on a modular pipeline:

• GLIM fuses lidar and IMU data for smooth, robust odometry.
• RTAB-Map handles global mapping, lifelong map updates and loop closure, using both lidar and vision.
• Nav2 executes path planning and control.
• Supervisory logic monitors localization confidence and handles recovery.

The entire pipeline runs on Stretch 4’s onboard Intel NUC, with optional offloading of heavy compute to a companion Jetson. Below, we break down key architectural decisions and performance across both hardware setups.

The code is open source: github.com/hello-robot/stretch4_rtabmap

Why GLIM and RTAB-Map together

RTAB-Map is a mature back end for lifelong navigation: it maintains a persistent multi-session graph, detects visual and geometric loop closures, and publishes the 2D occupancy grid Nav2 plans on. However, its built-in visual and ICP odometry rely on sequential matching without tightly coupled IMU preintegration, making them prone to slipping during quick turns or across floor thresholds.

GLIM solves that front-end limitation by tightly coupling lidar scans with high-rate IMU preintegration in a continuous factor graph. It provides smooth, drift-resilient local odometry, but lacks the higher-level infrastructure needed for navigation—specifically visual place recognition, persistent multi-session databases, and 2D costmap integration.

We combine them by cleanly decoupling local estimation from global optimization:

• GLIM publishes local odometry (odom -> base_link): Nav2’s controller tracks this directly, and RTAB-Map uses it to link consecutive nodes in its pose graph.
• RTAB-Map publishes global corrections (map -> glim_map): It detects loop closures, corrects cumulative drift above GLIM’s frames, and publishes the map Nav2 plans on.

To prevent competing optimizers from causing discrete pose jumps, we disable GLIM’s internal global mapping and remap its root frame to glim_map. From here on, GLIM owns odometry, and RTAB-Map owns the map.

Seeing: lidar for shape, cameras for place

Stretch 4 uses two inverted Hesai JT128 lidars, tilted 45° toward the floor, and two OAK fisheye head cameras. Each does what it is best at: the lidars give precise room geometry for odometry and the occupancy grid, and the cameras recognize places (“have I been here before?”) for loop closures. Before reaching the navigation stack, both streams are turned into clean, purpose-built inputs.

Lidar pipeline

One pass fuses the two lidars into two outputs. A merged 3D point cloud with the robot’s own body filtered out (/lidar_points_both) feeds GLIM and RTAB-Map. A filtered 2D scan (/scan_filtered) drives Nav2’s costmaps and collision monitor.

Vision pipeline

The camera streams go through two steps before reaching RTAB-Map:

• Stereo emulation. Using the cameras’ calibration, the fisheye frames are rectified into a row-aligned stereo pair, and stereo_sync bundles each left and right frame into one message. RTAB-Map needs that bundle because its native stereo input silently drops the lidar cloud. We also throttle the cameras from 10 to 2 fps: RTAB-Map adds about one node per second, so more frames would only cost CPU.
• Self-masking. The head cameras see parts of Stretch’s own base, arm and gripper. Features on them would look like static landmarks and corrupt loop closures. Our robot_self_mask node projects the robot’s collision meshes into each frame and covers them with a feathered box blur. A blur removes the texture without creating the sharp artificial corners a solid black mask would.

Fusing the streams in RTAB-Map

RTAB-Map registers with both streams (Reg/Strategy: 2). The visual stream proposes loop-closure candidates and an initial transform guess. Lidar ICP then refines that guess, matches against nearby nodes, and builds the occupancy grid.

Odometry: keeping the robot on the floor

GLIM estimates full 6-DoF motion, and on Stretch 4 that turned out to be more than we wanted. Over long runs the estimated height crept steadily upward, reaching about 6 m, while the robot never left the floor.

The cause is where the IMU sits: inside the right lidar, about 1.5 m up the mast. When the base tilts slightly, for example rolling over a threshold, that long lever arm adds a centripetal term to the measured vertical acceleration. The term always has the same sign, so it never averages out. GLIM’s smoother keeps only the last few seconds of states and its local map is short-lived, so the error is absorbed rather than corrected. And with no sensor measuring absolute height, nothing pulls it back.

A wheeled robot in a home stays on one floor, so we treat planar motion as a constraint rather than something to estimate. We added a flatten_odom_tf option to glim_ros2 that publishes
odom -> base_link with height, roll and pitch fixed at zero, keeping x, y and heading. The full 6-DoF estimate remains available on /odom. RTAB-Map applies the same constraint to its corrections with Reg/Force3DoF; without it, small tilts in those corrections leaned the scan by about 1.2°, and the costmap discarded the affected points.

Mapping: making physical changes stick

The point of mapping while navigating is that the map keeps up as the environment changes. Out of the box, RTAB-Map struggled with this: moved obstacles would appear, disappear, and reappear across visits.

This stemmed from RTAB-Map’s graph reduction (Mem/ReduceGraph), the mechanism that merges new nodes into existing ones to keep the graph compact. When merging, RTAB-Map threw away the new node’s local occupancy grid and preserved the old, stale view. Merged nodes also remained in the database along with dangling one-way links. Over time, these loaded back as “zombie” chains that rejoined the active graph whenever a loop closure landed on one. Simply disabling graph reduction wasn’t an option. For long-term, continuous operation, every pass through a room would add redundant nodes until memory bloat and optimization latency broke the system.

We resolved this on our stretch/map-persistence branch with one rule: the newest observation wins.

• Mem/ReduceGraphGridOwnership: When nodes merge, the fresh occupancy view transfers into the surviving node. Nearby older nodes only lose cells that directly contradict the new scan; agreeing cells remain. To prevent dynamic obstacles (like walking people) from being permanently baked into the map, a cell is only committed when at least one other recent node (one of the last 10) saw it the same way, occupied or free.
• Mem/ReduceGraphMergedKept: false: Merged nodes are deleted from the database instead of saved half-linked, ending zombie chains.
• Incremental grid updates: RTAB-Map now publishes which sub-grids changed, so the ROS side reloads only those instead of serving stale cached grids until a restart.

In our first iteration, clearing all observed cells from older nodes left dotted holes across the map (2,442 broken cells against a 1,054 reference). Removing only contradicting cells brought that count to 1,052.

Alongside the persistence patch, we tuned four RTAB-Map settings to keep long-running sessions stable:

• Memory management disabled (Rtabmap/TimeThr: 0, Rtabmap/MemoryThr: 0): Moving nodes out to disk shifted the map origin and caused parts of /map to vanish.
• GTSAM/Incremental: false: Switching to batch GTSAM prevents iSAM2 crashes when graph reduction deletes edges from the graph.
• GridGlobal/MinSize: 50: Allocates a fixed initial grid boundary, preventing the map from growing in steps, which resized Nav2’s costmap 143 times in one run.
• 120 s shutdown timeout: Gives SQLite sufficient time to serialize the database on Ctrl+C (the default 5 s timeout cut it off mid-write).

Knowing when it’s safe to map

The stack has two modes. In SLAM mode (mode:=slam) the robot builds a new map from scratch and maps from the first scan; there is no earlier map to protect. In localization mode (mode:=localization) it starts from a saved map, and that map is what needs protecting: if the robot is wrong about where it is, everything it adds lands in the wrong place. So a localization session starts in pure localization, and mapping is something the robot earns. A small node,
rtabmap_supervisor, watches RTAB-Map’s loop closures and the distance driven since the last one, and makes the call without anyone watching.

Two accepted closures within 3 m of driving make the robot LOCALIZED. After 10 s of that, the supervisor switches RTAB-Map to mapping (JOINING), and the robot only starts adding to the map (MAPPING) once it has linked back to the old one. After 6 m without a fix it is LOST and drops back to localization. While mapping, it also counts as lost after 10 updates near old nodes without a closure back to them. Each retry waits twice as long as the last, from 10 s up to 120 s, because every switch costs RTAB-Map about 35 s. Being lost never stops the robot: driving is usually how it finds a closure again.

Light is the other condition. The lidars don’t care about the lights, but in a dark room the camera finds few features, so whatever RTAB-Map adds is poorly anchored. Once a second, the supervisor measures the head camera image’s average brightness, on the 0 (black) to 255 (white) pixel scale. If it stays below 40 for 5 seconds, the room counts as dark: mapping stops and the robot keeps localizing on the lidars alone. It hands RTAB-Map its current pose on the switch, so it is localized again within about 2 s. Mapping is allowed again only after the brightness has stayed above 70 for 30 seconds, so a flicker or someone walking past doesn’t flip it back and forth.

Getting unstuck

Plain Nav2 assumes the map is right. Because our map updates continuously, the robot regularly meets differences between the stored layout and the physical room, and two of them stop stock Nav2. A thin layer, mapped_navigator, takes goals instead of Nav2 and handles both.

The first is waking up inside an obstacle. When localization corrects the pose, or an obstacle the map still remembers has since moved, the robot can find itself inside an occupied cell, and Nav2 refuses to plan (START_OCCUPIED). The navigator checks the live lidar scan first. If something really is inside the footprint, it stays put. If the space is clear and only the costmap is stale, it clears the costmap around the robot. If the map itself is stale, it inches the robot to the nearest free pose, at most 0.5 m, so the planner can start again.

The second is no path to a perfectly good goal, because the map hasn’t seen the way yet: a doorway still blocked by old marks, or a goal in unexplored space. The navigator first clears the global costmap. If there is still no path and the robot is mapping, it picks frontiers between known and unknown space within 4 m of the goal, spins in place at each, waits 3 s for /map to update, and plans again.

Every command the mapped_navigator sends goes through Nav2’s collision monitor, which stops the robot on stale scans, so it never drives blind.

What it takes to run

On the robot’s NUC (Intel Core Ultra 5 225H, 14 cores), the whole stack uses about 8 cores while mapping: a bit over half the machine, with room left for other work. Roughly:

The number to watch isn’t a percentage, it’s RTAB-Map’s update time. RTAB-Map works on one update at a time, mostly on a single core, and an update takes about a second at its 1 Hz rate, mostly lidar matching against nearby parts of the map plus graph optimization. When updates take longer than that, RTAB-Map skips frames. Nodes then land farther apart, and the robot gets fewer chances to close loops.

Two settings make a real difference:

• Match lidar scans on a 10 cm voxel grid (Icp/VoxelSize: 0.10). At 5 cm the NUC hit 102 °C, and scans arrived 1.0-1.7 s old, late enough that the collision monitor stopped the robot.
• Run the lidars in single-return mode (strongest). On the same map, dual return (last + strongest) adds about 17% more points, costs about half a core more, and nearly doubles RTAB-Map’s update time (0.84 s to 1.55 s), with no gain we could see.

With the optional Jetson

GLIM and RTAB-Map can run on a Jetson Orin NX on the robot instead, in a Docker container (ROS Jazzy needs Ubuntu 24.04; JetPack 6 is 22.04). Everything else stays on the NUC: a raw camera stream alone is about 70 MB/s, more than gigabit Ethernet carries, and the collision monitor needs fresh scans next to the motors. Only the masked stereo pair (2 Hz, about 28 MB/s), the lidar cloud (about 10 MB/s), the IMU and the transforms cross over, through two linked Zenoh routers. One launch file on the NUC starts both halves, and a small jetson tool deploys the code and checks the link before a run.

So the Jetson buys room on the NUC, about 2.4 cores, not faster mapping. Its Cortex-A78AE cores are slower than the NUC’s, and RTAB-Map’s single-core update feels that most. The GPU sits mostly idle: only GLIM’s odometry runs on CUDA, in short bursts, and RTAB-Map doesn’t use the GPU at all.

Resources

• Repository: github.com/hello-robot/stretch4_rtabmap
• Running GLIM and RTAB-Map on a Jetson: README_GPU.md