[griswaldbrooks]

Motivation

An absolute IMU orientation near ±pi can produce a roughly 2pi yaw residual for a 0.02-radian heading difference. This can cause incorrect heading corrections while a simulated robot faces west.

What changed

Wrap roll, pitch, and yaw differences before information weighting in NormalPriorPose3DEuler, its AutoDiff functor, and the sibling NormalPriorOrientation3DEulerCostFunctor. Preserve the local Jacobians away from the pi-error cut. Add regressions through AbsolutePose3DStampedEulerConstraint::costFunction() and the AutoDiff cost covering both yaw crossing directions, q/−q, ordinary yaw, nonzero roll/pitch, correlated weighting, and matching Jacobians.

Cover the sibling fix through AbsoluteOrientation3DStampedEulerConstraint with both yaw directions and correlated weighting. No additional absolute Euler prior with this defect remains in fuse_constraints.

How it was tested

Using an existing ROS Jazzy image in a temporary network-isolated container:

colcon build --packages-select fuse_constraints --executor sequential --cmake-args -DBUILD_TESTING=ON -DCMAKE_BUILD_TYPE=Release
colcon test --packages-select fuse_constraints --executor sequential --return-code-on-test-failure
colcon test-result --verbose

The final package builds; all 115 tests pass, with no errors, failures, or skips. The pose and orientation constraint suites account for 16 of those tests. Before the fix, the three new regression tests produced 2 failures and 1 pass: both analytic and AutoDiff yaw residuals were ±6.263 instead of ∓0.02, and wrapping-before-weighting failed. The sibling yaw regression also fails before the fix. All four added tests pass with the fix; the ordinary-angle test passes before and after.

CI's rolling build currently fails in the covariance_geometry_ros dependency before fuse is built, so it is unrelated to this change; the Jazzy colcon build and tests above pass.