diff --git a/calibrators/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py b/calibrators/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py index d3be2bfe..f542caa8 100644 --- a/calibrators/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py +++ b/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 diff --git a/calibrators/interactive_camera_lidar_calibrator/rviz/default_profile.rviz b/calibrators/interactive_camera_lidar_calibrator/rviz/default_profile.rviz new file mode 100644 index 00000000..0423aacb --- /dev/null +++ b/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: + 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: + 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: + 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 diff --git a/calibrators/interactive_camera_lidar_calibrator/setup.py b/calibrators/interactive_camera_lidar_calibrator/setup.py index 3873fd49..a389010b 100644 --- a/calibrators/interactive_camera_lidar_calibrator/setup.py +++ b/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, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py index b0c98b2d..dbe6c33f 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py +++ b/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) diff --git a/common/tier4_calibration_views/tier4_calibration_views/image_view_ros_interface.py b/common/tier4_calibration_views/tier4_calibration_views/image_view_ros_interface.py index c32bf28c..bfce896d 100644 --- a/common/tier4_calibration_views/tier4_calibration_views/image_view_ros_interface.py +++ b/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) diff --git a/sensor_calibration_manager/launch/default_project/interactive_camera_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/default_project/interactive_camera_lidar_calibrator.launch.xml new file mode 100644 index 00000000..16a29f67 --- /dev/null +++ b/sensor_calibration_manager/launch/default_project/interactive_camera_lidar_calibrator.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py index 530b5460..ab6e1210 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py +++ b/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", diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/interactive_camera_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/interactive_camera_lidar_calibrator.py new file mode 100644 index 00000000..c4e7ffcd --- /dev/null +++ b/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