feat: interactive calibrator - new api

calibrators/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py

@@ -103,7 +103,7 @@ def calibration_api_service_callback(
             result = CalibrationResult()
             result.success = True
             result.score = self.calibration_error
-            result.message = "The score corresponds to the reprojection error"
+            result.message.data = "The score corresponds to the reprojection error"
             result.transform_stamped = self.output_transform_msg
             result.transform_stamped.header.frame_id = self.image_frame
             result.transform_stamped.child_frame_id = self.lidar_frame

calibrators/interactive_camera_lidar_calibrator/rviz/default_profile.rviz

@@ -0,0 +1,176 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /pointcloud1/Topic1
+        - /Axes1
+      Splitter Ratio: 0.5
+    Tree Height: 1526
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: pointcloud
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: pointcloud
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /sensing/lidar/concatenated/pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: lidar_frame
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 4.908139228820801
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.6476134061813354
+        Y: 0.20366430282592773
+        Z: 1.1267125606536865
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5153982639312744
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.1603987216949463
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 2032
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f7000006eefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006ee0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000006eefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000006ee0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e740000005afc0100000002fb0000000800540069006d0065010000000000000e740000058100fffffffb0000000800540069006d0065010000000000000450000000000000000000000b06000006ee00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 3700
+  X: 1220
+  Y: 54

calibrators/interactive_camera_lidar_calibrator/setup.py

@@ -1,3 +1,5 @@
+from glob import glob
+
 from setuptools import setup
 
 package_name = "interactive_camera_lidar_calibrator"
@@ -9,6 +11,7 @@
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/rviz", glob("rviz/*")),
     ],
     install_requires=["setuptools"],
     zip_safe=True,

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py

@@ -136,7 +136,7 @@ def send_data(self, topic, data):
         # cSpell:enableCompoundWords
         # cSpell:ignore imgmsg
         if isinstance(msg, Image):
-            image_data = self.bridge.imgmsg_to_cv2(msg)
+            image_data = self.bridge.imgmsg_to_cv2(msg, "bgr8")
             stamp = msg.header.stamp.sec + 1e-9 * msg.header.stamp.nanosec
             self.data_callback(image_data, stamp)
 

common/tier4_calibration_views/tier4_calibration_views/image_view_ros_interface.py

@@ -14,6 +14,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import array
 from collections import deque
 import threading
 from typing import Callable
@@ -142,14 +143,19 @@ def camera_info_callback(self, camera_info_msg: CameraInfo):
         self.image_frame = camera_info_msg.header.frame_id
 
         if self.use_rectified:
+
             self.camera_info.k[0] = self.camera_info.p[0]
             self.camera_info.k[2] = self.camera_info.p[2]
             self.camera_info.k[4] = self.camera_info.p[5]
             self.camera_info.k[5] = self.camera_info.p[6]
-            self.camera_info.d = 0.0 * self.camera_info.d
+            self.camera_info.d = array.array("d", 0.0 * np.array(self.camera_info.d))
 
     def check_sync(self):
-        if len(self.image_queue) == 0 or len(self.pointcloud_queue) == 0:
+        if (
+            len(self.image_queue) == 0
+            or len(self.pointcloud_queue) == 0
+            or self.camera_info is None
+        ):
             return
 
         min_delay = np.inf
@@ -191,7 +197,7 @@ def check_sync(self):
                 image_data = np.frombuffer(self.image_sync.data, np.uint8)
                 self.image_sync = cv2.imdecode(image_data, cv2.IMREAD_COLOR)
             else:
-                self.image_sync = self.bridge.imgmsg_to_cv2(self.image_sync)
+                self.image_sync = self.bridge.imgmsg_to_cv2(self.image_sync, "bgr8")
                 # image = cv2.cvtColor(self.raw_image, cv2.COLOR_BGR2RGB)
 
             self.sensor_data_callback(self.image_sync, self.camera_info_sync, points_np, min_delay)

sensor_calibration_manager/launch/default_project/interactive_camera_lidar_calibrator.launch.xml

@@ -0,0 +1,39 @@
+<launch>
+  <arg name="camera_optical_link_frame" description="E.g., camera0/camera_optical_link"/>
+  <!-- this is needed for the calibrator interface - transformation algebra-->
+  <arg name="lidar_frame" description="E.g., velodyne_top. this is used for the extrinsic_calibration_manager under the hood"/>
+  <arg name="image_topic" default="/sensing/camera/camera0/image_rect_color/decompressed" description="Internal topic to decompress the image"/>
+  <arg name="camera_info_topic" default="/sensing/camera/camera0/camera_info"/>
+  <arg name="pointcloud_topic" default="/sensing/lidar/top/pointcloud_raw"/>
+
+  <arg name="use_rectified_image" default="false" description="image_rect vs. raw"/>
+  <arg name="use_compressed_image" default="false" description=""/>
+  <arg name="timer_period" default="1.0" description="Calibration timer period in seconds"/>
+  <arg name="delay_tolerance" default="0.06" description="Maximum time difference allowed between sensors in seconds"/>
+
+  <let name="rviz_profile" value=""/>
+  <let name="rviz_profile" value="$(find-pkg-share interactive_camera_lidar_calibrator)/rviz/default_profile.rviz"/>
+
+  <!-- interactive calibrator -->
+  <node pkg="interactive_camera_lidar_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen">
+    <remap from="pointcloud" to="$(var pointcloud_topic)"/>
+    <remap from="image" to="$(var image_topic)"/>
+    <remap from="camera_info" to="$(var camera_info_topic)"/>
+    <remap from="extrinsic_calibration" to="calibrate_camera_lidar"/>
+
+    <param name="use_rectified" value="$(var use_rectified_image)"/>
+    <param name="use_compressed" value="$(var use_compressed_image)"/>
+    <param name="timer_period" value="$(var timer_period)"/>
+    <param name="delay_tolerance" value="$(var delay_tolerance)"/>
+
+    <param name="use_calibration_api" value="true"/>
+    <param name="can_publish_tf" value="false"/>
+  </node>
+
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="dummy_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>
+</launch>

sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py

@@ -1,4 +1,5 @@
 from .ground_plane_calibrator import GroundPlaneCalibrator
+from .interactive_camera_lidar_calibrator import InteractiveCameraLidarCalibrator
 from .lidar_lidar_2d_calibrator import LidarLidar2DCalibrator
 from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator
 from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator
@@ -10,6 +11,7 @@
 
 __all__ = [
     "GroundPlaneCalibrator",
+    "InteractiveCameraLidarCalibrator",
     "LidarLidar2DCalibrator",
     "MappingBasedBaseLidarCalibrator",
     "MappingBasedLidarLidarCalibrator",

sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/interactive_camera_lidar_calibrator.py

@@ -0,0 +1,60 @@
+#!/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="default_project", calibrator_name="interactive_camera_lidar_calibrator"
+)
+class InteractiveCameraLidarCalibrator(CalibratorBase):
+    required_frames = []
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.camera_optical_link_frame = kwargs["camera_optical_link_frame"]
+        self.lidar_frame = kwargs["lidar_frame"]
+
+        self.required_frames.append(self.camera_optical_link_frame)
+        self.required_frames.append(self.lidar_frame)
+
+        self.add_calibrator(
+            service_name="calibrate_camera_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=self.camera_optical_link_frame, child=self.lidar_frame),
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        optical_link_to_lidar_transform = calibration_transforms[self.camera_optical_link_frame][
+            self.lidar_frame
+        ]
+
+        result = {
+            self.lidar_frame: {
+                self.camera_optical_link_frame: np.linalg.inv(optical_link_to_lidar_transform)
+            }
+        }
+        return result