correct_ros2_dewarp_math.md

Plan: Correct ROS2 Dewarp Math

Fix the dewarping around one consistent reference frame: beam i should use scan_start + i * time_increment, scan readiness should wait only through the last real beam, each beam should be transformed with a proper 2D rigid-body transform into the scan-start frame, and the legacy quarter-turn cloud correction should be removed or isolated because it currently mixes reference frames after dewarping.

Steps

1. Phase 1: lock down intended semantics. Treat LaserScan.header.stamp as the acquisition time of beam 0 and make the scan-start pose the sole reference pose for cloud_from_laser_pub and scan_2. Keep the existing start-time stamps at points.header.stamp = scan_msg.header.stamp and dewarped_scan.header.stamp = scan_msg.header.stamp, but rewrite comments/docstrings so the timestamp, transform math, and published frame all describe the same scan-start reference. This blocks later math changes.
2. Phase 2: fix beam timing and readiness math in /home/chris/src/lidar_dewarping/dewarp_scan_ros2.py. Beam index i should use scan_start + i * time_increment, not (i + 1) * time_increment; scan_end_ns() should use the timestamp of the last real beam, which is scan_start + (len(ranges) - 1) * time_increment for non-empty scans. This depends on step 1.
3. Phase 3: replace the current point transform in process_scan() with a proper 2D rigid-body transform. Compute the sampled robot pose relative to the scan-start pose in the scan-start frame, then add the beam vector rotated by the sampled relative heading. Do not add a world-frame translation vector directly to a scan-frame beam vector. This depends on step 2.
4. Phase 4: remove the legacy whole-cloud correction beginning at robot_dth2 = robot_dth / 4.0 once the primary transform is mathematically correct. If temporary comparison is useful during rollout, gate it behind a clearly named compatibility parameter and default it off; the recommended path is to delete it rather than preserve a quarter-angle heuristic. This depends on step 3.
5. Phase 5: re-check the downstream scan-domain filter at removed_items = self.filter_scan(dewarped_scan, robot_dth). Keep it only if bag replay still shows a clear benefit after the math fix; otherwise convert it into an opt-in diagnostic filter so scan_2 represents the raw corrected scan and scan_3 stays a debugging stream. This depends on step 4.
6. Phase 6: make the math testable. Extract the beam timing and point projection math into small pure helpers inside /home/chris/src/lidar_dewarping/dewarp_scan_ros2.py so they can be exercised with deterministic inputs without spinning ROS 2. If the repo stays dependency-light, add a small stdlib unittest regression module; if not, add a repeatable script that checks known pose/beam cases. This can be prepared in parallel with step 1 but should be finalized after steps 2-4.
7. Phase 7: update /home/chris/src/lidar_dewarping/README.md to document the corrected scan timing semantics, the fact that scan dewarping now uses the scan-start pose consistently, and whether the legacy filter remains enabled by default. This depends on steps 1-5.

Relevant files

• /home/chris/src/lidar_dewarping/dewarp_scan_ros2.py — fix scan_end_ns(), process_scan(), the legacy midpoint/quarter-turn block, and the post-dewarp filter wiring; reuse OdomHistory.get_interpolated_pose() and the existing worker/queue flow.
• /home/chris/src/lidar_dewarping/README.md — document the corrected scan timing semantics and any behavior change in scan_2 / scan_3.
• /home/chris/src/lidar_dewarping/dewarp_scan.py — use only as a historical reference while removing legacy parity hacks; do not treat it as the correctness source.

Verification

1. Add deterministic math checks for three cases before bag replay: stationary robot should preserve beam geometry exactly; pure translation with a non-zero start yaw should project correctly into the scan-start frame; pure in-place rotation should rotate beams without translation drift.
2. Run the standalone node on the provided ROS 2 bag and confirm /scan_2 still publishes continuously while replaying 2020-02-16-11-52-35_ros2, then compare the corrected scan against cloud_from_laser_pub to verify the scan matches the pre-filter cloud rather than a quarter-turn-adjusted variant.
3. Visually inspect a hallway or corner segment in RViz before and after the change. The corrected output should no longer depend on the robot's absolute heading in the odometry frame, and straight structures should stay straight regardless of scan start orientation.
4. If the sector filter remains, replay a turning segment and confirm the data removed to scan_3 is still justified; if it is not, disable it by default and verify scan_2 remains stable without it.

Decisions

• Included: correctness-driven changes that may change published scan geometry, timestamp semantics documentation, and regression coverage for the transform math.
• Excluded: broad ROS 1 backport work, performance tuning of the worker threads, and unrelated message/statistics cleanup unless a math fix exposes a direct dependency.
• Chosen direction: prioritize geometric correctness over legacy bag-tuned output parity.

Implementation Status (2026-07-11)

The core timing and scan-start projection changes were added in commit d99dc6b (fix: dewarp scans in the scan-start frame). The implementation is not yet complete: in particular, the active node still combines odometry poses for base_link with beam vectors expressed in laser without applying the recorded base_link -> laser extrinsic transform (approximately pi yaw). That frame mismatch can still produce the scan/cloud alignment failures described in problems.md.

Plan phase	Status	Current implementation / remaining work
1. Scan-start semantics	Mostly implemented	Module comments and output header stamps consistently use the input scan timestamp as scan start.
2. Beam timing and readiness	Implemented	beam_sample_time_ns() uses scan_start + i * time_increment; scan_end_ns() uses the final real beam at (len(ranges) - 1) * time_increment.
3. Proper 2D point projection	Partially implemented	relative_pose_in_reference_frame() and project_beam_to_reference_frame() replace direct world-frame translation. They currently use base_link odometry axes while publishing in the scan/laser frame; apply the base_link -> laser transform before considering this complete.
4. Remove quarter-turn cloud correction	Implemented	The legacy robot_dth / 4 whole-cloud correction was removed.
5. Re-check downstream scan filter	Functionally disabled	BYPASS_LEGACY_SCAN_FILTER = True, so scan_2 remains the raw rebinned dewarped scan and scan_3 is empty. Convert this global debug flag into an explicit opt-in diagnostic control if it remains useful.
6. Deterministic math checks	Not implemented	Pure helpers were extracted, but no stdlib unit tests or repeatable known-pose check script exists yet.
7. README update	Not implemented	README.md does not yet document scan-start timing, scan-start reference geometry, or the disabled-by-default filter behavior.
Replay and RViz validation	Not evidenced	No committed validation demonstrates continuous /scan_2 publishing, cloud/scan alignment, or turning behavior on either the original or corrected ROS 2 bag.