[tag_based_sfm_calibrator] Camera BA results are silently cancelled out in the manager's post_process (shadowed by the visualization tfs)

[abailinrun]

Summary

When using the tag-based SfM calibrator through the sensor_calibration_manager, the calibrated camera poses written to the output yaml are not the BA solutions — they are a rigid carry-over of the initial robot-description values, bit-exact up to floating point noise, regardless of whether the BA converged. The same applies to some lidar/kit outputs depending on the project. The BA itself is healthy; only the final output is affected.

How we found it

While calibrating our own vehicle (a downstream project whose calibrator follows the same post_process pattern as xx1_15), we compared the output yaml against the initial values element-wise: all 7 camera rows matched the initial values to ≤ 1.2 µm / ≤ 0.061 mdeg, while the optimized poses stored in the calibration database differed from the initial values by 0.7–2.6 deg / 11–41 cm. The cancellation is exact and unconditional, which is why it is easy to miss — the outputs are valid-looking poses, nothing fails, and the scores/visualization all reflect the true optimized poses.

Root cause

1. Once the calibration finishes, the calibrator node broadcasts the optimized sensor poses (main sensor -> calibration frames) for visualization purposes (publish_tfs, enabled by default).
2. tf2 only allows a single parent per frame, so these broadcasts re-parent the calibration frames and shadow the constant transforms defined in the robot description (e.g. the cameraX/camera_link -> cameraX/camera_optical_link joints).
3. The post_process implementations of the rdv, x2, and xx1_15 base_lidars_cameras calibrators query those constant transforms after the calibration has finished, so the query path is forced through the broadcast tfs and the returned value contains the inverse of the optimized pose:

$$ T_{\mathrm{optical}}^{\mathrm{link}}(\mathrm{queried}) = \mathrm{BA}^{-1} \cdot T_{\mathrm{urdf\ chain}} $$

4. When post_process composes it with the calibration result, the optimized pose cancels out exactly:

$$ T_{\mathrm{kit}}^{\mathrm{main}} \cdot \mathrm{BA} \cdot \left( \mathrm{BA}^{-1} \cdot T_{\mathrm{urdf\ chain}} \right) = T_{\mathrm{kit}}^{\mathrm{main}} \cdot T_{\mathrm{urdf\ chain}} $$

The general rule: any constant-tf query in post_process whose tf path traverses a frame that is broadcast as a child by the calibrator returns a contaminated value and cancels the corresponding BA factor.

Affected outputs (current tier4/universe)

Project	Cancelled outputs	Unaffected outputs
xx1_15	all camera rows (optical_link -> camera_link query)	lidars (no tf query)
rdv	all camera rows + lidar *_base_link rows (lidar -> lidar_base query)	—
x2	all camera rows + base -> front/rear kit rows (kit -> pandar_40p_* queries)	top kit (kit -> main path never traverses a broadcast frame)
default_project	none (no post_process, raw BA results are returned as-is)	all

(The x2/rdv lidar/kit paths are derived algebraically from the same identity; we verified the camera path end-to-end on our vehicle.)

Verification of the fix direction

Caching the constant transforms right after the tfs become available (i.e. before any calibration result can be broadcast) and using the cached values in post_process resolves the issue: re-running a saved calibration database on our vehicle, the camera rows then matched the optimized poses stored in the database to ≤ 0.007 mm / ≤ 0.0004 deg, with the lidar rows and all BA scores unchanged. publish_tfs stays fully functional.

I will open a PR with this fix shortly and link it here.