Step 1: reuse map-bucket lookups across LM iterations (~1.7x faster, identical results)

[michalpelka]

Summary

In lidar_odometry_step_1, every LM iteration looked up each point's indoor/outdoor NDT bucket twice: once to build the normal-vector histograms (anisotropic weighting), then again when building the Hessian. Step 1 typically runs ~170–180 iterations per chunk, so these hash lookups made up a large share of the runtime.

This PR:

• Fewer lookups: keeps each point's buckets (PointBuckets: pointers, keys, histogram bins) across the iterations of a chunk. Each bucket is looked up once per iteration, and after the first iteration only for points whose bucket key changed.
• Incremental histograms: updates the normal-vector histograms only for the points that changed bucket. The tables hold integer counts, so the result is exact.
• Signature change: optimize_lidar_odometry now takes the cached buckets instead of the maps. The stale declaration in lidar_odometry_utils.h is updated to match.

The bucket maps are only modified between chunks (process_worker_step_update_rgd_after, on the same worker thread), so the cached pointers stay valid. The GUI only reads the maps under the same mutex.

Port of the same optimization from mini_lio (header-only port of step 1).

Results: identical output, ~1.6–1.7× faster

Apple M5, Release build. Original = 25bc0ca, patched = this branch.

Dataset	Params	Step 1 optimization: original → patched	LM iterations	Output
MandeyeMappingLoopClosure (Livox, 650 s, 6505 chunks)	dataset's own .toml	722.0 s → 425.3 s (1.70×)	1,177,777 both	byte-identical
Hesai JT128 hand-held walk (140 s, 2658 chunks)	defaults	142.2 s → 87.9 s (1.62×)	456,615 both	byte-identical

Wall time on MandeyeMappingLoopClosure: 782.6 s → 484.3 s; CPU time (user): 4755 s → 2631 s.

How "byte-identical" was checked on MandeyeMappingLoopClosure:

• Files: all 260 output files (scan_lio_*.laz, trajectory_lio_*.csv, session_poses.mrp, session_ini_poses.mri, point_sizes_per_chunk.json) are identical (cmp).
• Hash: all trajectories plus poses hash to the same SHA-256 (fdd94290…34cd) in both runs.
• Trajectory length: 962.6747473255328 m in both runs.
• Not compared: session.mjs differs only in elapsed time and output paths. The HDMapping_* params/results files were skipped because they contain timestamps.

Note: both MandeyeMappingLoopClosure runs used real_time_threshold_seconds = 1e9, with the machine kept awake (caffeinate). That threshold is a wall-clock cutoff, so a stall (e.g. the machine sleeping) truncates a chunk's iterations and makes results timing-dependent. In normal runs, chunks take ~0.1 s and it never fires.

Test plan

• Release build of lidar_odometry_step_1
• MandeyeMappingLoopClosure: original vs patched, all outputs byte-identical
• JT128 session: original vs patched, all outputs byte-identical

🤖 Generated with Claude Code