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