Skip to content

Fix scan accept/reject gating and pose bookkeeping; expose plane rejection stats#60

Open
tejasps28 wants to merge 1 commit intosuperxslam:dev/humanoid_mid360from
tejasps28:dev/humanoid_mid360
Open

Fix scan accept/reject gating and pose bookkeeping; expose plane rejection stats#60
tejasps28 wants to merge 1 commit intosuperxslam:dev/humanoid_mid360from
tejasps28:dev/humanoid_mid360

Conversation

@tejasps28
Copy link
Copy Markdown
Collaborator

This PR fixes a logic bug in LidarSlam.cpp that effectively disabled scan rejection, corrects
last_T_w_lidar bookkeeping so motion deltas are computed against the last accepted pose, and makes
plane rejection diagnostics visible in /super_odometry_stats. It also wires the global
use_imu_roll_pitch parameter so YAML settings are actually respected.

Changes

  1. super_odometry/src/LidarProcess/LidarSlam.cpp
    • checkMotionThresholds() now returns the real acceptResult (no forced accept) and guards
      invalid dt.
    • last_T_w_lidar is initialized only on the first frame and updated after accept/reject.
    • Plane rejection breakdown fields (plane_*) are populated from MatchRejectionHistogramPlane.
  2. super_odometry/src/parameter/parameter.cpp
    • use_imu_roll_pitch is declared and loaded into global USE_IMU_ROLL_PITCH, with logging.

Why

  • Previously, scan rejection never took effect because the code forced acceptance at the end.
  • last_T_w_lidar was overwritten every frame, making motion statistics and gating inconsistent.
  • Plane correspondence health metrics were always zero, blocking diagnostics.

Validation

  • Re-ran humanoid benchmarks (aligned). Metrics such as n_iterations, latency, and translation
    increments remained stable, while plane_* diagnostics are now non-zero and informative.
  • No regression observed in map visualization on the test bag (expected if rejection thresholds
    rarely trigger).

@tejasps28 tejasps28 requested a review from shibowing February 18, 2026 12:51
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.

1 participant