Skip to content

Fix: deterministic multithreaded lidar odometry#377

Closed
bloom256 wants to merge 4 commits intoMapsHD:mainfrom
bloom256:fix/deterministic-lidar-odometry-2
Closed

Fix: deterministic multithreaded lidar odometry#377
bloom256 wants to merge 4 commits intoMapsHD:mainfrom
bloom256:fix/deterministic-lidar-odometry-2

Conversation

@bloom256
Copy link
Copy Markdown
Collaborator

Problem

optimize_lidar_odometry gives different results between multithreaded runs.
The multithreaded path used tbb::combinable + combine_each to add up
Hessian matrices from per-thread local copies. combine_each goes through
thread-local storage in an unspecified order, and since floating-point addition
is not associative
, different summation orders give different Hessian matrices.
Over many optimiser iterations, these small differences grow and can lead to
different convergence paths.

Fix

Replace tbb::combinable with per-point AtPA/AtPB storage. Each point's
6x6 and 6x1 Hessian contributions are computed in parallel (via
tbb::parallel_for over points), then summed sequentially in fixed point order similar for ST and MT.
The ST and MT paths now share the same code -- only the loop type differs
(tbb::parallel_for vs plain for).

Changes in lidar_odometry_utils_optimizers.cpp

  • Removed #include <tbb/combinable.h> and #include <tbb/blocked_range.h>
  • add_indoor_hessian_contribution / add_outdoor_hessian_contribution now
    write to per-point output matrices instead of accumulating into the global
    Hessian directly
  • compute_hessian takes per-point vectors + point index instead of global
    matrices
  • Unified ST/MT caller: allocate per-point vectors, compute (parallel or
    sequential), sum sequentially in point order
  • Fixed orphaned UTL_PROFILER_END(before_iter) that was inside
    process_worker_step_lidar_odometry_core but its matching BEGIN was in the
    caller -- moved it back to the caller
  • Added UTL_PROFILER_SCOPE to process_worker_step_1, process_worker_step_2,
    and process_worker_step_lidar_odometry_core

Note: This was not caught in the previous nondeterminism PR because the
floating-point differences are very small and only showed up on certain datasets
during extended testing.

Testing

Tested on 7 datasets (4 MT + 4 ST runs each), lengths from 500m to 5000m.
All runs showed deterministic behaviour, with identical results between ST and MT.

@bloom256 bloom256 closed this Feb 27, 2026
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