From d8aaca910e7ffed03d67e76dcd955b5ef736a76b Mon Sep 17 00:00:00 2001 From: abailinrun Date: Thu, 23 Jul 2026 23:25:35 +0900 Subject: [PATCH] fix(sensor_calibration_manager): cache the constant tfs used by the tag-based SfM post_process The tag_based_sfm_calibrator broadcasts the optimized sensor poses (main sensor -> calibration frames) once the calibration finishes, for visualization purposes. Since tf2 only allows a single parent per frame, these broadcasts re-parent the calibration frames and shadow the constant transforms defined in the robot description. 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 queries resolve through the broadcast tfs and return values containing the inverse of the optimized poses. When composed with the calibration results, the optimized poses cancel out exactly and the outputs degenerate to a rigid carry-over of the initial (robot description) values, silently discarding the BA solutions for the cameras (and for the front/rear kits on x2 and the lidar base links on rdv). Cache the constant transforms as soon as the required tfs become available (before any calibration result can be broadcast) and use the cached values in post_process. Signed-off-by: abailinrun --- ...ased_sfm_base_lidars_cameras_calibrator.py | 46 ++++++++++++-- ...ased_sfm_base_lidars_cameras_calibrator.py | 60 +++++++++++++++---- ...ased_sfm_base_lidars_cameras_calibrator.py | 40 +++++++++++-- 3 files changed, 124 insertions(+), 22 deletions(-) diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py index b32065ac..e08055ac 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py +++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -14,7 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging from typing import Dict +from typing import List +from typing import Optional import numpy as np @@ -87,24 +90,55 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ], ) - def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.cached_constant_transforms = False + self.cached_sensor_kit_to_main_sensor_transform: Optional[np.array] = None + self.cached_lidar_to_lidar_base_transforms: Optional[List[np.array]] = None + self.cached_optical_link_to_camera_link_transforms: Optional[List[np.array]] = None + + def on_check_tf_timer(self): + super().on_check_tf_timer() + + if self.tfs_ready and not self.cached_constant_transforms: + self.cache_constant_transforms() + + def cache_constant_transforms(self): + """Cache the constant tfs needed by `post_process` before calibrating. + + `post_process` runs after the calibration has finished, at which point the + calibrator node is already broadcasting the optimized sensor poses for + visualization purposes. Since tf2 only allows a single parent per frame, those + broadcasts re-parent the calibration frames, and any query whose path traverses + one of them returns a value containing the inverse of the optimized poses, + which cancels out the calibration results during `post_process`. To avoid this, + the constant tfs are cached here, as soon as they become available and before + any calibration result can be broadcast. + """ + self.cached_sensor_kit_to_main_sensor_transform = self.get_transform_matrix( self.sensor_kit_frame, self.main_sensor_frame ) - - lidar_to_lidar_base_transforms = [ + self.cached_lidar_to_lidar_base_transforms = [ self.get_transform_matrix(lidar_frame, lidar_base_frame) for lidar_frame, lidar_base_frame in zip( self.calibration_lidar_frames, self.calibration_lidar_base_frames ) ] - - optical_link_to_camera_link_transforms = [ + self.cached_optical_link_to_camera_link_transforms = [ self.get_transform_matrix(camera_optical_link_frame, camera_link_frame) for camera_optical_link_frame, camera_link_frame in zip( self.calibration_camera_optical_link_frames, self.calibration_camera_link_frames ) ] + self.cached_constant_transforms = True + logging.info("Cached the constant tfs used by post_process") + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + if not self.cached_constant_transforms: + logging.warning("The constant tfs were not cached. Falling back to a live query") + self.cache_constant_transforms() + + sensor_kit_to_mapping_lidar_transform = self.cached_sensor_kit_to_main_sensor_transform + lidar_to_lidar_base_transforms = self.cached_lidar_to_lidar_base_transforms + optical_link_to_camera_link_transforms = self.cached_optical_link_to_camera_link_transforms base_to_top_sensor_kit_transform = np.linalg.inv( sensor_kit_to_mapping_lidar_transform diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py index 1d3b51a2..eb7c5863 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py +++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -15,7 +15,9 @@ # limitations under the License. from collections import defaultdict +import logging from typing import Dict +from typing import Optional import numpy as np @@ -92,24 +94,40 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ], ) - def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - main_sensor_to_base_transform = calibration_transforms[self.main_sensor_frame][ - self.base_frame - ] - - top_kit_to_main_lidar_transform = self.get_transform_matrix( + self.cached_constant_transforms = False + self.cached_top_kit_to_main_lidar_transform: Optional[np.array] = None + self.cached_front_kit_to_front_lower_lidar_transform: Optional[np.array] = None + self.cached_rear_kit_to_rear_lower_lidar_transform: Optional[np.array] = None + self.cached_optical_link_to_camera_link_transforms: Optional[Dict[str, np.array]] = None + + def on_check_tf_timer(self): + super().on_check_tf_timer() + + if self.tfs_ready and not self.cached_constant_transforms: + self.cache_constant_transforms() + + def cache_constant_transforms(self): + """Cache the constant tfs needed by `post_process` before calibrating. + + `post_process` runs after the calibration has finished, at which point the + calibrator node is already broadcasting the optimized sensor poses for + visualization purposes. Since tf2 only allows a single parent per frame, those + broadcasts re-parent the calibration frames, and any query whose path traverses + one of them returns a value containing the inverse of the optimized poses, + which cancels out the calibration results during `post_process`. To avoid this, + the constant tfs are cached here, as soon as they become available and before + any calibration result can be broadcast. + """ + self.cached_top_kit_to_main_lidar_transform = self.get_transform_matrix( self.top_unit_frame, self.main_sensor_frame ) - - front_kit_to_front_lower_lidar_transform = self.get_transform_matrix( + self.cached_front_kit_to_front_lower_lidar_transform = self.get_transform_matrix( self.front_unit_frame, "pandar_40p_front" ) - - rear_kit_to_rear_lower_lidar_transform = self.get_transform_matrix( + self.cached_rear_kit_to_rear_lower_lidar_transform = self.get_transform_matrix( self.rear_unit_frame, "pandar_40p_rear" ) - - optical_link_to_camera_link_transforms = { + self.cached_optical_link_to_camera_link_transforms = { camera_optical_link_frame: self.get_transform_matrix( camera_optical_link_frame, camera_link_frame ) @@ -117,6 +135,24 @@ def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): self.calibration_camera_optical_link_frames, self.calibration_camera_link_frames ) } + self.cached_constant_transforms = True + logging.info("Cached the constant tfs used by post_process") + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + main_sensor_to_base_transform = calibration_transforms[self.main_sensor_frame][ + self.base_frame + ] + + if not self.cached_constant_transforms: + logging.warning("The constant tfs were not cached. Falling back to a live query") + self.cache_constant_transforms() + + top_kit_to_main_lidar_transform = self.cached_top_kit_to_main_lidar_transform + front_kit_to_front_lower_lidar_transform = ( + self.cached_front_kit_to_front_lower_lidar_transform + ) + rear_kit_to_rear_lower_lidar_transform = self.cached_rear_kit_to_rear_lower_lidar_transform + optical_link_to_camera_link_transforms = self.cached_optical_link_to_camera_link_transforms base_to_top_kit_transform = np.linalg.inv( top_kit_to_main_lidar_transform @ main_sensor_to_base_transform diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py index fdd2e329..4e39d667 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py +++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -15,8 +15,10 @@ # limitations under the License. +import logging from typing import Dict from typing import List +from typing import Optional import numpy as np @@ -75,17 +77,47 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ], ) - def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.cached_constant_transforms = False + self.cached_sensor_kit_to_main_sensor_transform: Optional[np.array] = None + self.cached_optical_link_to_camera_link_transforms: Optional[List[np.array]] = None + + def on_check_tf_timer(self): + super().on_check_tf_timer() + + if self.tfs_ready and not self.cached_constant_transforms: + self.cache_constant_transforms() + + def cache_constant_transforms(self): + """Cache the constant tfs needed by `post_process` before calibrating. + + `post_process` runs after the calibration has finished, at which point the + calibrator node is already broadcasting the optimized sensor poses for + visualization purposes. Since tf2 only allows a single parent per frame, those + broadcasts re-parent the calibration frames, and any query whose path traverses + one of them returns a value containing the inverse of the optimized poses, + which cancels out the calibration results during `post_process`. To avoid this, + the constant tfs are cached here, as soon as they become available and before + any calibration result can be broadcast. + """ + self.cached_sensor_kit_to_main_sensor_transform = self.get_transform_matrix( self.sensor_kit_frame, self.main_sensor_frame ) - - optical_link_to_camera_link_transforms = [ + self.cached_optical_link_to_camera_link_transforms = [ self.get_transform_matrix(camera_optical_link_frame, camera_link_frame) for camera_optical_link_frame, camera_link_frame in zip( self.calibration_camera_optical_link_frames, self.calibration_camera_link_frames ) ] + self.cached_constant_transforms = True + logging.info("Cached the constant tfs used by post_process") + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + if not self.cached_constant_transforms: + logging.warning("The constant tfs were not cached. Falling back to a live query") + self.cache_constant_transforms() + + sensor_kit_to_mapping_lidar_transform = self.cached_sensor_kit_to_main_sensor_transform + optical_link_to_camera_link_transforms = self.cached_optical_link_to_camera_link_transforms base_to_top_sensor_kit_transform = np.linalg.inv( sensor_kit_to_mapping_lidar_transform