Fix predict_to_current_time forward extrapolation and Omnidirectional3D jacobian

[bkanator]

Summary

Two fixes to the 3D omnidirectional estimation path, found while debugging a turn-rate-proportional yaw lag on a mecanum base (MoveIt Pro hangar_sim).

1. fix(odometry_3d): restore predict_to_current_time forward extrapolation

Odometry3DPublisher::predict() computed the forward delta as std::min(to_predict_to - stamp, 0.0). Since to_predict_to (now) is always ≥ the latest optimized stamp, this forces dt == 0 and silently disables predict_to_current_time: the publisher emits the stale latest-optimized pose stamped as "now". Regressed in #24 (a 2‑line change riding along in an unrelated multi-camera PR); the 2D publisher does it correctly.

Fix: std::min → std::max (clamp to non-negative — preserve forward prediction, still prevent backward). Extracted into detail::forwardPredictionDt() with a regression test.

2. fix(omnidirectional_3d): compute prediction jacobian in RPY space

The assembled state jacobian used jacobian << J[0], J[1], J[2], J[3], J[4].block<15,2>(0,0) — placing the quaternion-space J[1] (15×4) into the orientation columns and truncating the acceleration block, giving Ceres an incorrect orientation gradient during 3D rotation. (This was the TODO(henrygerardmoore): figure out how to fix this referencing locusrobotics#354.) Converts J[1] to RPY space (15×3) via the pseudo-inverse of the quat→RPY jacobian (chain rule). Covered by the existing analytic-vs-autodiff comparison in test_omnidirectional_3d_state_cost_function.

Measured impact (fuse /odom_filtered yaw error vs. ground truth, mecanum spins)

turn rate	before (buggy)	after (both fixes)
8–15 °/s	1.8°	0.72°
15–25 °/s	3.0°	0.76°
25–40 °/s	4.4°	0.85°

The lag was linear in turn rate (a ~150 ms constant delay); the fix flattens it to <1° at all turn rates. predict_to_current_time on/off was verified to make no difference before the fix (extrapolation was inert) and to work after.

Tests

• New test_prediction_time (regression for the predict_to_current_time delta).
• Existing test_omnidirectional_3d_state_cost_function analytic-vs-autodiff jacobian comparison passes with the jacobian fix.
• fuse_models builds and all touched tests pass on Humble.

Notes

• Both the ros-humble-fuse and ros-jazzy-fuse apt packages build from this humble branch, so this fixes both distros.

[bkanator]

Updated to address review:

• Gimbal-lock robustness: the quat→RPY pseudo-inverse now uses CompleteOrthogonalDecomposition instead of an explicit Aᵀ(AAᵀ)⁻¹, which would NaN where quaternion2rpy zeros rows of the quat→rpy jacobian (|pitch|≈90°). Added <Eigen/QR>, const on the locals, removed the stale TODO(henrygerardmoore).
• License headers added to the two new files (detail/prediction_time.hpp, test_prediction_time.cpp).
• Include ordering in odometry_3d_publisher.cpp fixed (common/ before detail/).

Cross-checked: the 2D publisher and unicycle path don't need either fix (2D dt is unclamped; unicycle jacobian has no quaternion block). fuse_models builds and test_prediction_time (2) + test_omnidirectional_3d_state_cost_function (2) + test_omnidirectional_3d_predict (9) all pass on Humble.

[griswaldbrooks]

Looks good. How hard is it to stand up a test for fuse_models::predict? Looks like it's just a function?

[bkanator]

[written by AI]

Good call — added it. TEST(Predict, predictJacobians) in test_omnidirectional_3d_predict.cpp (it is just a function, so the harness was already there for the value tests).

It computes the analytic 15×15 state jacobian via the Vector3d/Quaterniond overload, then the same jacobian by ceres::Jet autodiff through the templated RPY-in/RPY-out overload, and EXPECT_MATRIX_NEARs the two at 1e-9. RPY is extracted with fuse_core::quaternion2rpy so the autodiff input matches the analytic input's convention.

Verified it's a real guard, not just green: it fails against the prior truncated J[4].block<15,2> assembly and passes with the RPY conversion. Folded into the jacobian commit; full test_omnidirectional_3d_predict suite is 10/10.