Skip to content

fix: apply IMU extrinsic in get_body_cloud / get_body_cloud_down#1

Open
leshy wants to merge 1 commit into
jeff/feat/fastlio-body-cloudfrom
ivan/fix/body-cloud-imu-extrinsic
Open

fix: apply IMU extrinsic in get_body_cloud / get_body_cloud_down#1
leshy wants to merge 1 commit into
jeff/feat/fastlio-body-cloudfrom
ivan/fix/body-cloud-imu-extrinsic

Conversation

@leshy

@leshy leshy commented Jul 2, 2026

Copy link
Copy Markdown
Member

get_body_cloud() / get_body_cloud_down() returned the undistorted scan in the raw LiDAR/sensor frame, while odometry/tf reports the IMU pose — downstream registration is off by the extrinsic (~5 cm on a Mid-360).

Transform each point LiDAR→IMU into a copy via the existing RGBpointBodyLidarToIMU (now const), mirroring vanilla FAST-LIO publish_frame_body / cloud_registered_body. feats_undistort / feats_down_body are untouched — the EKF still reads them.

The get_body_cloud groundwork is otherwise kept as-is. dimos-side flake bump follows after merge.

@aur3l14no

Copy link
Copy Markdown

Could we also add a get_world_cloud_down() getter here?

For downstream consumers such as navigation / PGO, it is useful to publish a registered world-frame scan without forcing the full dense cloud path. get_world_cloud() already exists, and this would mirror the existing get_body_cloud_down() behavior using FAST-LIO's downsampled scan.

I have a small branch that adds it on top of this work for reference:
https://github.com/aur3l14no/dimos-module-fastlio2/tree/feat/world-cloud-down

Since this PR is still unmerged, I wasn't sure whether you'd prefer folding this into this branch or handling it later as a stacked/follow-up PR.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants