diff --git a/calibrators/tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py b/calibrators/tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py
index 4b687e64..9851cca2 100644
--- a/calibrators/tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py
+++ b/calibrators/tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py
@@ -26,16 +26,14 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("apriltag_detections_topic", "apriltag/detection_array")
decompressor_node = ComposableNode(
- package="autoware_image_transport_decompressor", # The package containing the component
- plugin="autoware::image_preprocessor::ImageTransportDecompressor", # The registered plugin name
- name="decompressor", # Node name within the container
+ package="autoware_image_transport_decompressor", # The package containing the component
+ plugin="autoware::image_preprocessor::ImageTransportDecompressor", # The registered plugin name
+ name="decompressor", # Node name within the container
remappings=[
("decompressor/input/compressed_image", LaunchConfiguration("image_compressed_topic")),
("decompressor/output/raw_image", LaunchConfiguration("image_topic")),
],
- parameters=[
- {"encoding": "default"}
- ]
+ parameters=[{"encoding": "default"}],
)
composable_node = ComposableNode(
diff --git a/docs/tutorials/mapping_based_calibrator.md b/docs/tutorials/mapping_based_calibrator.md
index 0186b9a4..b276c93e 100644
--- a/docs/tutorials/mapping_based_calibrator.md
+++ b/docs/tutorials/mapping_based_calibrator.md
@@ -147,7 +147,6 @@ The image below displays the vehicle within the pointcloud, allowing for a compa
## FAQ
- Why does the calibration fail?
-
- In most cases, the failure is due to mapping issues. The possible error messages are listed below and should be displayed in the console. For these cases, restart the experiment and drive more stably and slowly.
- Mapping failed. Angle between keyframes is too high.
- Mapping failed. Interpolation error is too high.
diff --git a/docs/tutorials/marker_radar_lidar_calibrator.md b/docs/tutorials/marker_radar_lidar_calibrator.md
index a50a2ee0..006b01d9 100644
--- a/docs/tutorials/marker_radar_lidar_calibrator.md
+++ b/docs/tutorials/marker_radar_lidar_calibrator.md
@@ -231,19 +231,16 @@ To evaluate the calibration result, the user can check that the calibrated radar
## FAQ
- Why does the reflector detection not appear on `RViz`?
-
- Make sure the center of the reflector faces toward the radar sensor, and the height of the reflector matches the radar's.
- Make sure the height of the radar reflector is not larger than the `reflector_max_height` parameter.
- Make sure the radar reflector is not in the background voxel (visualize the topic mentioned before).
- Why does the calibration error seem high?
-
- Make sure that there are no outliers in the calibration pairs list.
- Make sure that the initial calibration is good enough to match the lidar detection and radar detection correctly.
- Radars like the ARS408 (the one we use in this tutorial) have a resolution of 0.2m. Given that, there is a theoretical limit to how low the calibration error can be. If the radar resolution is low, it will strongly limit the lower bound of the calibration error.
- When can I stop the calibration process?
-
- It is recommended to stop the calibration when the curve in the cross-validation error has converged.
- With more matched pairs without outliers, the calibration result should improve if the number of pairs increases.
diff --git a/docs/tutorials/tag_based_pnp_calibrator.md b/docs/tutorials/tag_based_pnp_calibrator.md
index e0689631..572a01f3 100644
--- a/docs/tutorials/tag_based_pnp_calibrator.md
+++ b/docs/tutorials/tag_based_pnp_calibrator.md
@@ -133,7 +133,6 @@ In the following images we can see how the projection looks using the initial ca
## FAQ
- Why does the tool fail to add calibration pairs?
-
- One possible reason is that the detections are too close to the previously collected data. In this case, the new detections are deemed redundant, and thus not accepted.
- The timestamps of the lidar and camera are not synchronized. This can be checked with `ros2 topic echo [topic_name] --field header.stamp`. Setting the parameter `use_receive_time` to `True` might help to solve the issue, but is not recommended as a long-term solution.
- The detections are not stable enough. This can happen due to the following reasons:
@@ -141,17 +140,14 @@ In the following images we can see how the projection looks using the initial ca
- The tag is physically unstable due to wind, mounting issues, or other external factors. Even if the detector functions correctly, these conditions prevent the detection from converging. If this is the case, please eliminate all unwelcome external factors before attempting calibration. Forcefully calibrating under these conditions can compromise the results.
- Why does the UI not launch?
-
- Check with `ros2 node list` if the relevant nodes have been launched. It is possible that the provided parameters do not match any of the valid arguments among other standard ROS issues.
- If the UI crashes (check the console for details), it is probably due to a bad PySide installation, invalid intrinsic parameters, invalid extrinsic parameters, etc.
- The timestamps of the lidar and camera are not synchronized.
- Why does the reprojection error increase when more data is collected?
-
- When there are few samples, the model will fit the available data the best it can, even in the presence of noise (over-fitting) or wrong detections. When more data is collected, the error may increase to a certain extent, but that corresponds to the model attempting to fit all the data, this time unable to fit the noise, resulting in a higher error. However, it should reach a more-or-less table peak with about 10-15 pairs (depending on the data collection pattern/sampling).
- Why does the reprojection error seem high?
-
- The intrinsics may not be accurate, thus limiting the performance of the method.
- The boards are not appropriate (are bent).
- The boards moved too much while calibrating.
diff --git a/sensor_calibration_manager/launch/drs/tag_based_pnp_calibrator.launch.xml b/sensor_calibration_manager/launch/drs/tag_based_pnp_calibrator.launch.xml
index a531e557..a0daf408 100644
--- a/sensor_calibration_manager/launch/drs/tag_based_pnp_calibrator.launch.xml
+++ b/sensor_calibration_manager/launch/drs/tag_based_pnp_calibrator.launch.xml
@@ -38,7 +38,7 @@
-
+
diff --git a/sensor_calibration_manager/launch/drs_seyond/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/drs_seyond/mapping_based_lidar_lidar_calibrator.launch.xml
new file mode 100644
index 00000000..b39a8671
--- /dev/null
+++ b/sensor_calibration_manager/launch/drs_seyond/mapping_based_lidar_lidar_calibrator.launch.xml
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensor_calibration_manager/launch/drs_seyond/tag_based_pnp_calibrator.launch.xml b/sensor_calibration_manager/launch/drs_seyond/tag_based_pnp_calibrator.launch.xml
new file mode 100644
index 00000000..adb7bf49
--- /dev/null
+++ b/sensor_calibration_manager/launch/drs_seyond/tag_based_pnp_calibrator.launch.xml
@@ -0,0 +1,154 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/__init__.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/__init__.py
new file mode 100644
index 00000000..b68bd981
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/__init__.py
@@ -0,0 +1,7 @@
+from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator
+from .tag_based_pnp_calibrator import TagBasedPNPCalibrator
+
+__all__ = [
+ "MappingBasedLidarLidarCalibrator",
+ "TagBasedPNPCalibrator",
+]
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/mapping_based_lidar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/mapping_based_lidar_lidar_calibrator.py
new file mode 100644
index 00000000..1645102d
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/mapping_based_lidar_lidar_calibrator.py
@@ -0,0 +1,70 @@
+#!/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_seyond", calibrator_name="mapping_based_lidar_lidar_calibrator"
+)
+class MappingBasedLidarLidarCalibrator(CalibratorBase):
+ required_frames = []
+
+ def __init__(self, ros_interface: RosInterface, **kwargs):
+ super().__init__(ros_interface)
+
+ self.base_link = kwargs["base_frame"]
+ self.mapping_lidar_frame = "lidar_front"
+ self.calibration_lidar_frames = ["lidar_left", "lidar_right", "lidar_rear"]
+
+ self.required_frames.extend(
+ [self.base_link, self.mapping_lidar_frame, *self.calibration_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]]):
+ base_to_mapping_transform = self.get_transform_matrix(
+ self.base_link, self.mapping_lidar_frame
+ )
+
+ base_to_calibration_lidar_transforms = [
+ base_to_mapping_transform
+ @ calibration_transforms[self.mapping_lidar_frame][calibration_lidar_frame]
+ for calibration_lidar_frame in self.calibration_lidar_frames
+ ]
+
+ result = {
+ self.base_link: dict(
+ zip(self.calibration_lidar_frames, base_to_calibration_lidar_transforms)
+ )
+ }
+ result[self.base_link][self.mapping_lidar_frame] = base_to_mapping_transform
+
+ return result
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/tag_based_pnp_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/tag_based_pnp_calibrator.py
new file mode 100644
index 00000000..5c9ca435
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/drs_seyond/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="drs_seyond", calibrator_name="tag_based_pnp_calibrator"
+)
+class TagBasedPNPCalibrator(CalibratorBase):
+ required_frames = []
+
+ 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(self.lidar_frame)
+ self.required_frames.append(f"{self.camera_name}/camera_link")
+ self.required_frames.append(f"{self.camera_name}/camera_optical_link")
+
+ 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_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_camera_link_transform}
+ }
+ return result