-
Notifications
You must be signed in to change notification settings - Fork 45
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: implemented camera-lidar calibrator for the drs
Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
- Loading branch information
Showing
6 changed files
with
407 additions
and
0 deletions.
There are no files selected for viewing
80 changes: 80 additions & 0 deletions
80
sensor_calibration_manager/launch/drs/mapping_based_base_lidar_calibrator.launch.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,80 @@ | ||
| <?xml version="1.0" encoding="UTF-8"?> | ||
| <launch> | ||
| <arg name="base_frame" default="base_link"/> | ||
|
|
||
| <arg name="rviz" default="true"/> | ||
|
|
||
| <arg name="lost_frame_max_acceleration" default="10.0"/> | ||
| <arg name="calibration_skip_keyframes" default="7"/> | ||
| <arg name="mapping_registrator" default="ndt" description="ndt or gicp"/> | ||
| <arg name="mapping_min_range" default="5.0" description="minimum distance to use for mapping"/> | ||
| <arg name="mapping_max_range" default="80.0" description="maximum distance to use for mapping"/> | ||
|
|
||
| <let name="calibrate_base_frame" value="true"/> | ||
| <arg name="base_lidar_crop_box_min_x" default="-5.0"/> | ||
| <arg name="base_lidar_crop_box_min_y" default="-5.0"/> | ||
| <arg name="base_lidar_crop_box_min_z" default="-5.0"/> | ||
| <arg name="base_lidar_crop_box_max_x" default="10.0"/> | ||
| <arg name="base_lidar_crop_box_max_y" default="5.0"/> | ||
| <arg name="base_lidar_crop_box_max_z" default="5.0"/> | ||
| <arg name="base_lidar_min_plane_points_percentage" default="10.0"/> | ||
| <arg name="base_lidar_max_inlier_distance" default="0.03"/> | ||
| <arg name="base_lidar_min_plane_points" default="500"/> | ||
| <arg name="base_lidar_max_cos_distance" default="0.2"/> | ||
| <arg name="base_lidar_max_iterations" default="500"/> | ||
| <arg name="base_lidar_overwrite_xy_yaw" default="false"/> | ||
|
|
||
| <let name="calibration_camera_optical_link_frames" value="['']"/> | ||
|
|
||
| <let name="calibration_lidar_frames" value="['']"/> | ||
|
|
||
| <let name="mapping_lidar_frame" value="pandar_top"/> | ||
| <let name="mapping_pointcloud" value="/sensing/lidar/top/pointcloud_raw"/> | ||
| <let name="detected_objects" value="/perception/object_recognition/detection/objects"/> | ||
|
|
||
| <let name="calibration_camera_info_topics" value="['']"/> | ||
|
|
||
| <let name="calibration_image_topics" value="[ | ||
| '']"/> | ||
|
|
||
| <let name="calibration_pointcloud_topics" value="['']"/> | ||
|
|
||
| <!-- mapping based calibrator --> | ||
| <include file="$(find-pkg-share mapping_based_calibrator)/launch/calibrator.launch.xml"> | ||
| <arg name="ns" value=""/> | ||
| <arg name="calibration_service_name" value="calibrate_base_lidar"/> | ||
|
|
||
| <arg name="rviz" value="$(var rviz)"/> | ||
| <arg name="base_frame" value="$(var base_frame)"/> | ||
|
|
||
| <arg name="calibration_camera_optical_link_frames" value="$(var calibration_camera_optical_link_frames)"/> | ||
| <arg name="calibration_lidar_frames" value="$(var calibration_lidar_frames)"/> | ||
| <arg name="mapping_lidar_frame" value="$(var mapping_lidar_frame)"/> | ||
|
|
||
| <arg name="mapping_pointcloud" value="$(var mapping_pointcloud)"/> | ||
| <arg name="detected_objects" value="$(var detected_objects)"/> | ||
|
|
||
| <arg name="calibration_camera_info_topics" value="$(var calibration_camera_info_topics)"/> | ||
| <arg name="calibration_image_topics" value="$(var calibration_image_topics)"/> | ||
| <arg name="calibration_pointcloud_topics" value="$(var calibration_pointcloud_topics)"/> | ||
|
|
||
| <arg name="mapping_registrator" value="$(var mapping_registrator)"/> | ||
| <arg name="mapping_min_range" value="$(var mapping_min_range)"/> | ||
| <arg name="mapping_max_range" value="$(var mapping_max_range)"/> | ||
| <arg name="lost_frame_max_acceleration" value="$(var lost_frame_max_acceleration)"/> | ||
| <arg name="calibration_skip_keyframes" value="$(var calibration_skip_keyframes)"/> | ||
|
|
||
| <arg name="base_lidar_crop_box_min_x" value="$(var base_lidar_crop_box_min_x)"/> | ||
| <arg name="base_lidar_crop_box_min_y" value="$(var base_lidar_crop_box_min_y)"/> | ||
| <arg name="base_lidar_crop_box_min_z" value="$(var base_lidar_crop_box_min_z)"/> | ||
| <arg name="base_lidar_crop_box_max_x" value="$(var base_lidar_crop_box_max_x)"/> | ||
| <arg name="base_lidar_crop_box_max_y" value="$(var base_lidar_crop_box_max_y)"/> | ||
| <arg name="base_lidar_crop_box_max_z" value="$(var base_lidar_crop_box_max_z)"/> | ||
| <arg name="base_lidar_min_plane_points_percentage" value="$(var base_lidar_min_plane_points_percentage)"/> | ||
| <arg name="base_lidar_max_inlier_distance" value="$(var base_lidar_max_inlier_distance)"/> | ||
| <arg name="base_lidar_min_plane_points" value="$(var base_lidar_min_plane_points)"/> | ||
| <arg name="base_lidar_max_cos_distance" value="$(var base_lidar_max_cos_distance)"/> | ||
| <arg name="base_lidar_max_iterations" value="$(var base_lidar_max_iterations)"/> | ||
| <arg name="base_lidar_overwrite_xy_yaw" value="$(var base_lidar_overwrite_xy_yaw)"/> | ||
| </include> | ||
| </launch> |
162 changes: 162 additions & 0 deletions
162
sensor_calibration_manager/launch/drs/tag_based_pnp_calibrator.launch.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,162 @@ | ||
| <launch> | ||
| <arg name="camera_name" default="camera0"> | ||
| <choice value="camera0"/> | ||
| <choice value="camera1"/> | ||
| <choice value="camera2"/> | ||
| <choice value="camera3"/> | ||
| <choice value="camera4"/> | ||
| <choice value="camera5"/> | ||
| <choice value="camera6"/> | ||
| <choice value="camera7"/> | ||
| </arg> | ||
|
|
||
| <arg name="view_only_ui" default="true" description="By default we use a minimal UI"/> | ||
| <arg name="calibration_pairs" default="14" description="Number of lidar-image pairs for the calibration to converge"/> | ||
| <arg name="calibration_pairs_min_distance" default="0.3" 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="lidar_frame" value="lidar_front" if="$(eval "'$(var camera_name)' == 'camera0' ")"/> | ||
| <let name="lidar_frame" value="lidar_front" if="$(eval "'$(var camera_name)' == 'camera1' ")"/> | ||
| <let name="lidar_frame" value="lidar_right" if="$(eval "'$(var camera_name)' == 'camera2' ")"/> | ||
| <let name="lidar_frame" value="lidar_right" if="$(eval "'$(var camera_name)' == 'camera3' ")"/> | ||
| <let name="lidar_frame" value="lidar_rear" if="$(eval "'$(var camera_name)' == 'camera4' ")"/> | ||
| <let name="lidar_frame" value="lidar_rear" if="$(eval "'$(var camera_name)' == 'camera5' ")"/> | ||
| <let name="lidar_frame" value="lidar_left" if="$(eval "'$(var camera_name)' == 'camera6' ")"/> | ||
| <let name="lidar_frame" value="lidar_left" if="$(eval "'$(var camera_name)' == 'camera7' ")"/> | ||
|
|
||
| <let name="pointcloud_topic" value="/sensing/lidar/front/nebula_points" if="$(eval "'$(var lidar_frame)' == 'lidar_front' ")"/> | ||
| <let name="pointcloud_topic" value="/sensing/lidar/rear/nebula_points" if="$(eval "'$(var lidar_frame)' == 'lidar_rear' ")"/> | ||
| <let name="pointcloud_topic" value="/sensing/lidar/left/nebula_points" if="$(eval "'$(var lidar_frame)' == 'lidar_left' ")"/> | ||
| <let name="pointcloud_topic" value="/sensing/lidar/right/nebula_points" if="$(eval "'$(var lidar_frame)' == 'lidar_right' ")"/> | ||
|
|
||
| <let name="camera_frame" value="$(var camera_name)/camera_link"/> | ||
| <let name="camera_optical_frame" value="$(var camera_name)/camera_optical_link"/> | ||
| <let name="rviz_profile" value="$(find-pkg-share tag_based_pnp_calibrator)/rviz/default_profile.rviz"/> | ||
|
|
||
| <let name="lidar_model" value="aeva_aeries2" if="$(eval "'$(var lidar_frame)' == 'lidar_front' ")"/> | ||
| <let name="lidar_model" value="seyond_falcon" if="$(eval "'$(var lidar_frame)' == 'lidar_rear' ")"/> | ||
| <let name="lidar_model" value="seyond_robin_w" if="$(eval "'$(var lidar_frame)' == 'lidar_left' ")"/> | ||
| <let name="lidar_model" value="seyond_robin_w" if="$(eval "'$(var lidar_frame)' == 'lidar_right' ")"/> | ||
|
|
||
| <let name="use_rectified_image" value="false"/> | ||
|
|
||
| <group> | ||
| <!-- 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)"/> | ||
| op | ||
| <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)"/> | ||
| </include> | ||
|
|
||
| <!-- interactive calibrator --> | ||
| <node pkg="interactive_camera_lidar_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen" if="$(eval "'$(var view_only_ui)' == 'false' ")"> | ||
| <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 "'$(var view_only_ui)' == 'true' ")"> | ||
| <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="tf_broadcaster" output="screen" args="0 0 0 0 0 0 $(var lidar_frame) lidar_frame"/> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var lidar_frame) --child-frame-id base_link --x 0.0 --y 0.0 --z -2.0 --yaw 0.0 --roll 0.0 --pitch 0.0" | ||
| /> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var lidar_frame) --child-frame-id $(var camera_frame) --x 0.0 --y 0.0 --z 0.0 --yaw 1.54 --roll 0.0 --pitch 0.0" | ||
| if="$(eval "'$(var lidar_frame)' == 'lidar_front' ")" | ||
| /> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var lidar_frame) --child-frame-id $(var camera_frame) --x 0.0 --y 0.0 --z 0.0 --yaw 0.523599 --roll 0.0 --pitch 0.523599" | ||
| if="$(eval "'$(var camera_name)' == 'camera2' ")" | ||
| /> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var lidar_frame) --child-frame-id $(var camera_frame) --x 0.0 --y 0.0 --z 0.0 --yaw -0.523599 --roll 0.0 --pitch 0.523599" | ||
| if="$(eval "'$(var camera_name)' == 'camera3' ")" | ||
| /> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var lidar_frame) --child-frame-id $(var camera_frame) --x 0.0 --y 0.0 --z 0.0 --yaw 0.0 --roll 0.0 --pitch 0.0" | ||
| if="$(eval "'$(var lidar_frame)' == 'lidar_rear' ")" | ||
| /> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var lidar_frame) --child-frame-id $(var camera_frame) --x 0.0 --y 0.0 --z 0.0 --yaw 0.523599 --roll 0.0 --pitch 0.523599" | ||
| if="$(eval "'$(var camera_name)' == 'camera6' ")" | ||
| /> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var lidar_frame) --child-frame-id $(var camera_frame) --x 0.0 --y 0.0 --z 0.0 --yaw -0.523599 --roll 0.0 --pitch 0.523599" | ||
| if="$(eval "'$(var camera_name)' == 'camera7' ")" | ||
| /> | ||
| <node | ||
| pkg="tf2_ros" | ||
| exec="static_transform_publisher" | ||
| name="tf_broadcaster" | ||
| output="screen" | ||
| args="--frame-id $(var camera_frame) --child-frame-id $(var camera_optical_frame) --x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5" | ||
| /> | ||
|
|
||
| <!-- 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> |
1 change: 1 addition & 0 deletions
1
sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
7 changes: 7 additions & 0 deletions
7
sensor_calibration_manager/sensor_calibration_manager/calibrators/drs/__init__.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,7 @@ | ||
| from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator | ||
| from .tag_based_pnp_calibrator import TagBasedPNPCalibrator | ||
|
|
||
| __all__ = [ | ||
| "MappingBasedLidarLidarCalibrator", | ||
| "TagBasedPNPCalibrator", | ||
| ] |
92 changes: 92 additions & 0 deletions
92
...anager/sensor_calibration_manager/calibrators/drs/mapping_based_lidar_lidar_calibrator.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| #!/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="drs", calibrator_name="mapping_based_lidar_lidar_calibrator" | ||
| ) | ||
| class MappingBasedLidarLidarCalibrator(CalibratorBase): | ||
| required_frames = [] | ||
|
|
||
| def __init__(self, ros_interface: RosInterface, **kwargs): | ||
| super().__init__(ros_interface) | ||
|
|
||
| self.sensor_kit_frame = "sensor_kit_base_link" | ||
| self.mapping_lidar_frame = "pandar_top" | ||
| self.calibration_lidar_frames = ["pandar_front", "pandar_left", "pandar_right"] | ||
| self.calibration_base_lidar_frames = [ | ||
| "pandar_front_base_link", | ||
| "pandar_left_base_link", | ||
| "pandar_right_base_link", | ||
| ] | ||
|
|
||
| self.required_frames.extend( | ||
| [ | ||
| self.sensor_kit_frame, | ||
| self.mapping_lidar_frame, | ||
| *self.calibration_lidar_frames, | ||
| *self.calibration_base_lidar_frames, | ||
| ] | ||
| ) | ||
|
|
||
| self.add_calibrator( | ||
| service_name="calibrate_lidar_lidar", | ||
| expected_calibration_frames=[ | ||
| FramePair(parent=self.mapping_lidar_frame, child=calibration_lidar_frame) | ||
| for calibration_lidar_frame in self.calibration_lidar_frames | ||
| ], | ||
| ) | ||
|
|
||
| def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): | ||
| sensor_kit_to_lidar_transform = self.get_transform_matrix( | ||
| self.sensor_kit_frame, self.mapping_lidar_frame | ||
| ) | ||
|
|
||
| calibration_lidar_to_base_lidar_transforms = [ | ||
| self.get_transform_matrix(calibration_lidar_frame, calibration_base_lidar_frame) | ||
| for calibration_lidar_frame, calibration_base_lidar_frame in zip( | ||
| self.calibration_lidar_frames, self.calibration_base_lidar_frames | ||
| ) | ||
| ] | ||
|
|
||
| sensor_kit_to_calibration_lidar_transforms = [ | ||
| sensor_kit_to_lidar_transform | ||
| @ calibration_transforms[self.mapping_lidar_frame][calibration_lidar_frame] | ||
| @ calibration_lidar_to_base_lidar_transform | ||
| for calibration_lidar_frame, calibration_lidar_to_base_lidar_transform in zip( | ||
| self.calibration_lidar_frames, calibration_lidar_to_base_lidar_transforms | ||
| ) | ||
| ] | ||
|
|
||
| result = { | ||
| self.sensor_kit_frame: { | ||
| calibration_base_lidar_frame: transform | ||
| for calibration_base_lidar_frame, transform in zip( | ||
| self.calibration_base_lidar_frames, sensor_kit_to_calibration_lidar_transforms | ||
| ) | ||
| } | ||
| } | ||
|
|
||
| return result |
Oops, something went wrong.