feat(stereo-nav): camera odometry + ray-traced global map for FlowBase

[shahtvisha]

Problem

FlowBase's RealSense D435i camera had no pipeline to turn depth data into odometry or a map.

Solution

Added StereoPointCloud: turns the camera's depth feed into a live point cloud, tracks the robot's pose (IMU + ICP), and publishes standard odometry. Wired that into the existing RayTracingVoxelMap + CostMapper modules to build a persistent map and navigation-ready costmap.

How to Test

dimos run coordinator-flowbase-stereo-nav

Connect with dimos-viewer and drive FlowBase around — point cloud, trajectory, and map should update live.

Lidar vs. Camera Benchmark (sanity check)

Compared the Mid-360 lidar (reference) against RealSense raw output and our StereoPointCloud pipeline, aligned via ICP using a tape-measured extrinsic (lidar 37cm / camera ~80.5cm height, 1cm offset, forward-facing).

Candidate	F-score@20cm	RMS error	IoU@5cm	Verdict
RealSense (raw)	0.28	~1.6m	0.01	POOR
Stereo (best single frame)	0.22	~1.4m	0.01	POOR (22% worse F-score than realsense-raw)
Stereo (accumulated over rotation)	0.12	~2.3m	0.00	POOR (59% worse F-score than realsense-raw)

Metrics: F-score@20cm = fraction of points matching within 20cm, both directions (1.0 = perfect match). RMS = average positional error in meters. IoU@5cm = fraction of 5cm voxel cells both sensors agree are occupied.

Contributor License Agreement

• I have read and approved the CLA.

[greptile-apps]

Greptile Summary

This PR adds a RealSense stereo depth pipeline for FlowBase navigation. The main changes are:

• Depth filtering and camera color-publishing controls.
• IMU and ICP-based point-cloud odometry.
• Floor-aligned cloud, trajectory, and odometry outputs.
• Ray-traced mapping and costmap wiring in a new FlowBase blueprint.

Confidence Score: 5/5

The updated code is safe to merge.

• No new blocking issues were found in the changed code.

Important Files Changed

Filename	Overview
dimos/perception/stereo_point_cloud/module.py	Adds depth unprojection, odometry, floor alignment, trajectory publishing, and standard odometry output.
dimos/perception/stereo_point_cloud/imu.py	Adds RealSense motion-sensor discovery and Madgwick orientation updates.
dimos/perception/stereo_point_cloud/filtered_realsense.py	Adds a filtered RealSense depth capture loop for the stereo mapping pipeline.
dimos/control/blueprints/mobile.py	Adds the FlowBase stereo mapping and navigation coordinator blueprint.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart LR
  Camera[Filtered RealSense camera] --> Depth[Depth image]
  Depth --> Stereo[StereoPointCloud]
  IMU[RealSense IMU] --> Stereo
  Stereo --> Cloud[Frame cloud]
  Stereo --> Odom[Odometry]
  Cloud --> Map[RayTracingVoxelMap]
  Odom --> Map
  Map --> Cost[CostMapper]
  Map --> Viewer[Rerun viewer]

Loading

%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
flowchart LR
  Camera[Filtered RealSense camera] --> Depth[Depth image]
  Depth --> Stereo[StereoPointCloud]
  IMU[RealSense IMU] --> Stereo
  Stereo --> Cloud[Frame cloud]
  Stereo --> Odom[Odometry]
  Cloud --> Map[RayTracingVoxelMap]
  Odom --> Map
  Map --> Cost[CostMapper]
  Map --> Viewer[Rerun viewer]

Loading

_{Reviews (3): Last reviewed commit: "chore: drop stereo_rerun.py from this PR..." | Re-trigger Greptile}

[greptile-apps]

Cloud and ray origin diverge

After calibration, frame_cloud is shifted upward by z_offset, but the odometry published below still uses unshifted t. RayTracingVoxelMap consumes both streams, so its ray origin is about one camera height below the shifted cloud and can clear or mark the wrong voxels in the global map.

Suggested change

	pose=Pose(
	position=[float(t[0]), float(t[1]), float(t[2])],
	orientation=[float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3])],
	),
	pose=Pose(
	position=[float(t[0]), float(t[1]), float(t[2] + z_shift)],
	orientation=[float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3])],
	),

[greptile-apps]

Floor calibration ignores orientation

_FloorCalibrator requires gravity-aligned camera-centered points, but this call passes only the optical-to-camera-link rotation. When the camera is pitched or the robot rolls, the floor histogram is biased and the resulting z_shift places every later map point at the wrong height.

Suggested change

	self._floor_calib.update(xyz_cam)
	self._floor_calib.update(xyz_cam @ R.T)

[greptile-apps]

IMU selects a different camera

The camera pipeline can be pinned with serial_number, but this independent IMU feed always opens the first device returned by Librealsense. On a host with multiple RealSense devices, the depth stream can come from the selected D435i while orientation comes from another device—or no IMU is found—producing incorrect odometry and a distorted global map.

[greptile-apps]

Navigation command path is absent

This registered stereo navigation blueprint comments out MovementManager and adds no planner. Rerun click events have no consumer, no module emits nav_cmd_vel, and the generated costmap is never used to drive FlowBase; only direct websocket teleop can reach cmd_vel.

Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!

[greptile-apps]

Camera and IMU can diverge. StereoPointCloud.Config.serial_number is separate from FilteredRealSenseCamera's configuration. When a deployment pins only the camera pipeline with serial_number, this module keeps its default None and RealSenseImuFeed selects the first device it discovers. On a host with multiple RealSense devices, depth can come from the pinned D435i while orientation comes from another camera, producing incorrect pose and map alignment. Propagate the active camera serial to this module or obtain the IMU from the active camera pipeline.

[greptile-apps]

Calibration requires valid gravity. This now correctly uses R to gravity-align points, but start() continues when IMU setup fails and R_at() then remains the identity transform. With a pitched or rolled camera, the floor histogram is still built in camera coordinates and can settle on the wrong floor height. That incorrect z_offset is then applied to every published cloud and odometry pose. Skip floor calibration until an orientation source is available, or explicitly handle the no-IMU mode.