-
Notifications
You must be signed in to change notification settings - Fork 48
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: implemented the new api files for the interactive calibrator an…
…d fixed some bugs Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
- Loading branch information
Showing
8 changed files
with
290 additions
and
5 deletions.
There are no files selected for viewing
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
176 changes: 176 additions & 0 deletions
176
calibrators/interactive_camera_lidar_calibrator/rviz/default_profile.rviz
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,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 |
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
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
39 changes: 39 additions & 0 deletions
39
...calibration_manager/launch/default_project/interactive_camera_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,39 @@ | ||
<launch> | ||
<arg name="camera_optical_link_frame" default="zedxm_left_camera_optical_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> |
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
59 changes: 59 additions & 0 deletions
59
...or_calibration_manager/calibrators/default_project/interactive_camera_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,59 @@ | ||
#!/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 |
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