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

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

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,31 +94,65 @@ 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
             )
             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]]):
+        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

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