feat: enable interactive calibrator to specify file intrinsics/extrinsics on runtime

calibrators/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# Copyright 2024 TIER IV, Inc.
+# Copyright 2024-2025 TIER IV, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -14,7 +14,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import json
 import logging
 import os
 import signal
@@ -40,8 +39,22 @@
 from interactive_camera_lidar_calibrator.utils import camera_lidar_calibrate_intrinsics
 import numpy as np
 import rclpy
-from rosidl_runtime_py.convert import message_to_ordereddict
 from tier4_calibration_views.image_view_ui import ImageViewUI
+import transforms3d
+import yaml
+
+
+def float_representer(dumper, value):
+    text = f"{round(value, 6)}"  # noqa E231
+    return dumper.represent_scalar("tag:yaml.org,2002:float", text)  # noqa E231
+
+
+def list_representer(dumper, value):
+    return dumper.represent_sequence("tag:yaml.org,2002:seq", value, flow_style=True)
+
+
+yaml.add_representer(float, float_representer)
+yaml.add_representer(list, list_representer)
 
 
 class InteractiveCalibratorUI(ImageViewUI):
@@ -133,30 +146,30 @@ def calibration_api_button_callback():
         calibration_status_helper_label.setText("(calibration vs. tf source)")
 
         self.calibration_status_points_label = QLabel()
-        self.calibration_status_points_label.setText("#points: ")
+        self.calibration_status_points_label.setText("# pairs: ")
         self.calibration_status_points_label.setAlignment(Qt.AlignTop | Qt.AlignLeft)
 
         self.calibration_status_error_label = QLabel()
-        self.calibration_status_error_label.setText("r.error: ")
+        self.calibration_status_error_label.setText("R.error: ")
         self.calibration_status_error_label.setAlignment(Qt.AlignTop | Qt.AlignLeft)
 
         self.calibration_status_inliers_label = QLabel()
         self.calibration_status_inliers_label.setText("inliers: ")
         self.calibration_status_inliers_label.setAlignment(Qt.AlignTop | Qt.AlignLeft)
 
         self.state_1_message = (
-            "To add a calibration pair\nfirst click the 3d point."
-            + "\nTo delete a calibration\npoint, click it in the\nimage"
+            "To add a calibration pair,\nfirst click the 3d point."
+            + "\nTo delete a calibration\npair, click it in the image."
         )
 
         user_message_label = QLabel()
         user_message_label.setText(self.state_1_message)
         user_message_label.setAlignment(Qt.AlignTop | Qt.AlignLeft)
 
-        save_calibration_button = QPushButton("Save calibration")
+        save_calibration_button = QPushButton("Save pairs and result")
         save_calibration_button.clicked.connect(self.save_calibration_callback)
 
-        load_calibration_button = QPushButton("Load calibration")
+        load_calibration_button = QPushButton("Load pairs")
         load_calibration_button.clicked.connect(self.load_calibration_callback)
 
         calibration_status_layout = QVBoxLayout()
@@ -179,12 +192,25 @@ def make_calibration_options(self):
         self.calibration_button.setEnabled(False)
 
         def calibration_intrinsics_callback():
+            is_init = self.optimized_camera_info is None
             self.optimized_camera_info = camera_lidar_calibrate_intrinsics(
                 np.array(self.object_calibration_points + self.external_object_calibration_points),
                 np.array(self.image_calibration_points + self.external_image_calibration_points),
-                self.camera_info,
+                self.source_camera_info,
+            )
+
+            self.ros_interface.get_logger().info(
+                f"Optimized camera intrinsics obtained: K = {self.optimized_camera_info.k}, D = {self.optimized_camera_info.d}"
             )
-            self.use_optimized_intrinsics_checkbox.setEnabled(True)
+
+            if is_init:
+                self.camera_info_source_combobox.insertItem(
+                    0, "Calibrator", "calibrator"
+                )  # prepend
+
+            # Force update
+            if self.camera_info_source_combobox.currentData() == "calibrator":
+                self.camera_info_source_callback("calibrator")
 
         self.calibration2_button = QPushButton("Calibrate intrinsics\n(experimental)")
         self.calibration2_button.clicked.connect(calibration_intrinsics_callback)
@@ -196,7 +222,7 @@ def calibration_intrinsics_callback():
         self.pnp_method_combobox.addItem("SQPNP")
 
         def use_ransac_callback(value):
-            if self.camera_info is None:
+            if self.source_camera_info is None:
                 return
 
             if value == Qt.Checked:
@@ -210,25 +236,6 @@ def use_ransac_callback(value):
         self.use_ransac_checkbox.stateChanged.connect(use_ransac_callback)
         self.use_ransac_checkbox.setChecked(False)
 
-        def use_optimized_intrinsics_callback(state):
-            if state == Qt.Checked:
-                self.image_view.set_camera_info(
-                    self.optimized_camera_info.k, self.optimized_camera_info.d
-                )
-                self.calibrator.set_camera_info(
-                    self.optimized_camera_info.k, self.optimized_camera_info.d
-                )
-            else:
-                self.image_view.set_camera_info(self.camera_info.k, self.camera_info.d)
-                self.calibrator.set_camera_info(self.camera_info.k, self.camera_info.d)
-
-        self.use_optimized_intrinsics_checkbox = QCheckBox("Use optimized Intrinsics")
-        self.use_optimized_intrinsics_checkbox.stateChanged.connect(
-            use_optimized_intrinsics_callback
-        )
-        self.use_optimized_intrinsics_checkbox.setEnabled(False)
-        self.use_optimized_intrinsics_checkbox.setChecked(False)
-
         def pnp_min_points_callback():
             self.calibration_possible = (
                 len(self.image_calibration_points) + len(self.external_image_calibration_points)
@@ -238,7 +245,7 @@ def pnp_min_points_callback():
             self.calibration_button.setEnabled(self.calibration_possible)
             self.calibration2_button.setEnabled(self.calibration_possible)
 
-        pnp_min_points_label = QLabel("Minimum pnp\n points")
+        pnp_min_points_label = QLabel("Minimum pnp pairs")
         self.pnp_min_points_spinbox = QSpinBox()
         self.pnp_min_points_spinbox.valueChanged.connect(pnp_min_points_callback)
         self.pnp_min_points_spinbox.setRange(4, 100)
@@ -248,7 +255,7 @@ def pnp_min_points_callback():
         def ransac_inlier_error_callback(value):
             self.image_view.set_inlier_distance(value)
 
-        ransac_inlier_error_label = QLabel("RANSAC inlier\nerror (px)")
+        ransac_inlier_error_label = QLabel("RANSAC inlier error (px)")
         self.ransac_inlier_error_spinbox = QDoubleSpinBox()
         self.ransac_inlier_error_spinbox.valueChanged.connect(ransac_inlier_error_callback)
         self.ransac_inlier_error_spinbox.setRange(0.0, 1000.0)
@@ -261,7 +268,6 @@ def ransac_inlier_error_callback(value):
         calibration_options_layout.addWidget(pnp_method_label)
         calibration_options_layout.addWidget(self.pnp_method_combobox)
         calibration_options_layout.addWidget(self.use_ransac_checkbox)
-        calibration_options_layout.addWidget(self.use_optimized_intrinsics_checkbox)
         calibration_options_layout.addWidget(pnp_min_points_label)
         calibration_options_layout.addWidget(self.pnp_min_points_spinbox)
         calibration_options_layout.addWidget(ransac_inlier_error_label)
@@ -320,15 +326,19 @@ def screenshot_callback():
         data_collection_options_layout.addStretch(1)
         self.data_collection_options_group.setLayout(data_collection_options_layout)
 
-    def sensor_data_callback(self):
-        super().sensor_data_callback()
-        with self.lock:
-            self.calibrator.set_camera_info(self.camera_info_tmp.k, self.camera_info_tmp.d)
-
-    def tf_source_callback(self, string):
-        super().tf_source_callback(string)
+    def tf_source_callback(self, source):
+        super().tf_source_callback(source)
         self.update_calibration_status()
 
+    def camera_info_source_callback(self, source):
+        super().camera_info_source_callback(source)
+        with self.lock:
+            if self.source_camera_info is not None:
+                self.calibrator.set_camera_info(
+                    self.source_camera_info.k,
+                    self.source_camera_info.d,
+                )
+
     def save_calibration_callback(self):
         output_folder = QFileDialog.getExistingDirectory(
             None,
@@ -361,12 +371,94 @@ def save_calibration_callback(self):
         np.savetxt(os.path.join(output_folder, "image_points.txt"), image_points)
 
         if self.optimized_camera_info is not None:
-            d = message_to_ordereddict(self.optimized_camera_info)
-
-            with open(os.path.join(output_folder, "optimized_camera_info.json"), "w") as f:
-                f.write(json.dumps(d, indent=4, sort_keys=False))
+            d = {}
+            d["image_width"] = self.optimized_camera_info.width
+            d["image_height"] = self.optimized_camera_info.height
+            d["camera_name"] = self.ros_interface.camera_name
+            d["camera_matrix"] = {
+                "rows": 3,
+                "cols": 3,
+                "data": np.array(self.optimized_camera_info.k).reshape(-1).tolist(),
+            }
+            d["distortion_model"] = self.optimized_camera_info.distortion_model
+            d["distortion_coefficients"] = {
+                "rows": 1,
+                "cols": len(self.optimized_camera_info.d),
+                "data": np.array(self.optimized_camera_info.d).reshape(-1).tolist(),
+            }
+            d["projection_matrix"] = {
+                "rows": 3,
+                "cols": 4,
+                "data": np.array(self.optimized_camera_info.p).reshape(-1).tolist(),
+            }
+            d["rectification_matrix"] = {
+                "rows": 3,
+                "cols": 3,
+                "data": np.array(self.optimized_camera_info.r).reshape(-1).tolist(),
+            }
+
+            with open(os.path.join(output_folder, "optimized_camera_info.yaml"), "w") as f:
+                yaml.dump(d, f, sort_keys=False)
+
+        # save calibrated extrinsics
+        assert self.calibrated_transform is not None
+
+        use_rpy = True
+
+        calibrated_tf = {
+            "x": self.calibrated_transform[0, 3].item(),
+            "y": self.calibrated_transform[1, 3].item(),
+            "z": self.calibrated_transform[2, 3].item(),
+        }
+        if use_rpy:
+            rpy = transforms3d.euler.mat2euler(self.calibrated_transform[0:3, 0:3])
+            calibrated_tf["roll"] = rpy[0]
+            calibrated_tf["pitch"] = rpy[1]
+            calibrated_tf["yaw"] = rpy[2]
+        else:
+            quat = transforms3d.quaternions.mat2quat(self.calibrated_transform[0:3, 0:3])
+            calibrated_tf["qx"] = quat[1]
+            calibrated_tf["qy"] = quat[2]
+            calibrated_tf["qz"] = quat[3]
+            calibrated_tf["qw"] = quat[0]
+
+        calibrated_d = {
+            self.ros_interface.image_frame: {self.ros_interface.lidar_frame: calibrated_tf}
+        }
+        with open(os.path.join(output_folder, "tf.yaml"), "w") as f:
+            yaml.dump(calibrated_d, f, sort_keys=False)
+
+        # postprocess the calibrated transform
+        try:
+            postprocessed_transform = self.ros_interface.get_parent_to_child_transform(
+                self.calibrated_transform
+            )
 
-        self.ros_interface.save_calibration_tfs(output_folder)
+            postprocessed_tf = {
+                "x": postprocessed_transform[0, 3].item(),
+                "y": postprocessed_transform[1, 3].item(),
+                "z": postprocessed_transform[2, 3].item(),
+            }
+            if use_rpy:
+                rpy = transforms3d.euler.mat2euler(postprocessed_transform[0:3, 0:3])
+                postprocessed_tf["roll"] = rpy[0]
+                postprocessed_tf["pitch"] = rpy[1]
+                postprocessed_tf["yaw"] = rpy[2]
+            else:
+                quat = transforms3d.quaternions.mat2quat(postprocessed_transform[0:3, 0:3])
+                postprocessed_tf["qx"] = quat[1]
+                postprocessed_tf["qy"] = quat[2]
+                postprocessed_tf["qz"] = quat[3]
+                postprocessed_tf["qw"] = quat[0]
+
+            postprocessed_d = {
+                self.ros_interface.parent_frame: {self.ros_interface.child_frame: postprocessed_tf}
+            }
+            with open(os.path.join(output_folder, "tf_postprocessed.yaml"), "w") as f:
+                yaml.dump(postprocessed_d, f, sort_keys=False)
+        except Exception as ex:
+            self.ros_interface.get_logger().error(f"Could not save postprocessed TF. {ex}")
+            return
 
     def load_calibration_callback(self):
         input_dir = QFileDialog.getExistingDirectory(
@@ -409,7 +501,7 @@ def load_calibration_callback(self):
         )
 
     def calibration_callback(self):
-        if self.camera_info is None:
+        if self.source_camera_info is None:
             return
 
         # Configure the calibrator
@@ -435,15 +527,19 @@ def calibration_callback(self):
             return
 
         if self.calibrated_transform is None:
-            self.tf_source_combobox.addItem("Calibrator")
+            self.tf_source_combobox.insertItem(0, "Calibrator", "calibrator")  # prepend
 
         self.calibrated_transform = transform
 
-        self.tf_source_callback(self.tf_source_combobox.currentText())
+        # Force update
+        self.update_calibration_status()
+        if self.tf_source_combobox.currentData() == "calibrator":
+            self.tf_source_callback("calibrator")
 
         self.calibration_api_button.setEnabled(
             self.calibration_api_request_received and self.calibrated_transform is not None
         )
+        # Pass tf so that ROS interface can publish if allowed.
         self.ros_interface.set_camera_lidar_transform(transform)
 
     def update_calibration_status(self):
@@ -482,7 +578,7 @@ def update_calibration_status(self):
             source_error_string = ""
             source_inliers = np.array([])
 
-        self.calibration_status_points_label.setText(f"#points: {len(object_calibration_points)}")
+        self.calibration_status_points_label.setText(f"# pairs: {len(object_calibration_points)}")
         self.calibration_status_error_label.setText(
             f"R.error: {calibrated_error_string} / {source_error_string}"
         )

calibrators/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# Copyright 2024 TIER IV, Inc.
+# Copyright 2024-2025 TIER IV, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -14,8 +14,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import json
-import os
 import threading
 import time
 
@@ -25,7 +23,6 @@
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.duration import Duration
 from rclpy.qos import qos_profile_system_default
-from rosidl_runtime_py.convert import message_to_ordereddict
 from tf2_ros import TransformException
 from tier4_calibration_views.image_view_ros_interface import ImageViewRosInterface
 from tier4_calibration_views.utils import decompose_transformation_matrix
@@ -34,7 +31,6 @@
 from tier4_calibration_views.utils import transform_points
 from tier4_sensor_calibration_msgs.msg import CalibrationResult
 from tier4_sensor_calibration_msgs.srv import ExtrinsicCalibrator
-import transforms3d
 
 
 class InteractiveCalibratorRosInterface(ImageViewRosInterface):
@@ -138,20 +134,6 @@ def set_camera_lidar_transform(self, camera_optical_lidar_transform):
             self.output_transform_msg.child_frame_id = self.lidar_frame
             self.new_output_tf = True
 
-    def save_calibration_tfs(self, output_dir):
-        with self.lock:
-            d = message_to_ordereddict(self.output_transform_msg)
-
-            q = self.output_transform_msg.transform.rotation
-            e = transforms3d.euler.quat2euler((q.w, q.x, q.y, q.z))
-
-            d["roll"] = e[0]
-            d["pitch"] = e[1]
-            d["yaw"] = e[2]
-
-            with open(os.path.join(output_dir, "tf.json"), "w") as f:
-                f.write(json.dumps(d, indent=4, sort_keys=False))
-
     def point_callback(self, point: PointStamped):
         point_xyz = np.array([point.point.x, point.point.y, point.point.z]).reshape(1, 3)