fix(graph_msf): preserve IMU propagation across optimizer updates

[nubertj]

Motivation

When graph states run below the IMU rate, an optimizer update can discard the IMU interval between its latest graph node and the latest received sample. The optimized state is also assigned the newer sample timestamp. At 400 Hz IMU and 40 Hz graph rates, the omitted interval can reach 22.5 ms.

Closest-IMU lookup also dereferences an invalid iterator when a request equals or exceeds the latest sample timestamp, or precedes the oldest sample.

Main change

Preserve the unfinished graph-step preintegration when starting optimizer catch-up. Keep the optimized timestamp and angular-velocity sample associated with the captured graph node. Initialization takes the measured gyro sample before bias correction. Direct callers of initPoseVelocityBiasGraph must supply that sample.

Handle empty, single-sample, endpoint, and out-of-range IMU lookups safely. Preserve nearest-sample selection and tolerance checks. Protect lookup and buffer eviction with the same mutex.

Design decisions and tradeoffs

Copy the step preintegrator into the catch-up preintegrator without aliasing the objects. Preserve its bias linearization and Jacobians so prediction can apply the optimized bias. The latest IMU timestamp remains separate for measurement buffering.

Test plan

• Reproduce the handoff failure with the actual GraphManager regression executable before the fix: Optimizer handoff lost position integration.
• Pass the complete graph_config_test suite against GTSAM 4.3 using FlexCode's Bazel overlay and --config re-amd64-ubuntu24.
• Check empty and partial graph intervals, repeated optimization without a new node, graph-key advancement, zero and nonzero biases, and graph-node angular-velocity association.
• Check empty and singleton IMU buffers, exact endpoints, nearest samples, ties, tolerance rejection, and eviction.
• Run git diff --check.

Passing regression run. No recording replay was run. Concurrent sample arrival during optimization was reviewed through the mutex and copy semantics, but is not simulated by these tests.

Co-authored by OpenAI Codex with user direction.

[nubertj]

looking good