feat(holistic_fusion): initialize heading or yaw and position from measurements with frames

[nubertj]

Motivation

• initYawAndPositionInWorld and the frameless initYawAndPosition took the measurement as if it were in the world. The fixed frame was ignored.
• The yaw transfer to the IMU ignored the tilt of the IMU and of the mount. With a strongly tilted mount, frame1 ended at yaw 1.76 rad where the target was 0.80 rad.
• The init was also required with no absolute reference, so callers passed dummy measurements.

Main change

• initHeadingAndPosition(T_M_S) sets the heading about the z-axis of the world, the same quantity as the heading factor (feat(holistic_fusion): add a heading factor that is defined for every attitude #55). It is closed form (twist of the offset about world z) and defined for every attitude.
• initYawAndPosition(yaw_W_S1, M_t_M_S2) takes the yaw in the world frame, as the yaw factor after refactor(holistic_fusion): remove the holistic yaw factor #56 (remove the holistic yaw factor). A yaw in another fixed frame throws. The position can be in a fixed frame.
• initHeadingAndPositionAsIdentity() sets the heading and the position of the initialization frame to identity, for setups without an absolute reference. It replaces the dummy identity measurements.
• Measurements in a fixed frame M other than the world go through the guess from initWorldFrameToFixedFrameTransform(), or identity, so the world starts at M. Roll and pitch always stay with the IMU alignment.
• The frameless initYawAndPosition and initYawAndPositionInWorld are removed. The ROS1 and ROS2 examples use the new functions.

Test plan

• New init_heading_and_position_test through the FlexCode Bazel build:
  • heading and position of S match the measured pose, including the x-axis vertical and a 1 degree tilt mismatch
  • a pose in a fixed frame goes through the T_W_M guess
  • the yaw of S1 in the world and the position of S2 through T_W_M match, roll and pitch stay with gravity
  • a yaw in a fixed frame is rejected
  • the identity init sets heading and position to identity and keeps the gravity tilt
  • with the old yaw transfer, the heading test fails
• graph_config_test passes.
• ROS1 and ROS2 CI build the migrated examples. I could not build them locally.

🤖 Generated with Claude Code. Co-authored with @nubertj.