fix(state_estimation_simple): correct twist and initial-guess corruption when wheel odometry is active#29
Conversation
…ion when wheel odometry is active Three inter-related bugs caused the LiDAR adaptive threshold sigma to grow toward maximum_sigma whenever wheel odometry was fused: 1. Wrong twist from fuse_pose() when odometry is active. fuse_pose() computed incrPose = new_ICP - last_pose, but last_pose had been modified by fuse_odometry() / fuse_odometry_3d_pose() between scans. The result was the odometry residual / dt instead of the true robot velocity, corrupting de-skewing and the rot_error term in the adaptive threshold. Fix: per-source SourceState in State tracks each source's own last absolute pose and timestamp. fuse_pose() now computes incrPose = new_ICP - src.last_pose, which is always the true ICP-to-ICP motion regardless of intervening odom updates. 2. CObservationRobotPose (3D odom path) overwrote last_pose_obs_tim. When BridgeROS2 forwards wheel odometry as CObservationRobotPose (odometry_as_robot_pose_observation=true), onNewObservation() routed it to fuse_pose(), which updated last_pose_obs_tim to the odom timestamp. If the next LiDAR ICP fuse_pose() arrived with a slightly earlier timestamp, dt < 0 and the call was silently rejected. last_pose was then the absolute wheel odometry pose (in the odom frame), producing ~90-degree initial-guess errors for ICP (confirmed by debug traces showing motionModelError with yaw ~ -1.56 rad and pitch ~ 0.51 rad on a stationary ground robot). Fix: route CObservationRobotPose matching do_process_odometry_labels_re to a new fuse_odometry_3d_pose() method that applies an incremental delta to last_pose (keeping it in the SLAM frame) and never touches last_pose_obs_tim. 3. fuse_odometry() ignored the velocity carried in CObservationOdometry. BridgeROS2 always populates hasVelocities / velocityLocal from the ROS /odom twist. Those values were discarded, leaving last_twist stale. Fix: when hasVelocities is true, update last_twist from velocityLocal. IMU angular rates (fuse_imu) still override wx/wy/wz when available. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
|
Warning Rate limit exceeded
To continue reviewing without waiting, purchase usage credits in the billing tab. ⌛ How to resolve this issue?After the wait time has elapsed, a review can be triggered using the We recommend that you space out your commits to avoid hitting the rate limit. 🚦 How do rate limits work?CodeRabbit enforces hourly rate limits for each developer per organization. Our paid plans have higher rate limits than the trial, open-source and free plans. In all cases, we re-allow further reviews after a brief timeout. Please see our FAQ for further information. ℹ️ Review info⚙️ Run configurationConfiguration used: defaults Review profile: CHILL Plan: Pro Run ID: 📒 Files selected for processing (2)
📝 WalkthroughWalkthroughThe PR refactors state estimation from a single global pose approach to a per-source, delta-based fusion strategy. It introduces per-source state tracking structures, per-source label-processing caches, a new 3D odometry fusion method, and updates twist and covariance calculation to leverage per-source timing and pose deltas. ChangesPer-Source Delta-Based Pose and Odometry Fusion
Estimated code review effort🎯 3 (Moderate) | ⏱️ ~25 minutes Possibly related PRs
Poem
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 Generate unit tests (beta)
Tip 💬 Introducing Slack Agent: The best way for teams to turn conversations into code.Slack Agent is built on CodeRabbit's deep understanding of your code, so your team can collaborate across the entire SDLC without losing context.
Built for teams:
One agent for your entire SDLC. Right inside Slack. Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 3
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
mola_state_estimation_simple/include/mola_state_estimation_simple/StateEstimationSimple.h (1)
68-70:⚠️ Potential issue | 🟡 Minor | ⚡ Quick winStale
\notecomment:frame_idis no longer ignored.
fuse_pose()now usesframe_idas a per-source map key (state_.per_source[frame_id]), so the note "ignores the passed 'frame_id'" is no longer accurate and should be updated to describe the new per-source keying behavior.📝 Suggested update
- * \note This implementation of mola::NavStateFilter ignores the passed - * "frame_id" and GNSS observations. + * \note This implementation of mola::NavStateFilter uses "frame_id" as a + * per-source key for twist estimation (see fuse_pose()) but does not + * perform any coordinate-frame transformation based on it. + * GNSS observations are ignored.Additionally, the coding guideline for this path requires
StateEstimationSimpleto "ignoreframe_idin sensor measurements". Sinceframe_idis now actively used for per-source discrimination infuse_pose(), please confirm with the team whether the guideline should be relaxed or whether a sensor-label-based key (as used infuse_odometry_3d_pose) would be a more guideline-compliant alternative for the per-source map.🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@mola_state_estimation_simple/include/mola_state_estimation_simple/StateEstimationSimple.h` around lines 68 - 70, The class-level note is stale: update the comment in StateEstimationSimple.h to remove "ignores the passed 'frame_id'" and instead document that fuse_pose() uses the measurement's frame_id as a per-source key (state_.per_source[frame_id]) for source discrimination; then either (a) change fuse_pose() to use a sensor-label-based key (same approach as fuse_odometry_3d_pose) to comply with the guideline to ignore frame_id, or (b) get team approval and update the comment to state the component treats frame_id as the per-source identifier—make the change around the StateEstimationSimple declaration and the fuse_pose()/state_.per_source usage sites.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@mola_state_estimation_simple/src/StateEstimationSimple.cpp`:
- Line 274: The code now selects per-source state with
state_.per_source[frame_id], which violates the guideline that
StateEstimationSimple must remain frame-unaware; change fuse_pose() (and its
callers) to use the sensor label instead of frame_id: add a sensorLabel
parameter to fuse_pose() and propagate it from onNewObservation (replace uses of
frame_id when indexing per_source with sensorLabel), keep the previous
[[maybe_unused]] annotation on frame_id if it must be retained, and update any
references to state_.per_source[frame_id] to state_.per_source[sensorLabel]; if
using frame_id here is intentional, update the class note/guideline to document
the frame-keyed behavior.
- Line 261: The for loop that calls ASSERT_GE_ on pose.cov(i, i) is written as a
single-line body and must use braced statements per the coding guidelines;
update the loop in StateEstimationSimple.cpp (the for (int i = 0; i < 6; i++)
ASSERT_GE_(pose.cov(i, i), .0); line) to use a block body { ... } surrounding
the ASSERT_GE_ call so the loop body is explicitly braced.
- Around line 87-104: The current call to state_.last_twist.emplace() overwrites
any IMU-populated angular rates (tw.wx/wy/wz); change the logic to preserve
existing angular-rate components if state_.last_twist already has a value:
before emplacing or assigning the new twist from odom.velocityLocal, read and
store existing wx/wy/wz from state_.last_twist (if present) and restore them
into the new tw, or construct tw from the existing twist and only overwrite
linear components (vx, vy, vz) and omega as appropriate; ensure
state_.last_twist_cov handling remains unchanged and keep the
MRPT_LOG_DEBUG_STREAM call.
---
Outside diff comments:
In
`@mola_state_estimation_simple/include/mola_state_estimation_simple/StateEstimationSimple.h`:
- Around line 68-70: The class-level note is stale: update the comment in
StateEstimationSimple.h to remove "ignores the passed 'frame_id'" and instead
document that fuse_pose() uses the measurement's frame_id as a per-source key
(state_.per_source[frame_id]) for source discrimination; then either (a) change
fuse_pose() to use a sensor-label-based key (same approach as
fuse_odometry_3d_pose) to comply with the guideline to ignore frame_id, or (b)
get team approval and update the comment to state the component treats frame_id
as the per-source identifier—make the change around the StateEstimationSimple
declaration and the fuse_pose()/state_.per_source usage sites.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: defaults
Review profile: CHILL
Plan: Pro
Run ID: 879778e7-3ac8-416e-95ac-bc7126c72edd
📒 Files selected for processing (2)
mola_state_estimation_simple/include/mola_state_estimation_simple/StateEstimationSimple.hmola_state_estimation_simple/src/StateEstimationSimple.cpp
| } | ||
| // and the sum of all strictly >0 | ||
| // Numerical sanity: variances >= 0 (== 0 allowed for some components only) | ||
| for (int i = 0; i < 6; i++) ASSERT_GE_(pose.cov(i, i), .0); |
There was a problem hiding this comment.
Missing braces on for loop — coding guideline violation.
🔧 Proposed fix
- for (int i = 0; i < 6; i++) ASSERT_GE_(pose.cov(i, i), .0);
+ for (int i = 0; i < 6; i++) { ASSERT_GE_(pose.cov(i, i), .0); }As per coding guidelines: "Use braced statements instead of one-liners (e.g., prefer { stmt; } over single-line if/for/while bodies)".
📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| for (int i = 0; i < 6; i++) ASSERT_GE_(pose.cov(i, i), .0); | |
| for (int i = 0; i < 6; i++) { ASSERT_GE_(pose.cov(i, i), .0); } |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@mola_state_estimation_simple/src/StateEstimationSimple.cpp` at line 261, The
for loop that calls ASSERT_GE_ on pose.cov(i, i) is written as a single-line
body and must use braced statements per the coding guidelines; update the loop
in StateEstimationSimple.cpp (the for (int i = 0; i < 6; i++)
ASSERT_GE_(pose.cov(i, i), .0); line) to use a block body { ... } surrounding
the ASSERT_GE_ call so the loop body is explicitly braced.
Codecov Report❌ Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## develop #29 +/- ##
===========================================
+ Coverage 73.90% 74.13% +0.22%
===========================================
Files 41 41
Lines 2905 2996 +91
Branches 251 256 +5
===========================================
+ Hits 2147 2221 +74
- Misses 758 775 +17
Flags with carried forward coverage won't be shown. Click here to find out more. ☔ View full report in Codecov by Sentry. 🚀 New features to boost your workflow:
|
1 participant
Three inter-related bugs caused the LiDAR adaptive threshold sigma to grow toward maximum_sigma whenever wheel odometry was fused:
Wrong twist from fuse_pose() when odometry is active. fuse_pose() computed incrPose = new_ICP - last_pose, but last_pose had been modified by fuse_odometry() / fuse_odometry_3d_pose() between scans. The result was the odometry residual / dt instead of the true robot velocity, corrupting de-skewing and the rot_error term in the adaptive threshold. Fix: per-source SourceState in State tracks each source's own last absolute pose and timestamp. fuse_pose() now computes incrPose = new_ICP - src.last_pose, which is always the true ICP-to-ICP motion regardless of intervening odom updates.
CObservationRobotPose (3D odom path) overwrote last_pose_obs_tim. When BridgeROS2 forwards wheel odometry as CObservationRobotPose (odometry_as_robot_pose_observation=true), onNewObservation() routed it to fuse_pose(), which updated last_pose_obs_tim to the odom timestamp. If the next LiDAR ICP fuse_pose() arrived with a slightly earlier timestamp, dt < 0 and the call was silently rejected. last_pose was then the absolute wheel odometry pose (in the odom frame), producing ~90-degree initial-guess errors for ICP (confirmed by debug traces showing motionModelError with yaw ~ -1.56 rad and pitch ~ 0.51 rad on a stationary ground robot). Fix: route CObservationRobotPose matching do_process_odometry_labels_re to a new fuse_odometry_3d_pose() method that applies an incremental delta to last_pose (keeping it in the SLAM frame) and never touches last_pose_obs_tim.
fuse_odometry() ignored the velocity carried in CObservationOdometry. BridgeROS2 always populates hasVelocities / velocityLocal from the ROS /odom twist. Those values were discarded, leaving last_twist stale. Fix: when hasVelocities is true, update last_twist from velocityLocal. IMU angular rates (fuse_imu) still override wx/wy/wz when available.
Summary by CodeRabbit