feat(edge_auto): added initial support for edge auto calibration

sensor_calibration_manager/launch/edge_auto/tag_based_pnp_calibrator.launch.xml

@@ -0,0 +1,91 @@
+<launch>
+  <arg name="camera_name" default="camera0">
+    <choice value="camera0"/>
+    <choice value="camera1"/>
+  </arg>
+
+  <arg name="lidar_model" default="at128">
+    <choice value="at128"/>
+    <choice value="pandar_xt32"/>
+  </arg>
+
+  <arg name="view_only_ui" default="true" description="By default we use a minimal UI">
+    <choice value="true"/>
+    <choice value="false"/>
+  </arg>
+  <arg name="use_receive_time" default="false" description="Use receive time instead of topic timestamps to make corresponding pairs">
+    <choice value="false"/>
+    <choice value="true"/>
+  </arg>
+  <arg name="calibration_pairs" default="9" description="Number of lidar-image pairs for the calibration to converge"/>
+  <arg name="calibration_pairs_min_distance" default="1.5" description="Minimum allowed between a new detection and current pairs"/>
+
+  <!-- we do not use the standard image_raw name to avoid naming conflicts -->
+  <let name="image_decompressed_topic" value="/sensing/camera/$(var camera_name)/image_raw/decompressed"/>
+  <let name="image_compressed_topic" value="/sensing/camera/$(var camera_name)/image_raw/compressed"/>
+
+  <let name="camera_info_topic" value="/sensing/camera/$(var camera_name)/camera_info"/>
+  <let name="pointcloud_topic" value="/sensing/lidar/xt32/aw_points"/>
+
+  <let name="calibrate_sensor" value="false"/>
+  <let name="calibrate_sensor" value="true" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
+  <let name="calibrate_sensor" value="true" if="$(eval &quot;'$(var camera_name)' == 'camera1' &quot;)"/>
+
+  <let name="camera_frame" value="$(var camera_name)/camera_link"/>
+  <let name="rviz_profile" value="$(find-pkg-share tag_based_pnp_calibrator)/rviz/default_profile.rviz"/>
+  <let name="lidar_frame" value="lidar"/>
+  <let name="use_rectified_image" value="false"/>
+
+  <group if="$(var calibrate_sensor)">
+    <!-- image decompressor -->
+    <node pkg="autoware_image_transport_decompressor" exec="image_transport_decompressor_node" name="decompressor" output="screen">
+      <remap from="decompressor/input/compressed_image" to="$(var image_compressed_topic)"/>
+      <remap from="decompressor/output/raw_image" to="$(var image_decompressed_topic)"/>
+
+      <param name="encoding" value="default"/>
+    </node>
+
+    <!-- tag based calibrator -->
+    <include file="$(find-pkg-share tag_based_pnp_calibrator)/launch/calibrator.launch.xml">
+      <arg name="image_topic" value="$(var image_decompressed_topic)"/>
+      <arg name="camera_info_topic" value="$(var camera_info_topic)"/>
+      <arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
+      <arg name="pointcloud_topic_ex" value="$(var camera_info_topic)"/>
+      <arg name="lidar_model" value="$(var lidar_model)"/>
+      <arg name="calibration_service_name" value="calibrate_camera_lidar"/>
+
+      <arg name="use_rectified_image" value="$(var use_rectified_image)"/>
+      <arg name="calibration_pairs" value="$(var calibration_pairs)"/>
+      <arg name="calibration_pairs_min_distance" value="$(var calibration_pairs_min_distance)"/>
+      <arg name="use_receive_time" value="$(var use_receive_time)"/>
+    </include>
+
+    <!-- interactive calibrator -->
+    <node pkg="interactive_camera_lidar_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen" if="$(eval &quot;'$(var view_only_ui)' == 'false' &quot;)">
+      <remap from="pointcloud" to="$(var pointcloud_topic)"/>
+      <remap from="image" to="$(var image_compressed_topic)"/>
+      <remap from="camera_info" to="$(var camera_info_topic)"/>
+      <remap from="calibration_points_input" to="calibration_points"/>
+
+      <param name="camera_frame" value="$(var camera_frame)"/>
+      <param name="use_calibration_api" value="false"/>
+      <param name="can_publish_tf" value="false"/>
+    </node>
+
+    <!-- camera view -->
+    <node pkg="tier4_calibration_views" exec="image_view_node.py" name="image_view_node_py" output="screen" if="$(eval &quot;'$(var view_only_ui)' == 'true' &quot;)">
+      <remap from="pointcloud" to="$(var pointcloud_topic)"/>
+      <remap from="image" to="$(var image_compressed_topic)"/>
+      <remap from="camera_info" to="$(var camera_info_topic)"/>
+      <remap from="calibration_points_input" to="calibration_points"/>
+    </node>
+
+    <!-- create a placeholder lidar frame to make the rviz profile generic -->
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="0 0 0 0 0 0 $(var lidar_frame) lidar_frame"/>
+
+    <!-- remap the pointcloud topic to make the rviz profile generic -->
+    <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)">
+      <remap from="pointcloud_topic_placeholder" to="$(var pointcloud_topic)"/>
+    </node>
+  </group>
+</launch>

sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py

@@ -1,4 +1,5 @@
 from .default_project import *  # noqa: F401, F403
+from .edge_auto import *  # noqa: F401, F403
 from .rdv import *  # noqa: F401, F403
 from .x1 import *  # noqa: F401, F403
 from .x2 import *  # noqa: F401, F403

sensor_calibration_manager/sensor_calibration_manager/calibrators/edge_auto/__init__.py

@@ -0,0 +1,5 @@
+from .tag_based_pnp_calibrator import TagBasedPNPCalibrator
+
+__all__ = [
+    "TagBasedPNPCalibrator",
+]

sensor_calibration_manager/sensor_calibration_manager/calibrators/edge_auto/tag_based_pnp_calibrator.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 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.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Dict
+
+import numpy as np
+
+from sensor_calibration_manager.calibrator_base import CalibratorBase
+from sensor_calibration_manager.calibrator_registry import CalibratorRegistry
+from sensor_calibration_manager.ros_interface import RosInterface
+from sensor_calibration_manager.types import FramePair
+
+
+@CalibratorRegistry.register_calibrator(
+    project_name="edge_auto", calibrator_name="tag_based_pnp_calibrator"
+)
+class TagBasedPNPCalibrator(CalibratorBase):
+    required_frames = ["base_link"]
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.camera_name = kwargs["camera_name"]
+        self.lidar_frame = kwargs["lidar_frame"]
+        self.required_frames.append(f"{self.camera_name}/camera_link")
+        self.required_frames.append(f"{self.camera_name}/camera_optical_link")
+        self.required_frames.append(self.lidar_frame)
+
+        self.add_calibrator(
+            service_name="calibrate_camera_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=f"{self.camera_name}/camera_optical_link", child=self.lidar_frame),
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        optical_link_to_lidar_transform = calibration_transforms[
+            f"{self.camera_name}/camera_optical_link"
+        ][self.lidar_frame]
+
+        camera_to_optical_link_transform = self.get_transform_matrix(
+            f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link"
+        )
+
+        lidar_to_camera_link_transform = np.linalg.inv(
+            camera_to_optical_link_transform @ optical_link_to_lidar_transform
+        )
+
+        result = {
+            self.lidar_frame: {f"{self.camera_name}/camera_link": lidar_to_camera_link_transform}
+        }
+        return result