fix(sensor_calibration_manager): cache the constant tfs used by the tag-based SfM post_process

[abailinrun]

Description

Fixes #278 — see the issue for the full analysis and the discovery story.

This PR fixes a subtle bug in the tag_based_sfm_calibrator manager integration that silently discards the camera BA results (and some lidar/kit results) from the final calibration output, replacing them with a rigid carry-over of the initial robot-description values — regardless of whether the BA converged.

Root cause

1. Once the calibration finishes, the calibrator node starts broadcasting 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. The query path is forced through the broadcast tfs, so 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 output row degenerates to the initial value, bit-exact up to floating point noise, no matter what the BA estimated.

Affected outputs

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 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.

Why it is hard to notice

• The degenerate outputs are valid-looking poses (they are the robot-description values, rigidly re-anchored by the base_link correction), so nothing fails and no warning is printed.
• The BA itself is healthy: reprojection errors, scores, and the visualization all reflect the true optimized poses. Only the final yaml is affected.
• Detecting it requires comparing the output against the initial values element-wise — in our case the camera rows matched the initial values to sub-micrometer level while the true BA solutions differed by 0.7–2.6 deg / 11–41 cm.

Fix

Cache the constant transforms as soon as the required tfs become available (on_check_tf_timer, i.e. before the UI becomes ready and therefore before any calibration result can exist), and use the cached values in post_process. A fallback with a warning is kept in case post_process is ever reached without the cache being populated.

This keeps publish_tfs fully functional — the visualization behavior is unchanged.

Related links

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

Tests performed

• The bug and the fix were verified end-to-end on our own vehicle project (a downstream fork whose calibrator uses the same post_process pattern as xx1_15):
  • Before the fix: re-running a saved calibration database, all 7 camera rows of the output matched the initial robot-description values to ≤ 1.2 µm / ≤ 0.061 mdeg (the cancellation identity), while the optimized poses stored in the database differed from the initial values by 0.7–2.6 deg / 11–41 cm.
  • After the fix: the camera rows matched the optimized poses stored in the database to ≤ 0.007 mm / ≤ 0.0004 deg (output rounding), and the manager log showed the new "Cached the constant tfs used by post_process" line. The lidar rows and all BA scores were unchanged, confirming the fix only affects the output stage.
• The three upstream calibrators in this PR were ported following the same pattern. They compile as Python (ast), and pass black/isort with the repository settings. I do not have access to rdv/x2/xx1_15 hardware, so the upstream files could not be tested end-to-end — the x2/rdv lidar/kit cancellation paths are derived algebraically from the same identity.

Notes for reviewers

• The caching moment is structurally safe: on_check_tf_timer stops as soon as the tfs become ready, which happens while the UI is still initializing, and data_->optimized_sensor_poses_map is necessarily empty at that point — so the cached values can never be contaminated by a previous run of the same node.
• Alternatives considered:
  • Broadcasting the visualization tfs under suffixed frame names would also fix the issue, but changes the RViz frame layout for every user.
  • Snapshotting the whole tf tree at calibration start inside CalibratorBase would fix all projects at once, but touches every calibrator; the per-calibrator caching keeps the diff minimal and local to the three affected files.
• default_project is intentionally untouched (it has no post_process and returns the raw BA results, which are correct).

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.