Repository navigation
Optionally retain point clouds and run ICP for fine-grained corrections - #58
Conversation
mbpeterson70
commented
Oct 14, 2026
mbpeterson70
commented
- support creating a point cloud map alongside lightweight segment map
- optionally use point cloud map + ICP to improve error of coarse object-based registration
for use in downstream fine-grained regisrtation corrections
used by fastsam and mapper point cloud aggregator
liqyn
left a comment
liqyn
left a comment
There was a problem hiding this comment.
Looks good! I only had a couple of small stylistic comments.
| return None | ||
| pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(points)) | ||
| pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid( | ||
| radius=self.icp_normal_radius, max_nn=30)) |
There was a problem hiding this comment.
small thing but max_nn should be parameter if everything else is
| def unproject_depth(depth, pcd_stride, depth_scale, depth_intrinsics): | ||
| """Return strided camera-frame XYZ (H, W, 3) and a valid-depth mask. | ||
|
|
||
| Matches Open3D depth unprojection, including float32 depth rounding and its | ||
| default 1000-meter truncation. Intrinsics refer to the full-resolution image. | ||
| """ | ||
| metadata = DepthProjectionMetadata(pcd_stride, depth_scale, depth_intrinsics) | ||
| depth = np.asarray(depth) | ||
| if depth.ndim != 2: | ||
| raise ValueError("depth image must have shape (H, W)") | ||
| s = metadata.pcd_stride | ||
| z = (depth[::s, ::s].astype(np.float64) / depth_scale).astype(np.float32).astype(np.float64) | ||
| v, u = np.mgrid[0:depth.shape[0]:s, 0:depth.shape[1]:s] | ||
| fx, fy = depth_intrinsics.get_focal_length() | ||
| cx, cy = depth_intrinsics.get_principal_point() | ||
| # Invalid depths are excluded by the mask; avoid warnings for inf * 0. | ||
| with np.errstate(invalid='ignore'): | ||
| xyz = np.stack([(u - cx) * z / fx, (v - cy) * z / fy, z], axis=-1) | ||
| valid = np.isfinite(z) & (z > 0) & (z < 1000.0) | ||
| return xyz, valid |
There was a problem hiding this comment.
Creating the DepthProjectionMetadata internally here seems pointless, and the intrinsics/scale doesn't even come from it in the calculations. For cleanliness, maybe have DepthProjectionMetadata as a function input instead of stride/scale/intrinsics separately. Then any caller can reuse metadata as well.
There was a problem hiding this comment.
Hah yeah, that's what I was trying to do and somehow only got halfway through it looks like. Thanks for catching!
| self.mapper.update_point_cloud(img_t, T_odom_camera, depth_data[0]) | ||
| else: | ||
| metadata = DepthProjectionMetadata(self.fastsam.pcd_stride, | ||
| self.fastsam.depth_scale, self.fastsam.depth_cam_intrinsics) |
There was a problem hiding this comment.
I suggest making this a property of FastSAMWrapper - then it can be shared here and in the depth unprojection in FastSAMWrapper itself.
There was a problem hiding this comment.
Nice, that sounds clean
|
|
||
| # feed the full scan (camera frame) to the mapper's aggregated point clouds | ||
| if self.mapper_params.store_aggregated_pcds: | ||
| if self.data_params.use_pointcloud: | ||
| self.mapper.update_point_cloud(img_t, T_odom_camera, depth_data[0]) | ||
| else: | ||
| metadata = DepthProjectionMetadata(self.fastsam.pcd_stride, | ||
| self.fastsam.depth_scale, self.fastsam.depth_cam_intrinsics) | ||
| self.mapper.update_point_cloud(img_t, T_odom_camera, depth_data, | ||
| is_depth_img=True, depth_projection_metadata=metadata) |
There was a problem hiding this comment.
Just noting - all other mapper updates go in update_segment_track. I don't think there is a clean way to put this anywhere else though, so it's fine
There was a problem hiding this comment.
Yeah, it's a bit clunky here, but I don't imagine this will be changing a whole lot in the future so will probably just leave it as is for now.
| self.icp_correspondence_distances = [1.0, 0.5, 0.25, 0.1] | ||
| self.icp_max_iterations = 30 | ||
| self.icp_min_fitness = 0.2 | ||
| self.icp_max_source_points = 10000 | ||
| self.icp_normal_radius = 0.4 | ||
| self.icp_max_translation = 1.5 | ||
| self.icp_max_rotation_deg = 10.0 |
There was a problem hiding this comment.
These defaults always get overridden; might it be cleaner to have a function that lets you set these params after the class is created? No worries either way - not a big deal.
There was a problem hiding this comment.
Yeah, I originally was okay with this because the defaults could still be used by a method that doesn't create registration with the params object, but I think it's cleaner to have defaults only in one place in the repo like you're suggesting. Changed now!
|
|
||
| correction = T @ np.linalg.inv(initial_transform) | ||
| moved = np.linalg.norm(correction[:3, 3]) | ||
| turned = np.rad2deg(np.arccos(np.clip((np.trace(correction[:3, :3]) - 1) / 2, -1, 1))) |
There was a problem hiding this comment.
Might want to use Rotation.from_matrix(...).magnitude() like mapper.py for consistency?
| xyz, valid = unproject_depth(point_cloud, metadata.pcd_stride, | ||
| metadata.depth_scale, metadata.depth_intrinsics) |
There was a problem hiding this comment.
As I mentioned earlier, having the metadata itself as the function input makes a lot more sense to me
