feat(perception): StereoPointCloud module + FlowBase stereo-nav blueprint#2768
feat(perception): StereoPointCloud module + FlowBase stereo-nav blueprint#2768shahtvisha wants to merge 20 commits into
Conversation
…ect, ghost clearing, global map
shahtvisha
requested review from
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
Greptile SummaryThis PR adds a stereo depth navigation path for FlowBase. The main changes are:
Confidence Score: 5/5This looks safe to merge from the follow-up changes reviewed here.
Important Files Changed
Reviews (11): Last reviewed commit: "[autofix.ci] apply automated fixes" | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
|
|
||
|
|
||
| # FlowBase + RealSense D435i stereo depth + CostMapper + nav stack | ||
| coordinator_flowbase_stereo_nav = ( |
There was a problem hiding this comment.
The documented dimos run coordinator-flowbase-stereo-nav path resolves through the generated blueprint registry, but this change only adds the blueprint variable. Since the registry was not updated with coordinator-flowbase-stereo-nav, the advertised command can fail before this blueprint is loaded.
| max_global_pts: int = 200_000 | ||
| publish_every: int = 3 # emit global_map every N frames | ||
| world_frame: str = "world" | ||
| camera_frame: str = "camera_depth_optical_frame" |
There was a problem hiding this comment.
The new stereo-nav blueprint uses the RealSense depth defaults, where aligned depth is published in the color optical frame, but this module defaults TF lookups to camera_depth_optical_frame. With a D435i, that projects pixels with aligned camera info while applying the depth optical pose, so obstacles can be shifted in the frame_cloud and global_map consumed by CostMapper.
|
|
||
| uu, vv = np.meshgrid(np.arange(W, dtype=np.float32), np.arange(H, dtype=np.float32)) | ||
| dd = depth[mask] | ||
| xyz_opt = np.column_stack([ |
There was a problem hiding this comment.
xyz_cam is rotated from optical coordinates into camera_link, but the later transform still uses the configured optical frame pose. When TF is available, applying an optical-frame pose to link-frame points rotates the cloud into the wrong world coordinates, so CostMapper can receive obstacles in the wrong cells.
| xyz_rel = xyz_vox - t | ||
| pts_snap = None | ||
|
|
||
| with self._lock: | ||
| if len(self._acc_pts) > 0: | ||
| free = _raycast_free_keys(xyz_rel, self.config.global_vox_size) | ||
| if len(free): | ||
| acc_rel = self._acc_pts - t | ||
| keys_acc = _pack(np.floor(acc_rel / self.config.global_vox_size).astype(np.int32)) | ||
| self._acc_pts = self._acc_pts[~np.isin(keys_acc, free)] |
There was a problem hiding this comment.
The clearing path subtracts only the current camera translation before comparing free-space keys with accumulated points. After the robot turns, current rays and older map points are keyed in different orientations, so this can clear unrelated obstacles or leave stale obstacles in global_map.
| if len(self._acc_pts) > self.config.max_global_pts: | ||
| keep = np.random.choice(len(self._acc_pts), self.config.max_global_pts, replace=False) | ||
| self._acc_pts = self._acc_pts[keep] |
There was a problem hiding this comment.
When the accumulated map exceeds max_global_pts, the code samples uniformly from the whole map immediately after appending the current frame. In dense scenes this can discard freshly observed nearby obstacles with the same probability as old distant points, causing CostMapper to intermittently lose occupied cells on the navigation path.
…tion, world-frame ghost clearing
| base_tf = self.tf.get( | ||
| self.config.world_frame, self.config.base_frame, img.ts, self.config.tf_timeout | ||
| ) | ||
| if base_tf is not None: |
There was a problem hiding this comment.
This uses the base_link origin height as the floor height whenever TF is available. On mobile bases where base_link is at the chassis or IMU height above the ground, the filter later keeps only points above base_link.z + floor_margin, so low obstacles and floor-adjacent geometry can be removed before frame_cloud or global_map reaches CostMapper. Use an actual floor frame/height estimate here instead of the robot body origin.
…e — floor calibrator, ICP fallback, rotation-only map
…, strip verbose comments
| xyz_ronly = xyz_vox - t | ||
| vk_r = np.floor(xyz_ronly / self.config.vox_size).astype(np.int32) | ||
| _, first_r = np.unique(_pack(vk_r), return_index=True) | ||
| xyz_vox_r = xyz_ronly[first_r] | ||
| xyz_for_map = xyz_vox_r[xyz_vox_r[:, 2] > self._world_floor_z + self.config.floor_margin] | ||
|
|
||
| pts_snap = None | ||
| with self._lock: | ||
| if len(self._acc_pts) > 0 and len(xyz_for_map) > 0: | ||
| free_keys = _raycast_free_keys(xyz_for_map, self.config.global_vox_size) | ||
| if len(free_keys): | ||
| keys_acc = _pack(np.floor(self._acc_pts / self.config.global_vox_size).astype(np.int32)) | ||
| self._acc_pts = self._acc_pts[~np.isin(keys_acc, free_keys)] |
There was a problem hiding this comment.
xyz_vox - t before ray-casting from (0, 0, 0), but _acc_pts stores points that were inserted after subtracting whatever translation was current in earlier frames. When the robot moves, old map points are not re-expressed relative to the current camera origin before keys_acc is compared with free_keys. Stale obstacles can stay in global_map after the camera sees free space, so CostMapper can continue planning around objects that are no longer there.
| vk_r = np.floor(xyz_ronly / self.config.vox_size).astype(np.int32) | ||
| _, first_r = np.unique(_pack(vk_r), return_index=True) | ||
| xyz_vox_r = xyz_ronly[first_r] | ||
| xyz_for_map = xyz_vox_r[xyz_vox_r[:, 2] > self._world_floor_z + self.config.floor_margin] |
There was a problem hiding this comment.
xyz_vox_r was computed after subtracting t, so its z value no longer includes the camera's world translation. _world_floor_z was saved as cam_z + floor_z, which does include that translation. Once ICP estimates a non-zero vertical offset, valid low obstacles can be filtered out before they enter global_map, and CostMapper can miss floor-adjacent obstacles.
| CostMapper.blueprint(), | ||
| MovementManager.blueprint(), | ||
| ControlCoordinator.blueprint( | ||
| hardware=[_flowbase_twist_base()], | ||
| tasks=[ | ||
| TaskConfig( | ||
| name="vel_base", | ||
| type="velocity", |
There was a problem hiding this comment.
cmd_vel, so the advertised stereo navigation workflow starts without actually navigating.
|
|
||
| if pts_snap is not None: | ||
| self.global_map.publish( | ||
| PointCloud2.from_numpy(pts_snap, frame_id=self.config.world_frame, timestamp=img.ts) |
There was a problem hiding this comment.
pts_snap is published with the world frame, but the accumulated points in _acc_pts were inserted after subtracting the current ICP translation through xyz_ronly. After the robot moves, CostMapper reads these raw coordinates as world points even though they are missing the translation offset. The generated costmap can place obstacles behind their true world positions by the accumulated odometry delta, so navigation can plan against shifted obstacle cells. Store accumulated points in true world coordinates before publishing, or publish them under a frame that matches the translation-stripped coordinates.
shahtvisha
force-pushed
the
pr/stereo-point-cloud
branch
from
002ded3 to
11699ae
Compare
| q0, q1, q2, q3 = self._q | ||
| gx, gy, gz = gyro.astype(np.float64) | ||
| a_n = np.linalg.norm(accel) | ||
| if a_n > 0.5: |
There was a problem hiding this comment.
Using it to skip the accel correction if the reading is basically zero (anything valid should be able to pass this) - let me assign it to a variable so it is more clear, sorry about that!
| if self._t_prev is None: | ||
| self._t_prev = t | ||
| return | ||
| dt = min(t - self._t_prev, 0.1) |
There was a problem hiding this comment.
this 0.1 should probably be a configurable value
There was a problem hiding this comment.
Got it, using it to cap the time step at 100ms so a gap in IMU data doesn't make the filter integrate a huge jump all at once
| for _ in range(self.ITERS): | ||
| dists, idx = tree.query(src + t_est, k=1, workers=1) | ||
| mask = dists < self.MAX_DIST | ||
| if mask.sum() < 30: |
There was a problem hiding this comment.
whats the units on this 30? meters?
There was a problem hiding this comment.
30 is matched point pair per ICP iteration (kept that as a threshold to trust the result to compute a reliable translation)
| pts_ga = (xyz_cam @ R.T).astype(np.float32) | ||
| with self._lock: | ||
| t_est = self._t.copy() | ||
| if self._prev_world is None or len(pts_ga) < 50: |
There was a problem hiding this comment.
lets not hardcode this 50 in two places
| depth = depth[:, :, 0] | ||
| depth = depth.astype(np.float32) | ||
| valid_d = depth[depth > 0] | ||
| if len(valid_d) and np.median(valid_d) > 100: |
There was a problem hiding this comment.
just checking if the depth values look like millimetres or metres, if the median is over 100 it can't be metres (camera only goes to ~10m), so must be mm, divide by 1000.
| K = info.get_K_matrix() | ||
| fx, fy, cx, cy = float(K[0, 0]), float(K[1, 1]), float(K[0, 2]), float(K[1, 2]) | ||
| else: | ||
| fx = fy = float(max(H, W)) / 2.0 |
There was a problem hiding this comment.
I think its best if we don't fallback on a rough intrinsics guess, at least without giving a warning. Otherwise someone could wire this up and be hitting the fallback every time without realizing it
There was a problem hiding this comment.
Got it, will add a warning atleast
| if self._floor_calib.ready: | ||
| keep = xyz_cam[:, 2] > (self._floor_calib.floor_z + self.config.floor_margin) | ||
| else: | ||
| keep = (xyz_cam[:, 2] >= -1.4) & (xyz_cam[:, 2] <= 1.5) |
There was a problem hiding this comment.
Set as a fallback used before floor calibration kicks in so here -1.4 is floor and 1.5 is ceiling
There was a problem hiding this comment.
main things:
- get rid of magic numbers
- add comments for rationale for each section in _on_depth
- expand var names (e.g.
t=> time or transform) - interpolate time/odom in _on_depth to get better accuracy
- use tf system (will discuss in person)
I think, based on the code, the global_map isn't actually global but I'll wait till its running on hardware to see. Stripping the ICP translation seems problematic to me
…pre-calib behavior
… + hole_filling_filter
…ax height limit to 5cm
| coordinator_flowbase_stereo_nav = ( | ||
| autoconnect( | ||
| FilteredRealSenseCamera.blueprint(enable_depth=True, enable_pointcloud=False), | ||
| StereoPointCloud.blueprint(), |
There was a problem hiding this comment.
CostMapper, MovementManager, and coordinator, but it never adds the planner/path-follower modules that turn a clicked goal into nav_cmd_vel. In the advertised click-to-drive flow, clicks can reach MovementManager and the costmap can be produced, but no component consumes the goal and costmap to publish navigation velocity commands, so autonomous cmd_vel is never generated.
| if len(xyz_for_map): | ||
| self._acc_pts = ( | ||
| np.vstack([self._acc_pts, xyz_for_map]) if len(self._acc_pts) else xyz_for_map.copy() |
There was a problem hiding this comment.
xyz_for_map comes from xyz_ronly = xyz_vox - t, so the current frame's map points have the ICP translation stripped. Appending those points directly into _acc_pts, then publishing _acc_pts as world, shifts new observations by the current odometry delta while older observations stay in their earlier basis. After the robot moves, CostMapper can receive duplicated or displaced obstacle cells in global_map.
| free_keys = _raycast_free_keys(xyz_for_map, self.config.global_vox_size) | ||
| if len(free_keys): | ||
| keys_acc = _pack(np.floor(self._acc_pts / self.config.global_vox_size).astype(np.int32)) | ||
| self._acc_pts = self._acc_pts[~np.isin(keys_acc, free_keys)] |
There was a problem hiding this comment.
xyz_for_map, which is in the translation-stripped frame where the current camera is at the origin. keys_acc is computed from _acc_pts, which is published as world-frame map data. Once t is non-zero, those voxel keys no longer refer to the same grid, so stale obstacles can remain uncleared or unrelated world voxels can be removed when the key spaces overlap.
shahtvisha
force-pushed
the
pr/stereo-point-cloud
branch
from
73c2fac to
b10281b
Compare
|
Want your agent to iterate on Greptile's feedback? Try greploops. |
2 participants
Summary
dimos/perception/stereo_point_cloud.py— newStereoPointCloudmodule: subscribes to depth image + camera info, runs gradient filter → pinhole backproject → floor removal → 2 cm voxel dedup → ray-cast ghost clearing → publishesframe_cloudandglobal_mapasPointCloud2dimos/control/blueprints/mobile.py— addscoordinator_flowbase_stereo_nav: FlowBase + RealSense D435i + StereoPointCloud + CostMapper + MovementManager + ControlCoordinatorTest plan
dimos run coordinator-flowbase-stereo-navwith D435i connected — verifyframe_cloudandglobal_mappublishCostMapperreceivesframe_cloudand produces a navigable costmap