diff --git a/.cspell.json b/.cspell.json index b7546200..4cb33a5a 100644 --- a/.cspell.json +++ b/.cspell.json @@ -1,17 +1,108 @@ { "words": [ - "apriltags", + "3dpoints", "Rodrigues", - "subsampled", - "undistortion", - "uniformingly", + "antialiasing", + "apriltag", + "apriltags", + "arange", + "autoware", + "astype", + "auxiliar", + "axisd", + "beforementioned", + "calib", + "cmap", + "coeffs", + "crossval", + "crossvalidation", + "opencv", + "discretization", + "distro", + "downsampling", + "downsample", + "dtype", + "eigen", + "eulers", + "extrinsics", + "figsize", + "gicp", + "hesai", + "homography", "hsize", + "icp", + "idless", + "idxs", + "imdecode", + "imread", + "imshow", + "imwrite", + "intrinsics", + "kalman", + "keyframes", + "libceres", + "lidars", + "lidartag", + "lidartags", + "linalg", + "matplotlib", + "matx", + "meshgrid", + "misdetection", + "nanosec", + "neighbours", + "ncols", + "nrows", + "omiya", + "overfits", + "pandar", + "permutate", + "pixmap", + "pnp", + "pointcloud", + "pointclouds", + "polyline", + "prerejective", + "pydot", + "pyplot", + "qcolor", + "quaterniond", + "ransac", + "rclcpp", + "rclpy", + "registrator", + "registrators", + "remappings", + "representer", + "reprojected", + "reprojection", + "rosbag", + "rosidl", + "ruamel", "rvec", - "tvec", "rvecs", + "rviz", + "slerp", + "solvepnp", + "sqpnp", + "srvs", + "subsampled", + "subsamples", + "subsampling", + "tvec", "tvecs", - "nrows", - "ncols", - "crossval" + "undistort", + "undistortion", + "uniformingly", + "velodyne", + "vectord", + "voxel", + "voxels", + "xaxis", + "xlabel", + "xlim", + "yaxis", + "ylabel", + "ylim" ] } diff --git a/build_depends.repos b/build_depends.repos index 0639d358..ceb76596 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -19,6 +19,10 @@ repositories: type: git url: https://github.com/astuff/astuff_sensor_msgs.git version: 3.2.0 + core/autoware_internal_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_internal_msgs.git + version: main core/common: type: git url: https://github.com/autowarefoundation/autoware_common.git diff --git a/sensor/point_cloud_accumulator/CMakeLists.txt b/common/point_cloud_accumulator/CMakeLists.txt similarity index 100% rename from sensor/point_cloud_accumulator/CMakeLists.txt rename to common/point_cloud_accumulator/CMakeLists.txt diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/__init__.py b/common/point_cloud_accumulator/COLCON_IGNORE similarity index 100% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/__init__.py rename to common/point_cloud_accumulator/COLCON_IGNORE diff --git a/sensor/point_cloud_accumulator/include/point_cloud_accumulator/point_cloud_accumulator.hpp b/common/point_cloud_accumulator/include/point_cloud_accumulator/point_cloud_accumulator.hpp similarity index 100% rename from sensor/point_cloud_accumulator/include/point_cloud_accumulator/point_cloud_accumulator.hpp rename to common/point_cloud_accumulator/include/point_cloud_accumulator/point_cloud_accumulator.hpp diff --git a/sensor/point_cloud_accumulator/launch/point_cloud_accumulator.launch.xml b/common/point_cloud_accumulator/launch/point_cloud_accumulator.launch.xml similarity index 100% rename from sensor/point_cloud_accumulator/launch/point_cloud_accumulator.launch.xml rename to common/point_cloud_accumulator/launch/point_cloud_accumulator.launch.xml diff --git a/sensor/point_cloud_accumulator/package.xml b/common/point_cloud_accumulator/package.xml similarity index 100% rename from sensor/point_cloud_accumulator/package.xml rename to common/point_cloud_accumulator/package.xml diff --git a/sensor/point_cloud_accumulator/src/point_cloud_accumulator.cpp b/common/point_cloud_accumulator/src/point_cloud_accumulator.cpp similarity index 100% rename from sensor/point_cloud_accumulator/src/point_cloud_accumulator.cpp rename to common/point_cloud_accumulator/src/point_cloud_accumulator.cpp diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt index 9fbd6245..4c03fb4d 100644 --- a/common/tier4_calibration_msgs/CMakeLists.txt +++ b/common/tier4_calibration_msgs/CMakeLists.txt @@ -17,6 +17,7 @@ ament_auto_find_build_dependencies() rosidl_generate_interfaces(${PROJECT_NAME} "msg/BoolStamped.msg" "msg/CalibrationPoints.msg" + "msg/CalibrationResult.msg" "msg/Files.msg" "msg/Float32Stamped.msg" "msg/EstimationResult.msg" @@ -25,10 +26,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/FilesSrv.srv" "srv/FilesListSrv.srv" "srv/CalibrationDatabase.srv" - "srv/ExtrinsicCalibrationManager.srv" - "srv/ExtrinsicCalibrator.srv" "srv/Frame.srv" "srv/IntrinsicsOptimizer.srv" + "srv/ExtrinsicCalibrator.srv" DEPENDENCIES std_msgs sensor_msgs diff --git a/common/tier4_calibration_msgs/msg/CalibrationResult.msg b/common/tier4_calibration_msgs/msg/CalibrationResult.msg new file mode 100644 index 00000000..83a683a5 --- /dev/null +++ b/common/tier4_calibration_msgs/msg/CalibrationResult.msg @@ -0,0 +1,4 @@ +geometry_msgs/TransformStamped transform_stamped +bool success +float32 score +std_msgs/String message diff --git a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv deleted file mode 100644 index 1fa2017b..00000000 --- a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv +++ /dev/null @@ -1,6 +0,0 @@ -string src_path -string dst_path ---- -bool success -float32 score -sensor_msgs/PointCloud2 debug_pointcloud diff --git a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv index c8e0a95b..13c3fad2 100644 --- a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv +++ b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv @@ -1,6 +1,2 @@ -geometry_msgs/Pose initial_pose --- -geometry_msgs/Pose result_pose -bool success -float32 score -sensor_msgs/PointCloud2 debug_pointcloud +tier4_calibration_msgs/CalibrationResult[] results diff --git a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp index f354cfb6..bd1dc221 100644 --- a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp +++ b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. diff --git a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp index ad6efd39..8030c62a 100644 --- a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp +++ b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -354,7 +354,7 @@ void pcl::JointIterativeClosestPointExtended:: ++nr_iterations_; - // Update the vizualization of icp convergence + // Update the visualization of icp convergence // if (update_visualizer_ != 0) // update_visualizer_(output, source_indices_good, *target_, target_indices_good ); diff --git a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp index dbae62d4..74558002 100644 --- a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp +++ b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -82,7 +82,7 @@ struct cloud_point_index_idx_triplets { return (idx0 < p.idx0) || (idx0 == p.idx0 && idx1 < p.idx1) || (idx0 == p.idx0 && idx1 == p.idx1 && idx2 < p.idx2); - } // test brancheless agains branched version with a dataset at some point \ along with some sort + } // test branchless against branched version with a dataset at some point \ along with some sort // implementations }; @@ -102,7 +102,7 @@ class VoxelGridTriplets : public VoxelGrid using VoxelGrid::min_b_; using VoxelGrid::max_b_; using VoxelGrid::div_b_; - using VoxelGrid::divb_mul_; + using VoxelGrid::divb_mul_; // cSpell:ignore divb using VoxelGrid::min_points_per_voxel_; using VoxelGrid::save_leaf_layout_; using VoxelGrid::leaf_layout_; diff --git a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp index ffbecc6a..d1894823 100644 --- a/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp +++ b/common/tier4_calibration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -123,6 +123,7 @@ void pcl::VoxelGridTriplets::applyFilter(PointCloud & output) div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones(); div_b_[3] = 0; + // cSpell:ignore divb // Set up the division multiplier divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0); @@ -199,6 +200,8 @@ void pcl::VoxelGridTriplets::applyFilter(PointCloud & output) } } + // cSpell:ignore spreadsort + // cSpell:ignore rightshift // Second pass: sort the index_vector vector using value representing target cell as index // in effect all points belonging to the same output cell will be next to each other // auto rightshift_func = [](const cloud_point_index_idx_triplets &x, const unsigned offset) { diff --git a/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp b/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp index 83f6f654..3ec74c3a 100644 --- a/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp +++ b/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. diff --git a/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp b/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp index a2492be2..92692ba6 100644 --- a/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp +++ b/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. diff --git a/common/tier4_calibration_views/CMakeLists.txt b/common/tier4_calibration_views/CMakeLists.txt new file mode 100644 index 00000000..95b1e5ab --- /dev/null +++ b/common/tier4_calibration_views/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(tier4_calibration_views) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(autoware_cmake REQUIRED) + +autoware_package() +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + scripts/image_view_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_dependencies(ament_cmake) +ament_export_dependencies(ament_cmake_python) +ament_package() diff --git a/common/tier4_calibration_views/package.xml b/common/tier4_calibration_views/package.xml new file mode 100644 index 00000000..9f1b4d27 --- /dev/null +++ b/common/tier4_calibration_views/package.xml @@ -0,0 +1,29 @@ + + + + tier4_calibration_views + 0.0.0 + TODO: Package description + Kenzo Lobos Tsunekawa + TODO: License declaration + + ament_cmake_auto + ament_cmake_python + + autoware_cmake + + python3-matplotlib + python3-pyside2.qtquick + python3-transforms3d + rclpy + ros2_numpy + ros2launch + tier4_calibration_msgs + ament_copyright + ament_flake8 + python3-pytest + + + ament_cmake + + diff --git a/sensor/extrinsic_interactive_calibrator/resource/extrinsic_interactive_calibrator b/common/tier4_calibration_views/resource/tier4_calibration_views similarity index 100% rename from sensor/extrinsic_interactive_calibrator/resource/extrinsic_interactive_calibrator rename to common/tier4_calibration_views/resource/tier4_calibration_views diff --git a/common/tier4_calibration_views/scripts/image_view_node.py b/common/tier4_calibration_views/scripts/image_view_node.py new file mode 100755 index 00000000..41f327cf --- /dev/null +++ b/common/tier4_calibration_views/scripts/image_view_node.py @@ -0,0 +1,53 @@ +#!/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. + +import logging +import os +import signal +import sys + +from PySide2.QtWidgets import QApplication +import rclpy +from tier4_calibration_views.image_view_ros_interface import ImageViewRosInterface +from tier4_calibration_views.image_view_ui import ImageViewUI + + +def main(args=None): + os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = "" + app = QApplication(sys.argv) + + rclpy.init(args=args) + + try: + signal.signal(signal.SIGINT, sigint_handler) + + ros_interface = ImageViewRosInterface() + ex = ImageViewUI(ros_interface) # noqa: F841 + + ros_interface.spin() + + sys.exit(app.exec_()) + except (KeyboardInterrupt, SystemExit): + logging.info("Received sigint. Quitting...") + rclpy.shutdown() + + +def sigint_handler(*args): + QApplication.quit() + + +if __name__ == "__main__": + main() diff --git a/sensor/extrinsic_interactive_calibrator/test/test_pep257.py b/common/tier4_calibration_views/test/test_pep257.py similarity index 100% rename from sensor/extrinsic_interactive_calibrator/test/test_pep257.py rename to common/tier4_calibration_views/test/test_pep257.py diff --git a/common/tier4_calibration_views/tier4_calibration_views/__init__.py b/common/tier4_calibration_views/tier4_calibration_views/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py b/common/tier4_calibration_views/tier4_calibration_views/image_view.py similarity index 93% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py rename to common/tier4_calibration_views/tier4_calibration_views/image_view.py index 51b4bdb1..52257ab8 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2020 Tier IV, Inc. +# 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. @@ -16,6 +16,7 @@ import copy +import logging import threading from PySide2.QtCore import QObject @@ -32,10 +33,10 @@ from PySide2.QtWidgets import QGraphicsItem from PySide2.QtWidgets import QGraphicsView import cv2 -from extrinsic_interactive_calibrator.utils import decompose_transformation_matrix -from extrinsic_interactive_calibrator.utils import transform_points import matplotlib.pyplot as plt import numpy as np +from tier4_calibration_views.utils import decompose_transformation_matrix +from tier4_calibration_views.utils import transform_points def intensity_to_rainbow_qcolor(value, alpha=1.0): @@ -453,6 +454,7 @@ def draw_pointcloud(self, painter): ): return + # Note: ccs=camera coordinate system pointcloud_ccs = transform_points( self.data_renderer.image_to_lidar_translation, self.data_renderer.image_to_lidar_rotation, @@ -483,8 +485,8 @@ def draw_pointcloud(self, painter): ), ) - # Transform (rescale) into the widet coordinate system - pointdloud_z = pointcloud_ccs[indexes, 2] + # Transform (rescale) into the widget coordinate system + pointcloud_z = pointcloud_ccs[indexes, 2] pointcloud_i = self.data_renderer.pointcloud_intensity[indexes] if self.data_renderer.marker_units == "meters": @@ -493,10 +495,10 @@ def draw_pointcloud(self, painter): * self.data_renderer.marker_size_meters * self.width_image_to_widget_factor ) - scale_px = factor / pointdloud_z + scale_px = factor / pointcloud_z else: factor = self.data_renderer.marker_size_pixels * self.width_image_to_widget_factor - scale_px = factor * np.ones_like(pointdloud_z) + scale_px = factor * np.ones_like(pointcloud_z) pointcloud_wcs = pointcloud_ics[indexes, :] * self.image_to_widget_factor @@ -513,7 +515,7 @@ def draw_pointcloud(self, painter): elif self.data_renderer.color_channel == "y": color_scalars = pointcloud_ccs[indexes, 1][indexes2] elif self.data_renderer.color_channel == "z": - color_scalars = pointdloud_z[indexes2] + color_scalars = pointcloud_z[indexes2] elif self.data_renderer.color_channel == "intensity": color_scalars = pointcloud_i[indexes2] min_value = color_scalars.min() @@ -525,8 +527,7 @@ def draw_pointcloud(self, painter): else: raise NotImplementedError except Exception as e: - print(e) - pass + logging.error(e) line_pen = QPen() line_pen.setWidth(2) @@ -579,8 +580,10 @@ def draw_calibration_points(self, painter): image_points = np.array(self.data_renderer.image_points) + # Note: lcs=lidar coordinate system object_points_lcs = np.array(self.data_renderer.object_points) + # Note: ccs=camera coordinate system object_points_ccs = transform_points( self.data_renderer.image_to_lidar_translation, self.data_renderer.image_to_lidar_rotation, @@ -597,7 +600,7 @@ def draw_calibration_points(self, painter): repr_err = np.linalg.norm(object_points_ics - image_points, axis=1) - # Transform (rescale) into the widet coordinate system + # Transform (rescale) into the widget coordinate system object_points_wcs = object_points_ics * self.image_to_widget_factor radius = 10 * self.width_image_to_widget_factor @@ -652,6 +655,7 @@ def draw_external_calibration_points(self, painter): ): return + # Note: lcs=lidar coordinate system, ccs=camera coordinate system. ics=image coordinate system object_points_lcs = np.array(self.data_renderer.external_object_points) object_points_ccs = transform_points( @@ -668,7 +672,7 @@ def draw_external_calibration_points(self, painter): ) object_points_ics = object_points_ics.reshape(-1, 2) - # Transform (rescale) into the widet coordinate system + # Transform (rescale) into the widget coordinate system object_points_wcs = object_points_ics * self.image_to_widget_factor radius = 10 * self.width_image_to_widget_factor @@ -682,6 +686,9 @@ def draw_external_calibration_points(self, painter): # Draw tag borders image_points = self.data_renderer.external_image_points + scaled_pix_size = self.pix.size() + scaled_pix_size.scale(self.data_renderer.widget_size, Qt.KeepAspectRatio) + for i1 in range(len(image_points)): tag_index = i1 // 4 i2 = 4 * tag_index + ((i1 + 1) % 4) @@ -700,6 +707,20 @@ def draw_external_calibration_points(self, painter): image_point_2_wcs[1], ) + if ( + np.any(np.isnan(object_point_1_wcs)) + or np.any(np.isnan(object_point_2_wcs)) + or object_point_1_wcs[0] < 0 + or object_point_1_wcs[0] > scaled_pix_size.width() + or object_point_1_wcs[1] < 0 + or object_point_1_wcs[1] > scaled_pix_size.height() + or object_point_2_wcs[0] < 0 + or object_point_2_wcs[0] > scaled_pix_size.width() + or object_point_2_wcs[1] < 0 + or object_point_2_wcs[1] > scaled_pix_size.height() + ): + continue + painter.setPen(object_line_pen) painter.drawLine( object_point_1_wcs[0], @@ -744,6 +765,7 @@ def draw_current_point(self, painter): ): return + # Note: wcs=widget coordinate system, ccs=camera coordinate system. ics=image coordinate system object_point_ccs = transform_points( self.data_renderer.image_to_lidar_translation, self.data_renderer.image_to_lidar_rotation, @@ -758,7 +780,7 @@ def draw_current_point(self, painter): ) object_point_ics = object_point_ics.reshape(1, 2) - # Transform (rescale) into the widet coordinate system + # Transform (rescale) into the widget coordinate system object_point_wcs = object_point_ics * self.image_to_widget_factor object_point_wcs = object_point_wcs.reshape( 2, @@ -800,7 +822,7 @@ def mousePressEvent(self, e): self.clicked_signal.emit(x, y) else: - print("Click out of image coordinates !") + logging.error("Click out of image coordinates !") self.prepareGeometryChange() return super().mousePressEvent(e) 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 new file mode 100644 index 00000000..179cc0a3 --- /dev/null +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view_ros_interface.py @@ -0,0 +1,239 @@ +#!/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 collections import deque +import threading +from typing import Callable +from typing import Deque +from typing import List +from typing import Optional +from typing import Union + +import cv2 +from cv_bridge import CvBridge +from geometry_msgs.msg import Point +import numpy as np +import rclpy +from rclpy.duration import Duration +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from rclpy.qos import qos_profile_system_default +import ros2_numpy +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import CompressedImage +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster +from tf2_ros.transform_listener import TransformListener +from tier4_calibration_msgs.msg import CalibrationPoints +from tier4_calibration_views.utils import stamp_to_seconds +from tier4_calibration_views.utils import tf_message_to_transform_matrix + + +class ImageViewRosInterface(Node): + def __init__(self, node_name="image_view"): + super().__init__(node_name) + + self.lock = threading.RLock() + + self.declare_parameter("use_rectified", False) + self.declare_parameter("use_compressed", True) + self.declare_parameter("timer_period", 1.0) + self.declare_parameter("delay_tolerance", 0.06) + + self.use_rectified = self.get_parameter("use_rectified").get_parameter_value().bool_value + self.use_compressed = self.get_parameter("use_compressed").get_parameter_value().bool_value + self.timer_period = self.get_parameter("timer_period").get_parameter_value().double_value + self.delay_tolerance = ( + self.get_parameter("delay_tolerance").get_parameter_value().double_value + ) + + self.image_frame: Optional[str] = None + self.lidar_frame: Optional[str] = None + + # Data + self.pointcloud_queue: Deque[PointCloud2] = deque([], 5) + self.image_queue: Deque[Union[CompressedImage, Image]] = deque([], 5) + + self.camera_info: Optional[CameraInfo] = None + self.pointcloud_sync: Optional[PointCloud2] = None + self.image_sync: Optional[Union[CompressedImage, Image]] = None + + # ROS Interface configuration + self.sensor_data_callback: Optional[Callable] = None + self.sensor_data_delay_callback: Optional[Callable] = None + self.transform_callback: Optional[Callable] = None + self.external_calibration_points_callback: Optional[Callable] = None + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.tf_publisher = StaticTransformBroadcaster(self) + self.bridge = CvBridge() + + self.lidar_subs = self.create_subscription( + PointCloud2, "pointcloud", self.pointcloud_callback, qos_profile_sensor_data + ) + + if self.use_compressed: + self.image_sub = self.create_subscription( + CompressedImage, "image", self.image_callback, qos_profile_sensor_data + ) + else: + self.image_sub = self.create_subscription( + Image, "image", self.image_callback, qos_profile_sensor_data + ) + + self.camera_info_sub = self.create_subscription( + CameraInfo, "camera_info", self.camera_info_callback, qos_profile_sensor_data + ) + + self.point_sub = self.create_subscription( + CalibrationPoints, + "calibration_points_input", + self.calibration_points_callback, + qos_profile_system_default, + ) + + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + def set_sensor_data_callback(self, callback): + with self.lock: + self.sensor_data_callback = callback + + def set_sensor_data_delay_callback(self, callback): + with self.lock: + self.sensor_data_delay_callback = callback + + def set_transform_callback(self, callback): + with self.lock: + self.transform_callback = callback + + def set_external_calibration_points_callback(self, callback): + with self.lock: + self.external_calibration_points_callback = callback + + def pointcloud_callback(self, pointcloud_msg: PointCloud2): + self.lidar_frame = pointcloud_msg.header.frame_id + self.pointcloud_queue.append(pointcloud_msg) + self.check_sync() + + def image_callback(self, image_msg: Union[CompressedImage, Image]): + self.image_queue.append(image_msg) + self.check_sync() + + def camera_info_callback(self, camera_info_msg: CameraInfo): + self.camera_info = camera_info_msg + 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 + + def check_sync(self): + if len(self.image_queue) == 0 or len(self.pointcloud_queue) == 0: + return + + min_delay = np.inf + + for pointcloud_msg in self.pointcloud_queue: + for image_msg in self.image_queue: + current_delay = abs( + stamp_to_seconds(pointcloud_msg.header.stamp) + - stamp_to_seconds(image_msg.header.stamp) + ) + + min_delay = min(min_delay, current_delay) + + if min_delay <= self.delay_tolerance: + break + + if min_delay > self.delay_tolerance: + self.sensor_data_delay_callback(min_delay) + return + + pc_data = ros2_numpy.numpify(pointcloud_msg) + points_np = np.zeros(pc_data.shape + (4,)) + points_np[..., 0] = pc_data["x"] + points_np[..., 1] = pc_data["y"] + points_np[..., 2] = pc_data["z"] + points_np[..., 3] = ( + pc_data["intensity"] + if "intensity" in pc_data.dtype.names + else np.zeros_like(pc_data["x"]) + ) + points_np = points_np.reshape(-1, 4) + + with self.lock: + self.camera_info_sync = self.camera_info + self.image_sync = image_msg + self.pointcloud_sync = pointcloud_msg + + if self.use_compressed: + 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) + # image = cv2.cvtColor(self.raw_image, cv2.COLOR_BGR2RGB) + + self.sensor_data_callback(self.image_sync, self.camera_info_sync, points_np, min_delay) + + self.image_queue.clear() + self.pointcloud_queue.clear() + + def calibration_points_callback(self, calibration_points: CalibrationPoints): + object_points: List[Point] = calibration_points.object_points + image_points: List[Point] = calibration_points.image_points + + assert len(object_points) == len(object_points) + + object_points = [np.array([p.x, p.y, p.z]) for p in object_points] + image_points = [np.array([p.x, p.y]) for p in image_points] + + self.external_calibration_points_callback(object_points, image_points) + + def timer_callback(self): + with self.lock: + if self.image_frame is None or self.lidar_frame is None: + return + + try: + transform = self.tf_buffer.lookup_transform( + self.image_frame, + self.lidar_frame, + rclpy.time.Time(seconds=0, nanoseconds=0), + timeout=Duration(seconds=0.2), + ) + + transform_matrix = tf_message_to_transform_matrix(transform) + self.transform_callback(transform_matrix) + except TransformException as ex: + self.get_logger().error( + f"Could not transform {self.image_frame} to {self.lidar_frame}: {ex}" + ) + + def spin(self): + self.ros_executor = MultiThreadedExecutor(num_threads=2) + self.ros_executor.add_node(self) + + self.thread = threading.Thread(target=self.executor.spin, args=()) + self.thread.setDaemon(True) + self.thread.start() diff --git a/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py b/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py new file mode 100644 index 00000000..9d2c2d5b --- /dev/null +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py @@ -0,0 +1,421 @@ +#!/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. + +import copy +import threading + +from PySide2.QtCore import Qt +from PySide2.QtCore import Signal +from PySide2.QtGui import QImage +from PySide2.QtGui import QPixmap +from PySide2.QtWidgets import QCheckBox +from PySide2.QtWidgets import QComboBox +from PySide2.QtWidgets import QDoubleSpinBox +from PySide2.QtWidgets import QGraphicsScene +from PySide2.QtWidgets import QGraphicsView +from PySide2.QtWidgets import QGroupBox +from PySide2.QtWidgets import QHBoxLayout +from PySide2.QtWidgets import QLabel +from PySide2.QtWidgets import QMainWindow +from PySide2.QtWidgets import QSpinBox +from PySide2.QtWidgets import QVBoxLayout +from PySide2.QtWidgets import QWidget +import numpy as np +from tier4_calibration_views.image_view import CustomQGraphicsView +from tier4_calibration_views.image_view import ImageView +from tier4_calibration_views.image_view_ros_interface import ImageViewRosInterface + + +class ImageViewUI(QMainWindow): + sensor_data_signal = Signal() + sensor_data_delay_signal = Signal(float) + transform_signal = Signal() + external_calibration_points_signal = Signal() + optimized_intrinsics_signal = Signal() + + def __init__(self, ros_interface: ImageViewRosInterface): + super().__init__() + self.setWindowTitle("Image view (camera-lidar delay=??)") + + # ROS Interface + self.ros_interface = ros_interface + self.ros_interface.set_sensor_data_callback(self.sensor_data_ros_callback) + self.ros_interface.set_sensor_data_delay_callback(self.sensor_data_delay_ros_callback) + self.ros_interface.set_transform_callback(self.transform_ros_callback) + self.ros_interface.set_external_calibration_points_callback( + self.external_calibration_points_ros_callback + ) + + self.sensor_data_signal.connect(self.sensor_data_callback) + self.sensor_data_delay_signal.connect(self.sensor_data_delay_callback) + self.transform_signal.connect(self.transform_callback) + self.external_calibration_points_signal.connect(self.external_calibration_points_callback) + + # Threading variables + self.lock = threading.RLock() + self.transform_tmp = None + self.external_object_calibration_points_tmp = None + self.external_image_calibration_points_tmp = None + self.pixmap_tmp = None + self.camera_info_tmp = None + self.pointcloud_tmp = None + self.delay_tmp = None + + # Calibration variables + self.camera_info = None + self.initial_transform = None + self.current_transform = None + self.calibrated_transform = None + self.source_transform = None + + # Parent widget + self.central_widget = QWidget(self) + self.left_menu_widget = None + self.right_menu_widget = None + + self.setCentralWidget(self.central_widget) + self.layout = QHBoxLayout(self.central_widget) + + # Image View + self.make_image_view() + + # Menu Widgets + self.make_left_menu() + self.make_right_menu() + + self.layout.addWidget(self.graphics_view) + + if self.left_menu_widget: + self.layout.addWidget(self.left_menu_widget) + + if self.right_menu_widget: + self.layout.addWidget(self.right_menu_widget) + + self.show() + + def make_left_menu(self): + pass + + def make_right_menu(self): + self.right_menu_widget = QWidget(self.central_widget) + self.right_menu_widget.setFixedWidth(210) + self.right_menu_layout = QVBoxLayout(self.right_menu_widget) + + # Visualization group + self.make_visualization_options() + + self.right_menu_layout.addWidget(self.visualization_options_group) + + def make_image_view(self): + self.image_view = ImageView() + # self.image_view.set_pixmap(pixmap) + self.image_view.clicked_signal.connect(self.image_click_callback) + + # We need the view to control the zoom + self.graphics_view = CustomQGraphicsView(self.central_widget) + self.graphics_view.setCacheMode(QGraphicsView.CacheBackground) + self.graphics_view.setViewportUpdateMode(QGraphicsView.BoundingRectViewportUpdate) + + # The scene contains the items + self.scene = QGraphicsScene() + + # Add the item into the scene + self.scene.addItem(self.image_view) + + # Add the scene into the view + self.graphics_view.setScene(self.scene) + + def make_calibration_options(self): + pass + + def make_data_collection_options(self): + pass + + def make_visualization_options(self): + self.visualization_options_group = QGroupBox("Visualization options") + self.visualization_options_group.setFlat(True) + + tf_source_label = QLabel("TF source:") + self.tf_source_combobox = QComboBox() + self.tf_source_combobox.currentTextChanged.connect(self.tf_source_callback) + + def marker_type_callback(value): + self.image_view.set_marker_type(value) + + marker_type_label = QLabel("Marker type:") + marker_type_combobox = QComboBox() + marker_type_combobox.currentTextChanged.connect(marker_type_callback) + marker_type_combobox.addItem("Circles") + marker_type_combobox.addItem("Rectangles") + + def marker_units_callback(value): + self.image_view.set_marker_units(value) + + marker_units_label = QLabel("Marker units:") + marker_units_combobox = QComboBox() + marker_units_combobox.currentTextChanged.connect(marker_units_callback) + marker_units_combobox.addItem("Meters") + marker_units_combobox.addItem("Pixels") + + def marker_color_callback(value): + self.image_view.set_color_channel(value) + + marker_color_label = QLabel("Marker color channel:") + marker_color_combobox = QComboBox() + marker_color_combobox.currentTextChanged.connect(marker_color_callback) + marker_color_combobox.addItem("Intensity") + marker_color_combobox.addItem("X") + marker_color_combobox.addItem("Y") + marker_color_combobox.addItem("Z") + + def marker_pixels_callback(value): + self.image_view.set_marker_size_pixels(value) + + marker_pixels_label = QLabel("Marker size (px)") + marker_pixels_spinbox = QSpinBox() + marker_pixels_spinbox.valueChanged.connect(marker_pixels_callback) + marker_pixels_spinbox.setRange(4, 100) + marker_pixels_spinbox.setSingleStep(1) + marker_pixels_spinbox.setValue(6) + + def marker_meters_callback(value): + self.image_view.set_marker_size_meters(value) + + marker_meters_label = QLabel("Marker size (m)") + marker_meters_spinbox = QDoubleSpinBox() + marker_meters_spinbox.valueChanged.connect(marker_meters_callback) + marker_meters_spinbox.setRange(0.01, 1.0) + marker_meters_spinbox.setSingleStep(0.01) + marker_meters_spinbox.setValue(0.05) + + def rainbow_distance_callback(value): + self.image_view.set_rainbow_distance(value) + + rainbow_distance_label = QLabel("Rainbow distance (m)") + rainbow_distance_spinbox = QDoubleSpinBox() + rainbow_distance_spinbox.valueChanged.connect(rainbow_distance_callback) + rainbow_distance_spinbox.setRange(0.0, 1000.0) + rainbow_distance_spinbox.setSingleStep(0.1) + rainbow_distance_spinbox.setValue(10.0) + + def rainbow_offset_callback(value): + self.image_view.set_rainbow_offset(value) + + rainbow_offset_label = QLabel("Rainbow offset") + rainbow_offset_spinbox = QDoubleSpinBox() + rainbow_offset_spinbox.valueChanged.connect(rainbow_offset_callback) + rainbow_offset_spinbox.setRange(0.0, 1.0) + rainbow_offset_spinbox.setSingleStep(0.05) + rainbow_offset_spinbox.setValue(0.0) + + def rendering_alpha_callback(value): + self.image_view.set_rendering_alpha(value) + + rendering_alpha_label = QLabel("Rendering alpha") + rendering_alpha_spinbox = QDoubleSpinBox() + rendering_alpha_spinbox.valueChanged.connect(rendering_alpha_callback) + rendering_alpha_spinbox.setRange(0.0, 1.0) + rendering_alpha_spinbox.setSingleStep(0.05) + rendering_alpha_spinbox.setValue(1.0) + + def marker_subsample_callback(value): + self.image_view.set_subsample_factor(value) + + marker_subsample_label = QLabel("PC subsample factor") + marker_subsample_spinbox = QSpinBox() + marker_subsample_spinbox.valueChanged.connect(marker_subsample_callback) + marker_subsample_spinbox.setRange(1, 10) + marker_subsample_spinbox.setSingleStep(1) + marker_subsample_spinbox.setValue(4) + + def rendering_min_distance_callback(value): + self.image_view.set_min_rendering_distance(value) + + rendering_min_distance_label = QLabel("Min rendering distance (m)") + rendering_min_distance_spinbox = QDoubleSpinBox() + rendering_min_distance_spinbox.valueChanged.connect(rendering_min_distance_callback) + rendering_min_distance_spinbox.setRange(0.01, 100.0) + rendering_min_distance_spinbox.setSingleStep(0.1) + rendering_min_distance_spinbox.setValue(0.1) + + def rendering_max_distance_callback(value): + self.image_view.set_max_rendering_distance(value) + + rendering_max_distance_label = QLabel("Max rendering distance (m)") + rendering_max_distance_spinbox = QDoubleSpinBox() + rendering_max_distance_spinbox.valueChanged.connect(rendering_max_distance_callback) + rendering_max_distance_spinbox.setRange(0.01, 100.0) + rendering_max_distance_spinbox.setSingleStep(0.1) + rendering_max_distance_spinbox.setValue(100.0) + + def render_pointcloud_callback(value): + self.image_view.set_draw_pointcloud(value == Qt.Checked) + + render_pointcloud_checkbox = QCheckBox("Render pointcloud") + render_pointcloud_checkbox.stateChanged.connect(render_pointcloud_callback) + render_pointcloud_checkbox.setChecked(True) + + def render_calibration_points_callback(value): + self.image_view.set_draw_calibration_points(value == Qt.Checked) + + render_calibration_points_checkbox = QCheckBox("Render calibration points") + render_calibration_points_checkbox.stateChanged.connect(render_calibration_points_callback) + render_calibration_points_checkbox.setChecked(True) + + def render_inliers_callback(value): + self.image_view.set_draw_inliers(value == Qt.Checked) + + self.render_inliers_checkbox = QCheckBox("Render inliers") + self.render_inliers_checkbox.stateChanged.connect(render_inliers_callback) + self.render_inliers_checkbox.setChecked(False) + self.render_inliers_checkbox.setEnabled(False) + + visualization_options_layout = QVBoxLayout() + visualization_options_layout.addWidget(tf_source_label) + visualization_options_layout.addWidget(self.tf_source_combobox) + visualization_options_layout.addWidget(marker_type_label) + visualization_options_layout.addWidget(marker_type_combobox) + visualization_options_layout.addWidget(marker_units_label) + visualization_options_layout.addWidget(marker_units_combobox) + visualization_options_layout.addWidget(marker_color_label) + visualization_options_layout.addWidget(marker_color_combobox) + visualization_options_layout.addWidget(render_pointcloud_checkbox) + visualization_options_layout.addWidget(render_calibration_points_checkbox) + visualization_options_layout.addWidget(self.render_inliers_checkbox) + + visualization_options_layout.addWidget(marker_pixels_label) + visualization_options_layout.addWidget(marker_pixels_spinbox) + visualization_options_layout.addWidget(marker_meters_label) + visualization_options_layout.addWidget(marker_meters_spinbox) + visualization_options_layout.addWidget(rainbow_distance_label) + visualization_options_layout.addWidget(rainbow_distance_spinbox) + visualization_options_layout.addWidget(rainbow_offset_label) + visualization_options_layout.addWidget(rainbow_offset_spinbox) + visualization_options_layout.addWidget(rendering_alpha_label) + visualization_options_layout.addWidget(rendering_alpha_spinbox) + visualization_options_layout.addWidget(marker_subsample_label) + visualization_options_layout.addWidget(marker_subsample_spinbox) + visualization_options_layout.addWidget(rendering_min_distance_label) + visualization_options_layout.addWidget(rendering_min_distance_spinbox) + visualization_options_layout.addWidget(rendering_max_distance_label) + visualization_options_layout.addWidget(rendering_max_distance_spinbox) + # visualization_options_layout.addStretch(1) + self.visualization_options_group.setLayout(visualization_options_layout) + + def tf_source_callback(self, string): + string = string.lower() + + if "current" in string: + assert self.current_transform is not None + self.source_transform = self.current_transform + elif "initial" in string: + assert self.initial_transform is not None + self.source_transform = self.initial_transform + elif "calibrator" in string: + self.source_transform = self.calibrated_transform + else: + raise NotImplementedError + + self.image_view.set_transform(self.source_transform) + self.image_view.update() + + def sensor_data_ros_callback(self, img, camera_info, pointcloud, delay): + # This method is executed in the ROS spin thread + with self.lock: + height, width, _ = img.shape + bytes_per_line = 3 * width + q_img = QImage( + img.data, width, height, bytes_per_line, QImage.Format_RGB888 + ).rgbSwapped() + self.pixmap_tmp = QPixmap(q_img) + + self.pointcloud_tmp = pointcloud + self.camera_info_tmp = camera_info + self.delay_tmp = delay + + self.sensor_data_signal.emit() + + def sensor_data_delay_ros_callback(self, delay): + with self.lock: + self.delay_tmp = delay + self.sensor_data_delay_signal.emit(delay) + + def transform_ros_callback(self, transform): + # This method is executed in the ROS spin thread + with self.lock: + self.transform_tmp = transform + + self.transform_signal.emit() + + def external_calibration_points_ros_callback(self, object_points, image_points): + # This method is executed in the ROS spin thread + with self.lock: + self.external_object_calibration_points_tmp = object_points + self.external_image_calibration_points_tmp = image_points + + self.external_calibration_points_signal.emit() + + def sensor_data_callback(self): + # This method is executed in the UI thread + with self.lock: + self.image_view.set_pixmap(self.pixmap_tmp) + self.image_view.set_pointcloud(self.pointcloud_tmp) + + self.camera_info = self.camera_info_tmp + self.image_view.set_camera_info(self.camera_info_tmp.k, self.camera_info_tmp.d) + + self.image_view.update() + self.graphics_view.update() + + self.setWindowTitle(f"Image view (camera-lidar delay={1000*self.delay_tmp:.2f} ms)") + + def sensor_data_delay_callback(self, delay): + # This method is executed in the UI thread + self.setWindowTitle(f"Image view (camera-lidar delay={1000*self.delay_tmp:.2f} ms)") + + def transform_callback(self): + # This method is executed in the UI thread + with self.lock: + if self.initial_transform is None: + self.initial_transform = np.copy(self.transform_tmp) + self.current_transform = self.initial_transform + self.tf_source_combobox.addItem("Initial /tf") + self.tf_source_combobox.addItem("Current /tf") + + self.image_view.update() + + changed = (self.transform_tmp != self.current_transform).any() + self.current_transform = np.copy(self.transform_tmp) + + # the Current /tf case + if "current" in self.tf_source_combobox.currentText().lower() and changed: + self.tf_source_callback(self.tf_source_combobox.currentText()) + + def image_click_callback(self, x, y): + pass + + def external_calibration_points_callback(self): + with self.lock: + self.external_object_calibration_points = copy.deepcopy( + self.external_object_calibration_points_tmp + ) + self.external_image_calibration_points = copy.deepcopy( + self.external_image_calibration_points_tmp + ) + + self.image_view.set_external_calibration_points( + self.external_object_calibration_points, self.external_image_calibration_points_tmp + ) diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/utils.py b/common/tier4_calibration_views/tier4_calibration_views/utils.py similarity index 92% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/utils.py rename to common/tier4_calibration_views/tier4_calibration_views/utils.py index 4ab9d364..81f4a5fb 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/utils.py +++ b/common/tier4_calibration_views/tier4_calibration_views/utils.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2020 Tier IV, Inc. +# 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. @@ -24,9 +24,9 @@ def tf_message_to_transform_matrix(msg): transform_matrix = np.eye(4) q = msg.transform.rotation - rot_matrix = transforms3d.quaternions.quat2mat((q.w, q.x, q.y, q.z)) + rotation_matrix = transforms3d.quaternions.quat2mat((q.w, q.x, q.y, q.z)) - transform_matrix[0:3, 0:3] = rot_matrix + transform_matrix[0:3, 0:3] = rotation_matrix transform_matrix[0, 3] = msg.transform.translation.x transform_matrix[1, 3] = msg.transform.translation.y transform_matrix[2, 3] = msg.transform.translation.z @@ -75,7 +75,7 @@ def decompose_transformation_matrix(transformation): def transform_points(translation_vector, rotation_matrix, point_array): - num_points, dim = point_array.shape + _, dim = point_array.shape assert dim == 3 return np.dot(point_array, np.transpose(rotation_matrix)) + translation_vector.reshape(1, 3) diff --git a/common/tier4_ground_plane_utils/CMakeLists.txt b/common/tier4_ground_plane_utils/CMakeLists.txt new file mode 100755 index 00000000..ad11f581 --- /dev/null +++ b/common/tier4_ground_plane_utils/CMakeLists.txt @@ -0,0 +1,24 @@ + +cmake_minimum_required(VERSION 3.5) +project(tier4_ground_plane_utils) + +find_package(autoware_cmake REQUIRED) + +autoware_package() + +ament_auto_add_library(tier4_ground_plane_utils_lib SHARED + src/ground_plane_utils.cpp +) + +target_link_libraries(tier4_ground_plane_utils_lib +) + +target_include_directories(tier4_ground_plane_utils_lib + PUBLIC + include) + +ament_export_include_directories( + include +) + +ament_auto_package() diff --git a/common/tier4_ground_plane_utils/include/tier4_ground_plane_utils/ground_plane_utils.hpp b/common/tier4_ground_plane_utils/include/tier4_ground_plane_utils/ground_plane_utils.hpp new file mode 100644 index 00000000..5e64b2e9 --- /dev/null +++ b/common/tier4_ground_plane_utils/include/tier4_ground_plane_utils/ground_plane_utils.hpp @@ -0,0 +1,133 @@ +// 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. + +#ifndef TIER4_GROUND_PLANE_UTILS__GROUND_PLANE_UTILS_HPP_ +#define TIER4_GROUND_PLANE_UTILS__GROUND_PLANE_UTILS_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace tier4_ground_plane_utils +{ + +using PointType = pcl::PointXYZ; + +struct GroundPlaneExtractorParameters +{ + bool verbose_; + bool use_crop_box_filter_; + double crop_box_min_x_; + double crop_box_min_y_; + double crop_box_min_z_; + double crop_box_max_x_; + double crop_box_max_y_; + double crop_box_max_z_; + bool use_pca_rough_normal_; + double max_inlier_distance_; + int min_plane_points_; + int min_plane_points_percentage_; + double max_cos_distance_; + int max_iterations_; + bool remove_outliers_; + double remove_outlier_tolerance_; + Eigen::Isometry3d initial_base_to_lidar_transform_; +}; + +/*! + * Extracts the ground plane from a pointcloud + * @param[in] pointcloud the input pointcloud + * @return A tuple containing wether or not th calibration plane was found, the estimated ground + * plane model, and the inliers of the respective model + */ +std::tuple::Ptr> extractGroundPlane( + pcl::PointCloud::Ptr & pointcloud, const GroundPlaneExtractorParameters & parameters, + std::vector & outlier_models); + +/*! + * Extracts a plane from a pointcloud + * @param[in] pointcloud the input pointcloud + * @param[in] max_inlier_distance maximum allowed inlier distance + * @param[in] max_iterations max iterations for the ransac algorithm + * @return A tuple containing a plane model and the inlier indices + */ +std::pair extractPlane( + pcl::PointCloud::Ptr pointcloud, double max_inlier_distance, int max_iterations); + +/*! + * Computes the fitting error of an estimated model and the initial one + * @param[in] estimated_model the estimated model + * @param[in] inliers the inliers of the current estimated model + */ +void evaluateModels( + const Eigen::Vector4d & initial_model, const Eigen::Vector4d & estimated_model, + pcl::PointCloud::Ptr inliers); + +/*! + * Computes a plane model given a pose. + * The normal of the plane is given by the z-axis of the rotation of the pose + * @param[in] pointcloud Point cloud to crop + * @param[in] max_range Range to crop the pointcloud to + * @return the plane model + */ +Eigen::Vector4d poseToPlaneModel(const Eigen::Isometry3d & pose); + +/*! + * Compute a pose from a plane model a*x + b*y +c*z +d = 0 + * The pose lies has its origin on the z-projection of the plane + * @param[in] model Point cloud to crop + * @return the plane pose + */ +Eigen::Isometry3d modelPlaneToPose(const Eigen::Vector4d & model); + +/*! + * Estimate / refine a lidar-base transform given an initial guess and an estimated ground plane + * @param[in] base_lidar_transform Initial base lidar transform + * @param[in] ground_plane_model ground plane model + * @return the refined base lidar pose + */ +Eigen::Isometry3d estimateBaseLidarTransform( + const Eigen::Isometry3d & initial_base_lidar_transform, const Eigen::Vector4d & model); + +/*! + * Removes the point that are consistent with an input plane from the pointcloud + * @param[in] input_pointcloud the pointcloud to filter + * @param[in] outlier_model the model that represents the outliers + * @param[in] outlier_tolerance the tolerance with which a point is still considered an outlier + * @return the refined base lidar pose + */ +pcl::PointCloud::Ptr removeOutliers( + pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, + double outlier_tolerance); + +/*! + * Overwrite the calibrated x, y, and yaw values of the calibrated base lidar transform with the + * initial ones + * @param[in] initial_base_lidar_transform_msg the initial base lidar transform msg + * @param[in] calibrated_base_lidar_transform_msg the calibrated base lidar transform msg + * @return the calibrated base lidar transform with its x, y, and yaw values being overwritten by + * the initial ones + */ +geometry_msgs::msg::TransformStamped overwriteXYYawValues( + const geometry_msgs::msg::TransformStamped & initial_base_lidar_transform_msg, + const geometry_msgs::msg::TransformStamped & calibrated_base_lidar_transform_msg); + +} // namespace tier4_ground_plane_utils + +#endif // TIER4_GROUND_PLANE_UTILS__GROUND_PLANE_UTILS_HPP_ diff --git a/sensor/extrinsic_dummy_calibrator/package.xml b/common/tier4_ground_plane_utils/package.xml old mode 100644 new mode 100755 similarity index 63% rename from sensor/extrinsic_dummy_calibrator/package.xml rename to common/tier4_ground_plane_utils/package.xml index 3b86ddaa..89e99232 --- a/sensor/extrinsic_dummy_calibrator/package.xml +++ b/common/tier4_ground_plane_utils/package.xml @@ -1,29 +1,31 @@ - extrinsic_dummy_calibrator - 0.1.0 - The extrinsic_dummy_calibrator package + tier4_ground_plane_utils + 0.0.1 + The tier4_ground_plane_utils package Kenzo Lobos Tsunekawa - Apache License 2.0 + + BSD ament_cmake_auto + autoware_cmake + eigen geometry_msgs - message_filters pcl_conversions pcl_ros rclcpp sensor_msgs std_msgs tf2 + tf2_eigen tf2_geometry_msgs - tier4_calibration_msgs - - ament_lint_auto - ament_lint_common + tf2_ros + tier4_autoware_utils + ament_cmake diff --git a/common/tier4_ground_plane_utils/src/ground_plane_utils.cpp b/common/tier4_ground_plane_utils/src/ground_plane_utils.cpp new file mode 100644 index 00000000..edd60bb9 --- /dev/null +++ b/common/tier4_ground_plane_utils/src/ground_plane_utils.cpp @@ -0,0 +1,375 @@ +// 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. + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tier4_ground_plane_utils +{ + +std::tuple::Ptr> extractGroundPlane( + pcl::PointCloud::Ptr & pointcloud, const GroundPlaneExtractorParameters & parameters, + std::vector & outlier_models) +{ + Eigen::Vector4d model; + pcl::PointCloud::Ptr inliers_pointcloud(new pcl::PointCloud); + + if (parameters.use_crop_box_filter_) { + pcl::CropBox boxFilter; + boxFilter.setMin(Eigen::Vector4f( + parameters.crop_box_min_x_, parameters.crop_box_min_y_, parameters.crop_box_min_z_, 1.0)); + boxFilter.setMax(Eigen::Vector4f( + parameters.crop_box_max_x_, parameters.crop_box_max_y_, parameters.crop_box_max_z_, 1.0)); + boxFilter.setInputCloud(pointcloud); + boxFilter.filter(*pointcloud); + } + + std::vector models; + Eigen::Vector3f rough_normal; + + if (parameters.use_pca_rough_normal_) { + // Obtain an idea of the ground plane using PCA + // under the assumption that the axis with less variance will be the ground plane normal + pcl::PCA pca; + pca.setInputCloud(pointcloud); + Eigen::MatrixXf vectors = pca.getEigenVectors(); + rough_normal = vectors.col(2); + } else { + rough_normal = (parameters.initial_base_to_lidar_transform_.inverse().rotation() * + Eigen::Vector3d(0.0, 0.0, 1.0)) + .cast(); + } + + if (parameters.verbose_) { + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), "Rough plane normal. x=%.3f, y=%.3f, z=%.3f", + rough_normal.x(), rough_normal.y(), rough_normal.z()); + } + + // Use RANSAC iteratively until we find the ground plane + // Since walls can have more points, we filter using the PCA-based hypothesis + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::SACSegmentation seg; + pcl::ExtractIndices extract; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(parameters.max_inlier_distance_); + seg.setMaxIterations(parameters.max_iterations_); + + pcl::PointCloud::Ptr iteration_cloud = pointcloud; + int iteration_size = iteration_cloud->height * iteration_cloud->width; + + while (iteration_size > parameters.min_plane_points_) { + seg.setInputCloud(iteration_cloud); + seg.segment(*inliers, *coefficients); + + if (inliers->indices.size() == 0) { + if (parameters.verbose_) { + RCLCPP_WARN(rclcpp::get_logger("ground_plane_utils"), "No plane found in the pointcloud"); + } + + break; + } + + Eigen::Vector3f normal( + coefficients->values[0], coefficients->values[1], coefficients->values[2]); + float cos_distance = 1.0 - std::abs(rough_normal.dot(normal)); + + model = Eigen::Vector4d( + coefficients->values[0], coefficients->values[1], coefficients->values[2], + coefficients->values[3]); + + int inlier_size = static_cast(inliers->indices.size()); + double inlier_percentage = 100.0 * inlier_size / pointcloud->size(); + + if ( + inlier_size > parameters.min_plane_points_ && + inlier_percentage > parameters.min_plane_points_percentage_ && + cos_distance < parameters.max_cos_distance_) { + if (parameters.verbose_) { + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), "Plane found: inliers=%ld (%.3f)", + inliers->indices.size(), inlier_percentage); + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), "Plane model. a=%.3f, b=%.3f, c=%.3f, d=%.3f", + model(0), model(1), model(2), model(3)); + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), "Cos distance: %.3f / %.3f", cos_distance, + parameters.max_cos_distance_); + } + + // Extract the ground plane inliers + extract.setInputCloud(iteration_cloud); + extract.setIndices(inliers); + extract.setNegative(false); + extract.filter(*inliers_pointcloud); + + return std::make_tuple(true, model, inliers_pointcloud); + } else { + if (parameters.remove_outliers_) { + bool accept = true; + + for (const auto & outlier_model : outlier_models) { + Eigen::Vector3f outlier_normal(outlier_model.x(), outlier_model.y(), outlier_model.z()); + float cos_distance = 1.0 - std::abs(outlier_normal.dot(normal)); + + if ( + cos_distance < parameters.max_cos_distance_ && + std::abs(outlier_model.w() - model.w()) < parameters.remove_outlier_tolerance_) { + accept = false; + } + } + + if (accept) { + outlier_models.push_back(model); + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), + "New outlier model: a=%.3f, b=%.3f, c=%.3f, d=%.3f", model(0), model(1), model(2), + model(3)); + } + } + + if (parameters.verbose_) { + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), + "Iteration failed. model: a=%.3f, b=%.3f, c=%.3f, d=%.3f inliers=%lu inlier " + "percentage=%.2f cos_distance=%.2f", + model(0), model(1), model(2), model(3), inliers->indices.size(), inlier_percentage, + cos_distance); + } + } + + // Extract the inliers from the pointcloud (the detected plane was not the ground plane) + extract.setInputCloud(iteration_cloud); + extract.setIndices(inliers); + extract.setNegative(true); + + pcl::PointCloud next_cloud; + extract.filter(next_cloud); + + iteration_cloud->swap(next_cloud); + iteration_size = iteration_cloud->height * iteration_cloud->width; + } + return std::make_tuple(false, model, inliers_pointcloud); +} + +std::pair extractPlane( + pcl::PointCloud::Ptr pointcloud, double max_inlier_distance, int max_iterations) +{ + // cSpell:ignore SACMODEL + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr final_inliers(new pcl::PointIndices); + pcl::SACSegmentation seg; + pcl::ExtractIndices extract; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(max_inlier_distance); + seg.setMaxIterations(max_iterations); + + seg.setInputCloud(pointcloud); + seg.segment(*final_inliers, *coefficients); + + Eigen::Vector4d model( + coefficients->values[0], coefficients->values[1], coefficients->values[2], + coefficients->values[3]); + + return std::make_pair(model, final_inliers); +} + +void evaluateModels( + const Eigen::Vector4d & initial_model, const Eigen::Vector4d & estimated_model, + pcl::PointCloud::Ptr inliers) +{ + auto model_error = + [](float a, float b, float c, float d, pcl::PointCloud::Ptr pc) -> float { + assert(std::abs(a * a + b * b + c * c - 1.f) < 1e-5); + float sum = 0.f; + for (auto & p : pc->points) { + sum += std::abs(a * p.x + b * p.y + c * p.z + d); + } + return sum / (pc->height * pc->width); + }; + + float initial_model_error = + model_error(initial_model(0), initial_model(1), initial_model(2), initial_model(3), inliers); + + float estimated_model_error = model_error( + estimated_model(0), estimated_model(1), estimated_model(2), estimated_model(3), inliers); + + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), "Initial calibration error: %3f m", + initial_model_error); + RCLCPP_INFO( + rclcpp::get_logger("ground_plane_utils"), "Estimated calibration error: %3f m", + estimated_model_error); +} + +Eigen::Vector4d poseToPlaneModel(const Eigen::Isometry3d & pose) +{ + Eigen::Vector3d normal_vector_base( + 0.0, 0.0, 1.0); // We use a +z for the normal of the plane. TODO: confirm if PCL does the same + Eigen::Vector3d normal_vector_lidar = pose.rotation() * normal_vector_base; + + Eigen::Vector4d model; // (a, b, c, d) from a*x + b*y + c*z + d = 0 + model(0) = normal_vector_lidar.x(); + model(1) = normal_vector_lidar.y(); + model(2) = normal_vector_lidar.z(); + model(3) = -normal_vector_lidar.dot(pose.translation()); + + return model; +} + +Eigen::Isometry3d modelPlaneToPose(const Eigen::Vector4d & model) +{ + Eigen::Vector3d n(model(0), model(1), model(2)); + n.normalize(); + + Eigen::Vector3d x0 = -n * model(3); + + // To create a real pose we need to invent a basis + Eigen::Vector3d base_x, base_y, base_z; + base_z = n; + + Eigen::Vector3d c1 = Eigen::Vector3d(1.0, 0.0, 0.0).cross(n); + Eigen::Vector3d c2 = Eigen::Vector3d(0.0, 1.0, 0.0).cross(n); + Eigen::Vector3d c3 = Eigen::Vector3d(0.0, 0.0, 1.0).cross(n); + + // Any non-zero would work but we use the one with the highest norm (there has to be a non zero) + if (c1.norm() > c2.norm() && c1.norm() > c3.norm()) { + base_x = c1; + } else if (c2.norm() > c3.norm()) { + base_x = c2; + } else { + base_x = c3; + } + + base_y = base_z.cross(base_x); + + Eigen::Matrix3d rot; + rot.col(0) = base_x.normalized(); + rot.col(1) = base_y.normalized(); + rot.col(2) = base_z.normalized(); + + Eigen::Isometry3d pose; + pose.translation() = x0; + pose.linear() = rot; + + return pose; +} + +Eigen::Isometry3d estimateBaseLidarTransform( + const Eigen::Isometry3d & initial_base_to_lidar_transform, const Eigen::Vector4d & model) +{ + const Eigen::Isometry3d lidar_to_initial_base_transform = + initial_base_to_lidar_transform.inverse(); + const Eigen::Isometry3d lidar_to_ground_transform = modelPlaneToPose(model); + + const Eigen::Isometry3d ground_to_initial_base_transform = + lidar_to_ground_transform.inverse() * lidar_to_initial_base_transform; + + Eigen::Vector3d ground_to_initial_base_projected_translation = + ground_to_initial_base_transform.translation(); + ground_to_initial_base_projected_translation.z() = 0; + + Eigen::Vector3d ground_to_initial_base_projected_rotation_x = + ground_to_initial_base_transform.rotation().col(0); + ground_to_initial_base_projected_rotation_x.z() = 0.0; + ground_to_initial_base_projected_rotation_x.normalize(); + + Eigen::Matrix3d ground_to_initial_base_projected_rotation; + ground_to_initial_base_projected_rotation.col(2) = Eigen::Vector3d(0.0, 0.0, 1.0); + ground_to_initial_base_projected_rotation.col(0) = ground_to_initial_base_projected_rotation_x; + ground_to_initial_base_projected_rotation.col(1) = + ground_to_initial_base_projected_rotation.col(2).cross( + ground_to_initial_base_projected_rotation.col(0)); + + Eigen::Isometry3d ground_to_estimated_base_transform; + ground_to_estimated_base_transform.translation() = ground_to_initial_base_projected_translation; + ground_to_estimated_base_transform.linear() = ground_to_initial_base_projected_rotation; + + return ground_to_estimated_base_transform.inverse() * lidar_to_ground_transform.inverse(); +} + +pcl::PointCloud::Ptr removeOutliers( + pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, + double outlier_tolerance) +{ + pcl::ExtractIndices extract; + pcl::PointCloud::Ptr inliers(new pcl::PointCloud); + pcl::PointIndices::Ptr outliers(new pcl::PointIndices); + + for (std::size_t index = 0; index < input_pointcloud->size(); index++) { + const auto & p = input_pointcloud->points[index]; + double error = std::abs( + outlier_plane_model.x() * p.x + outlier_plane_model.y() * p.y + + outlier_plane_model.z() * p.z + outlier_plane_model.w()); + + if (error < outlier_tolerance) { + outliers->indices.push_back(index); + } + } + + // Extract the inliers from the pointcloud (the detected plane was not the ground plane) + extract.setInputCloud(input_pointcloud); + extract.setIndices(outliers); + extract.setNegative(true); + extract.filter(*inliers); + + return inliers; +} + +geometry_msgs::msg::TransformStamped overwriteXYYawValues( + const geometry_msgs::msg::TransformStamped & initial_base_lidar_transform_msg, + const geometry_msgs::msg::TransformStamped & calibrated_base_lidar_transform_msg) +{ + geometry_msgs::msg::TransformStamped msg = calibrated_base_lidar_transform_msg; + + // Overwrite xy + msg.transform.translation.x = initial_base_lidar_transform_msg.transform.translation.x; + msg.transform.translation.y = initial_base_lidar_transform_msg.transform.translation.y; + + auto initial_rpy = + tier4_autoware_utils::getRPY(initial_base_lidar_transform_msg.transform.rotation); + + auto calibrated_rpy = + tier4_autoware_utils::getRPY(calibrated_base_lidar_transform_msg.transform.rotation); + + // Overwrite only yaw + auto output_rpy = calibrated_rpy; + output_rpy.z = initial_rpy.z; + + msg.transform.rotation = + tier4_autoware_utils::createQuaternionFromRPY(output_rpy.x, output_rpy.y, output_rpy.z); + return msg; +} + +} // namespace tier4_ground_plane_utils diff --git a/common/tier4_tag_utils/CMakeLists.txt b/common/tier4_tag_utils/CMakeLists.txt index 772639d3..f80b013a 100755 --- a/common/tier4_tag_utils/CMakeLists.txt +++ b/common/tier4_tag_utils/CMakeLists.txt @@ -8,7 +8,6 @@ find_package(OpenCV REQUIRED) autoware_package() ament_auto_add_library(tier4_tag_utils_lib SHARED - src/cv/sqpnp.cpp src/lidartag_hypothesis.cpp src/apriltag_hypothesis.cpp ) diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_filter.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_filter.hpp index 3add1cc0..c258cc35 100644 --- a/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_filter.hpp +++ b/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -47,9 +47,9 @@ class ApriltagFilter : public rclcpp::Node std::unordered_map tag_sizes_map_; double max_no_observation_time_; - double new_hypothesis_transl_; - double measurement_noise_transl_; - double process_noise_transl_; + double new_hypothesis_translation_; + double measurement_noise_translation_; + double process_noise_translation_; double min_tag_size_; double max_tag_distance_; double max_allowed_homography_error_; diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_hypothesis.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_hypothesis.hpp index 3f3ec532..9b045314 100644 --- a/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_hypothesis.hpp +++ b/common/tier4_tag_utils/include/tier4_tag_utils/apriltag_hypothesis.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -19,7 +19,6 @@ #include #include #include -#include #include @@ -57,39 +56,33 @@ class ApriltagHypothesis bool converged() const; - void setDynamicsModel(DynamicsModel dynamics_model); void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); - void setMaxConvergenceThreshold(double transl); - void setNewHypothesisThreshold(double transl); - void setMeasurementNoise(double transl); - void setProcessNoise(double transl); + void setMaxConvergenceThreshold(double translation); + void setNewHypothesisThreshold(double translation); + void setMeasurementNoise(double translation); + void setProcessNoise(double translation); void setTagSize(double size); protected: void reset(); - void initKalman(const std::vector & corners); - - void initStaticKalman(const std::vector & corners); - cv::Mat toState(const cv::Point2d & corner); - double convergence_transl_; - double new_hypothesis_transl_; + double convergence_translation_; + double new_hypothesis_translation_; double min_convergence_time_; double max_no_observation_time_; double tag_size_; // Kalman related cv::KalmanFilter kalman_filters_[4]; - double process_noise_transl_; + double process_noise_translation_; - double measurement_noise_transl_; + double measurement_noise_translation_; // General variables bool first_observation_; - DynamicsModel dynamics_model_; int id_; rclcpp::Time first_observation_timestamp_; rclcpp::Time last_observation_timestamp_; diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/cv/LICENSE.txt b/common/tier4_tag_utils/include/tier4_tag_utils/cv/LICENSE.txt deleted file mode 100644 index d6456956..00000000 --- a/common/tier4_tag_utils/include/tier4_tag_utils/cv/LICENSE.txt +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/cv/_precomp.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/cv/_precomp.hpp deleted file mode 100644 index 4972a20d..00000000 --- a/common/tier4_tag_utils/include/tier4_tag_utils/cv/_precomp.hpp +++ /dev/null @@ -1,139 +0,0 @@ -/*M/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. -// Copyright (C) 2009, Willow Garage Inc., all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of the copyright holders may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors "as is" and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the Intel Corporation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -//M*/ -#ifndef TIER4_TAG_UTILS__CV___PRECOMP_HPP_ -#define TIER4_TAG_UTILS__CV___PRECOMP_HPP_ - -#include "opencv2/core/utility.hpp" - -// #include "opencv2/core/private.hpp" - -#include "opencv2/calib3d.hpp" -#include "opencv2/core/ocl.hpp" -#include "opencv2/features2d.hpp" -#include "opencv2/imgproc.hpp" - -#define GET_OPTIMIZED(func) (func) - -namespace cv -{ -/** - * Compute the number of iterations given the confidence, outlier ratio, number - * of model points and the maximum iteration number. - * - * @param p confidence value - * @param ep outlier ratio - * @param modelPoints number of model points required for estimation - * @param maxIters maximum number of iterations - * @return - * \f[ - * \frac{\ln(1-p)}{\ln\left(1-(1-ep)^\mathrm{modelPoints}\right)} - * \f] - * - * If the computed number of iterations is larger than maxIters, then maxIters is returned. - */ -int RANSACUpdateNumIters(double p, double ep, int modelPoints, int maxIters); - -class CV_EXPORTS PointSetRegistrator : public Algorithm -{ -public: - class CV_EXPORTS Callback - { - public: - virtual ~Callback() {} - virtual int runKernel(InputArray m1, InputArray m2, OutputArray model) const = 0; - virtual void computeError( - InputArray m1, InputArray m2, InputArray model, OutputArray err) const = 0; - virtual bool checkSubset(InputArray, InputArray, int) const { return true; } - }; - - virtual void setCallback(const Ptr & cb) = 0; - virtual bool run(InputArray m1, InputArray m2, OutputArray model, OutputArray mask) const = 0; -}; - -CV_EXPORTS Ptr createRANSACPointSetRegistrator( - const Ptr & cb, int modelPoints, double threshold, - double confidence = 0.99, int maxIters = 1000); - -CV_EXPORTS Ptr createLMeDSPointSetRegistrator( - const Ptr & cb, int modelPoints, double confidence = 0.99, - int maxIters = 1000); - -template -inline int compressElems(T * ptr, const uchar * mask, int mstep, int count) -{ - int i, j; - for (i = j = 0; i < count; i++) - if (mask[i * mstep]) { - if (i > j) ptr[j] = ptr[i]; - j++; - } - return j; -} - -static inline bool haveCollinearPoints(const Mat & m, int count) -{ - int j, k, i = count - 1; - const Point2f * ptr = m.ptr(); - - // check that the i-th selected point does not belong - // to a line connecting some previously selected points - // also checks that points are not too close to each other - for (j = 0; j < i; j++) { - double dx1 = ptr[j].x - ptr[i].x; - double dy1 = ptr[j].y - ptr[i].y; - for (k = 0; k < j; k++) { - double dx2 = ptr[k].x - ptr[i].x; - double dy2 = ptr[k].y - ptr[i].y; - if ( - fabs(dx2 * dy1 - dy2 * dx1) <= - FLT_EPSILON * (fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2))) - return true; - } - } - return false; -} - -} // namespace cv - -int checkChessboardBinary(const cv::Mat & img, const cv::Size & size); - -#endif // TIER4_TAG_UTILS__CV___PRECOMP_HPP_ diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/cv/sqpnp.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/cv/sqpnp.hpp deleted file mode 100644 index 8d72479b..00000000 --- a/common/tier4_tag_utils/include/tier4_tag_utils/cv/sqpnp.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// This file is part of OpenCV project. -// It is subject to the license terms in the LICENSE file found in the top-level directory -// of this distribution and at http://opencv.org/license.html - -// This file is based on file issued with the following license: - -/* -BSD 3-Clause License - -Copyright (c) 2020, George Terzakis -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef TIER4_TAG_UTILS__CV__SQPNP_HPP_ -#define TIER4_TAG_UTILS__CV__SQPNP_HPP_ - -#include -#include - -namespace cv -{ -namespace sqpnp -{ - -class PoseSolver -{ -public: - /** - * @brief PoseSolver constructor - */ - PoseSolver(); - - /** - * @brief Finds the possible poses of a camera given a set of 3D points - * and their corresponding 2D image projections. The poses are - * sorted by lowest squared error (which corresponds to lowest - * reprojection error). - * @param objectPoints Array or vector of 3 or more 3D points defined in object coordinates. - * 1xN/Nx1 3-channel (float or double) where N is the number of points. - * @param imagePoints Array or vector of corresponding 2D points, 1xN/Nx1 2-channel. - * @param rvec The output rotation solutions (up to 18 3x1 rotation vectors) - * @param tvec The output translation solutions (up to 18 3x1 vectors) - */ - void solve( - InputArray objectPoints, InputArray imagePoints, OutputArrayOfArrays rvec, - OutputArrayOfArrays tvec); - -private: - struct SQPSolution - { - cv::Matx r_hat; - cv::Matx t; - double sq_error; - SQPSolution() : sq_error(0) {} - }; - - /* - * @brief Computes the 9x9 PSD Omega matrix and supporting matrices. - * @param objectPoints Array or vector of 3 or more 3D points defined in object coordinates. - * 1xN/Nx1 3-channel (float or double) where N is the number of points. - * @param imagePoints Array or vector of corresponding 2D points, 1xN/Nx1 2-channel. - */ - void computeOmega(InputArray objectPoints, InputArray imagePoints); - - /* - * @brief Computes the 9x9 PSD Omega matrix and supporting matrices. - */ - void solveInternal(); - - /* - * @brief Produces the distance from being orthogonal for a given 3x3 matrix - * in row-major form. - * @param e The vector to test representing a 3x3 matrix in row major form. - * @return The distance the matrix is from being orthogonal. - */ - static double orthogonalityError(const cv::Matx & e); - - /* - * @brief Processes a solution and sorts it by error. - * @param solution The solution to evaluate. - * @param min_error The current minimum error. - */ - void checkSolution(SQPSolution & solution, double & min_error); - - /* - * @brief Computes the determinant of a matrix stored in row-major format. - * @param e Vector representing a 3x3 matrix stored in row-major format. - * @return The determinant of the matrix. - */ - static double det3x3(const cv::Matx & e); - - /* - * @brief Tests the cheirality for a given solution. - * @param solution The solution to evaluate. - */ - inline bool positiveDepth(const SQPSolution & solution) const; - - /* - * @brief Determines the nearest rotation matrix to a given rotaiton matrix. - * Input and output are 9x1 vector representing a vector stored in row-major - * form. - * @param e The input 3x3 matrix stored in a vector in row-major form. - * @param r The nearest rotation matrix to the input e (again in row-major form). - */ - static void nearestRotationMatrix(const cv::Matx & e, cv::Matx & r); - - /* - * @brief Runs the sequential quadratic programming on orthogonal matrices. - * @param r0 The start point of the solver. - */ - SQPSolution runSQP(const cv::Matx & r0); - - /* - * @brief Steps down the gradient for the given matrix r to solve the SQP system. - * @param r The current matrix step. - * @param delta The next step down the gradient. - */ - void solveSQPSystem(const cv::Matx & r, cv::Matx & delta); - - /* - * @brief Analytically computes the inverse of a symmetric 3x3 matrix using the - * lower triangle. - * @param Q The matrix to invert. - * @param Qinv The inverse of Q. - * @param threshold The threshold to determine if Q is singular and non-invertible. - */ - bool analyticalInverse3x3Symm( - const cv::Matx & Q, cv::Matx & Qinv, - const double & threshold = 1e-8); - - /* - * @brief Computes the 3D null space and 6D normal space of the constraint Jacobian - * at a 9D vector r (representing a rank-3 matrix). Note that K is lower - * triangular so upper triangle is undefined. - * @param r 9D vector representing a rank-3 matrix. - * @param H 6D row space of the constraint Jacobian at r. - * @param N 3D null space of the constraint Jacobian at r. - * @param K The constraint Jacobian at r. - * @param norm_threshold Threshold for column vector norm of Pn (the projection onto the null - * space of the constraint Jacobian). - */ - void computeRowAndNullspace( - const cv::Matx & r, cv::Matx & H, cv::Matx & N, - cv::Matx & K, const double & norm_threshold = 0.1); - - static const double RANK_TOLERANCE; - static const double SQP_SQUARED_TOLERANCE; - static const double SQP_DET_THRESHOLD; - static const double ORTHOGONALITY_SQUARED_ERROR_THRESHOLD; - static const double EQUAL_VECTORS_SQUARED_DIFF; - static const double EQUAL_SQUARED_ERRORS_DIFF; - static const double POINT_VARIANCE_THRESHOLD; - static const int SQP_MAX_ITERATION; - static const double SQRT3; - - cv::Matx omega_; - cv::Vec s_; - cv::Matx u_; - cv::Matx p_; - cv::Vec3d point_mean_; - int num_null_vectors_; - - SQPSolution solutions_[18]; - int num_solutions_; -}; - -} // namespace sqpnp -} // namespace cv - -#endif // TIER4_TAG_UTILS__CV__SQPNP_HPP_ diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_filter.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_filter.hpp index 344d9a2d..096a2f49 100644 --- a/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_filter.hpp +++ b/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -46,14 +46,14 @@ class LidartagFilter : public rclcpp::Node // Parameters double max_no_observation_time_; double new_hypothesis_distance_; - double new_hypothesis_transl_; - double new_hypothesis_rot_; - double measurement_noise_transl_; - double measurement_noise_rot_; - double process_noise_transl_; - double process_noise_transl_dot_; - double process_noise_rot_; - double process_noise_rot_dot_; + double new_hypothesis_translation_; + double new_hypothesis_rotation_; + double measurement_noise_translation_; + double measurement_noise_rotation_; + double process_noise_translation_; + double process_noise_translation_dot_; + double process_noise_rotation_; + double process_noise_rotation_dot_; // ROS Interface rclcpp::Publisher::SharedPtr pub_; diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp index 785decb3..3a89e3ce 100644 --- a/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp +++ b/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -19,7 +19,6 @@ #include #include #include -#include #include @@ -48,31 +47,27 @@ class LidartagHypothesis cv::Point3d getCenter() const; double getTransCov() const; - double getTransDotCov() const; double getRotCov() const; - double getRotDotCov() const; double getSpeed() const; bool converged() const; double timeSinceFirstObservation(const rclcpp::Time & stamp) const; double timeSinceLastObservation(const rclcpp::Time & stamp) const; - void setDynamicsModel(DynamicsModel dynamics_mode); void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); - void setMaxConvergenceThreshold(double transl, double tansl_dot, double angle, double angle_dot); - void setNewHypothesisThreshold(double transl, double angle); - void setMeasurementNoise(double transl, double angle); - void setProcessNoise(double transl, double transl_dot, double rot, double rot_dot); + void setMaxConvergenceThreshold( + double translation, double translation_dot, double angle, double angle_dot); + void setNewHypothesisThreshold(double translation, double angle); + void setMeasurementNoise(double translation, double angle); + void setProcessNoise( + double translation, double translation_dot, double rotation, double rotation_dot); protected: void reset(); void initKalman(const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix); - void initStaticKalman( - const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix); - void initConstantVelocityKalman( const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix); @@ -87,26 +82,24 @@ class LidartagHypothesis cv::Matx31d rot2euler(const cv::Matx33d & rotation_matrix); cv::Matx33d euler2rot(const cv::Matx31d & euler); - DynamicsModel dynamics_model_; - - double convergence_transl_; - double convergence_transl_dot_; - double convergence_rot_; - double convergence_rot_dot_; - double new_hypothesis_transl_; - double new_hypothesis_rot_; + double convergence_translation_; + double convergence_translation_dot_; + double convergence_rotation_; + double convergence_rotation_dot_; + double new_hypothesis_translation_; + double new_hypothesis_rotation_; double min_convergence_time_; double max_no_observation_time_; // Kalman related cv::KalmanFilter kalman_filter_; - double process_noise_transl_; - double process_noise_transl_dot_; - double process_noise_rot_; - double process_noise_rot_dot_; + double process_noise_translation_; + double process_noise_translation_dot_; + double process_noise_rotation_; + double process_noise_rotation_dot_; - double measurement_noise_transl_; - double measurement_noise_rot_; + double measurement_noise_translation_; + double measurement_noise_rotation_; // General variables int id_; diff --git a/common/tier4_tag_utils/include/tier4_tag_utils/types.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/types.hpp deleted file mode 100644 index 62abb881..00000000 --- a/common/tier4_tag_utils/include/tier4_tag_utils/types.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2023 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. - -#ifndef TIER4_TAG_UTILS__TYPES_HPP_ -#define TIER4_TAG_UTILS__TYPES_HPP_ - -namespace tier4_tag_utils -{ - -enum class EstimationMode { - NoFiltering, - SingleStepEstimationKalmanFiltering, - MultiStepEstimationKalmanFiltering -}; - -enum class DynamicsModel { - Static, - ConstantVelocity, -}; - -} // namespace tier4_tag_utils - -#endif // TIER4_TAG_UTILS__TYPES_HPP_ diff --git a/common/tier4_tag_utils/src/apriltag_filter.cpp b/common/tier4_tag_utils/src/apriltag_filter.cpp index 060b0678..54ac040a 100644 --- a/common/tier4_tag_utils/src/apriltag_filter.cpp +++ b/common/tier4_tag_utils/src/apriltag_filter.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,24 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include #include -#include -#include - -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else #include +#include #include -#endif -#include -#include -#include +#include namespace tier4_tag_utils { @@ -44,21 +36,23 @@ ApriltagFilter::ApriltagFilter(const rclcpp::NodeOptions & options) min_margin_ = this->declare_parameter("min_margin"); ; max_no_observation_time_ = this->declare_parameter("max_no_observation_time"); - new_hypothesis_transl_ = this->declare_parameter("new_hypothesis_transl"); - measurement_noise_transl_ = this->declare_parameter("measurement_noise_transl"); - process_noise_transl_ = this->declare_parameter("process_noise_transl"); + new_hypothesis_translation_ = this->declare_parameter("new_hypothesis_translation"); + measurement_noise_translation_ = this->declare_parameter("measurement_noise_translation"); + process_noise_translation_ = this->declare_parameter("process_noise_translation"); std::vector tag_families = this->declare_parameter>("tag_families"); // std::vector tag_ids = this->declare_parameter>("tag_ids"); - std::vector tag_sizes = this->declare_parameter>("tag_sizes"); + std::vector tag_sizes = + this->declare_parameter>("tag_sizes", std::vector{}); - if (tag_families.size() != tag_sizes.size()) { + if (tag_sizes.size() > 0 && tag_families.size() != tag_sizes.size()) { throw std::invalid_argument("Tag ids and sizes must be of the same size"); } for (std::size_t i = 0; i < tag_families.size(); i++) { - tag_sizes_map_["tag" + tag_families[i]] = tag_sizes[i]; + double size = i < tag_sizes.size() ? tag_sizes[i] : 0.0; + tag_sizes_map_["tag" + tag_families[i]] = size; } camera_info_sub_ = this->create_subscription( @@ -150,10 +144,10 @@ void ApriltagFilter::detectionsCallback( auto & h = hypotheses_map_[family_and_id]; h.setMaxConvergenceThreshold(0.0); h.setMaxNoObservationTime(max_no_observation_time_); - h.setMeasurementNoise(measurement_noise_transl_); + h.setMeasurementNoise(measurement_noise_translation_); h.setMinConvergenceTime(std::numeric_limits::max()); - h.setNewHypothesisThreshold(new_hypothesis_transl_); - h.setProcessNoise(process_noise_transl_); + h.setNewHypothesisThreshold(new_hypothesis_translation_); + h.setProcessNoise(process_noise_translation_); h.setTagSize(tag_sizes_map_[detection.family]); h.update(corners, timestamp); } else { diff --git a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp index e8157f70..a149e192 100644 --- a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,18 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include namespace tier4_tag_utils { ApriltagHypothesis::ApriltagHypothesis( int id, image_geometry::PinholeCameraModel & pinhole_camera_model) -: first_observation_(true), - dynamics_model_(tier4_tag_utils::DynamicsModel::Static), - id_(id), - pinhole_camera_model_(pinhole_camera_model) +: first_observation_(true), id_(id), pinhole_camera_model_(pinhole_camera_model) { } @@ -46,7 +43,7 @@ bool ApriltagHypothesis::update( cv::Point2d filtered_center = getCenter2d(filtered_corner_points_2d_); cv::Point2d current_center = getCenter2d(corners); - if (cv::norm(filtered_center - current_center) > new_hypothesis_transl_) { + if (cv::norm(filtered_center - current_center) > new_hypothesis_translation_) { first_observation_timestamp_ = stamp; filtered_corner_points_2d_ = corners; @@ -58,29 +55,13 @@ bool ApriltagHypothesis::update( for (int i = 0; i < 4; ++i) { cv::KalmanFilter & kalman_filter = kalman_filters_[i]; - if (dynamics_model_ == DynamicsModel::Static) { - cv::Mat prediction = kalman_filter.predict(); - cv::Mat observation = toState(corners[i]); + cv::Mat prediction = kalman_filter.predict(); + cv::Mat observation = toState(corners[i]); - cv::Mat estimated = kalman_filter.correct(observation); + cv::Mat estimated = kalman_filter.correct(observation); - filtered_corner_points_2d_[i].x = estimated.at(0); - filtered_corner_points_2d_[i].y = estimated.at(1); - } else { - // non-fixed timestep - double dt = (stamp - last_observation_timestamp_).seconds(); - kalman_filter.transitionMatrix.at(0, 3) = dt; - kalman_filter.transitionMatrix.at(1, 4) = dt; - kalman_filter.transitionMatrix.at(2, 5) = dt; - kalman_filter.transitionMatrix.at(6, 9) = dt; - - cv::Mat prediction = kalman_filter.predict(); - cv::Mat observation = toState(corners[i]); - cv::Mat estimated = kalman_filter.correct(observation); - - filtered_corner_points_2d_[i].x = estimated.at(0); - filtered_corner_points_2d_[i].y = estimated.at(1); - } + filtered_corner_points_2d_[i].x = estimated.at(0); + filtered_corner_points_2d_[i].y = estimated.at(1); } return true; @@ -137,7 +118,6 @@ std::vector ApriltagHypothesis::getFilteredPoints3d() const std::vector ApriltagHypothesis::getPoints3d( const std::vector & image_points) const { - std::vector undistorted_points; std::vector object_points; std::vector apriltag_template_points = { @@ -146,23 +126,17 @@ std::vector ApriltagHypothesis::getPoints3d( cv::Point3d(0.5 * tag_size_, -0.5 * tag_size_, 0.0), cv::Point3d(-0.5 * tag_size_, -0.5 * tag_size_, 0.0)}; - cv::undistortPoints( - image_points, undistorted_points, pinhole_camera_model_.intrinsicMatrix(), - pinhole_camera_model_.distortionCoeffs()); + cv::Mat rvec, tvec; - cv::sqpnp::PoseSolver solver; - std::vector rvec_vec, tvec_vec; - solver.solve(apriltag_template_points, undistorted_points, rvec_vec, tvec_vec); + bool success = cv::solvePnP( + apriltag_template_points, image_points, pinhole_camera_model_.intrinsicMatrix(), + pinhole_camera_model_.distortionCoeffs(), rvec, tvec, false, cv::SOLVEPNP_SQPNP); - if (tvec_vec.size() == 0) { - assert(false); + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("tier4_tag_utils"), "PNP failed"); return object_points; } - assert(rvec_vec.size() == 1); - cv::Mat rvec = rvec_vec[0]; - cv::Mat tvec = tvec_vec[0]; - cv::Matx31d translation_vector = tvec; cv::Matx33d rotation_matrix; @@ -210,9 +184,9 @@ bool ApriltagHypothesis::converged() const // decide based on the variance const cv::Mat & cov = kalman_filter.errorCovPost; - double max_transl_cov = std::max({cov.at(0, 0), cov.at(1, 1)}); + double max_translation_cov = std::max({cov.at(0, 0), cov.at(1, 1)}); - if (std::sqrt(max_transl_cov) > convergence_transl_) { + if (std::sqrt(max_translation_cov) > convergence_translation_) { converged = false; break; @@ -222,60 +196,55 @@ bool ApriltagHypothesis::converged() const return converged; } -void ApriltagHypothesis::setDynamicsModel(DynamicsModel dynamics_model) -{ - dynamics_model_ = dynamics_model; -} - void ApriltagHypothesis::setMinConvergenceTime(double convergence_time) { min_convergence_time_ = convergence_time; } -void ApriltagHypothesis::setMaxConvergenceThreshold(double transl) { convergence_transl_ = transl; } +void ApriltagHypothesis::setMaxConvergenceThreshold(double translation) +{ + convergence_translation_ = translation; +} -void ApriltagHypothesis::setNewHypothesisThreshold(double max_transl) +void ApriltagHypothesis::setNewHypothesisThreshold(double max_translation) { - new_hypothesis_transl_ = max_transl; + new_hypothesis_translation_ = max_translation; } void ApriltagHypothesis::setMaxNoObservationTime(double time) { max_no_observation_time_ = time; } -void ApriltagHypothesis::setMeasurementNoise(double transl) { measurement_noise_transl_ = transl; } +void ApriltagHypothesis::setMeasurementNoise(double translation) +{ + measurement_noise_translation_ = translation; +} -void ApriltagHypothesis::setProcessNoise(double transl) { process_noise_transl_ = transl; } +void ApriltagHypothesis::setProcessNoise(double translation) +{ + process_noise_translation_ = translation; +} void ApriltagHypothesis::setTagSize(double size) { tag_size_ = size; } void ApriltagHypothesis::initKalman(const std::vector & corners) -{ - if (dynamics_model_ == DynamicsModel::Static) { - initStaticKalman(corners); - } else { - assert(false); - // initConstantVelocityKalman(corners); - } -} - -void ApriltagHypothesis::initStaticKalman(const std::vector & corners) { for (int i = 0; i < 4; ++i) { cv::KalmanFilter & kalman_filter = kalman_filters_[i]; kalman_filter.init(2, 2, 0, CV_64F); // states x observations - const double process_cov_transl = process_noise_transl_ * process_noise_transl_; + const double process_cov_translation = process_noise_translation_ * process_noise_translation_; cv::setIdentity(kalman_filter.processNoiseCov, cv::Scalar::all(1.0)); - kalman_filter.processNoiseCov.at(0, 0) = process_cov_transl; - kalman_filter.processNoiseCov.at(1, 1) = process_cov_transl; + kalman_filter.processNoiseCov.at(0, 0) = process_cov_translation; + kalman_filter.processNoiseCov.at(1, 1) = process_cov_translation; - const double measurement_cov_transl = measurement_noise_transl_ * measurement_noise_transl_; + const double measurement_cov_translation = + measurement_noise_translation_ * measurement_noise_translation_; cv::setIdentity(kalman_filter.measurementNoiseCov, cv::Scalar::all(1.0)); - kalman_filter.measurementNoiseCov.at(0, 0) = measurement_cov_transl; - kalman_filter.measurementNoiseCov.at(1, 1) = measurement_cov_transl; + kalman_filter.measurementNoiseCov.at(0, 0) = measurement_cov_translation; + kalman_filter.measurementNoiseCov.at(1, 1) = measurement_cov_translation; cv::setIdentity(kalman_filter.errorCovPost, cv::Scalar::all(1.0)); cv::setIdentity(kalman_filter.transitionMatrix, cv::Scalar::all(1.0)); @@ -287,22 +256,12 @@ void ApriltagHypothesis::initStaticKalman(const std::vector & corne cv::Mat ApriltagHypothesis::toState(const cv::Point2d & corner) { - if (dynamics_model_ == DynamicsModel::Static) { - cv::Mat kalman_state(2, 1, CV_64F); - - kalman_state.at(0, 0) = corner.x; - kalman_state.at(1, 0) = corner.y; + cv::Mat kalman_state(2, 1, CV_64F); - return kalman_state; - } else { - cv::Mat kalman_state(2, 1, CV_64F); - kalman_state.setTo(cv::Scalar(0.0)); + kalman_state.at(0, 0) = corner.x; + kalman_state.at(1, 0) = corner.y; - kalman_state.at(0, 0) = corner.x; - kalman_state.at(1, 0) = corner.y; - - return kalman_state; - } + return kalman_state; } } // namespace tier4_tag_utils diff --git a/common/tier4_tag_utils/src/cv/LICENSE.txt b/common/tier4_tag_utils/src/cv/LICENSE.txt deleted file mode 100644 index d6456956..00000000 --- a/common/tier4_tag_utils/src/cv/LICENSE.txt +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/common/tier4_tag_utils/src/cv/sqpnp.cpp b/common/tier4_tag_utils/src/cv/sqpnp.cpp deleted file mode 100644 index b8147b9a..00000000 --- a/common/tier4_tag_utils/src/cv/sqpnp.cpp +++ /dev/null @@ -1,793 +0,0 @@ -// This file is part of OpenCV project. -// It is subject to the license terms in the LICENSE file found in the top-level directory -// of this distribution and at http://opencv.org/license.html - -// This file is based on file issued with the following license: - -/* -BSD 3-Clause License - -Copyright (c) 2020, George Terzakis -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -// #include "cv/precomp.hpp" -#include "tier4_tag_utils/cv/sqpnp.hpp" - -#include -#include -#include - -template -cv::Matx<_Tp, m, n> div(const cv::Matx<_Tp, m, n> & a, double alpha) -{ - return cv::Matx<_Tp, m, n>(a, 1. / alpha, cv::Matx_ScaleOp()); -} - -namespace cv -{ -namespace sqpnp -{ -const double PoseSolver::RANK_TOLERANCE = 1e-7; -const double PoseSolver::SQP_SQUARED_TOLERANCE = 1e-10; -const double PoseSolver::SQP_DET_THRESHOLD = 1.001; -const double PoseSolver::ORTHOGONALITY_SQUARED_ERROR_THRESHOLD = 1e-8; -const double PoseSolver::EQUAL_VECTORS_SQUARED_DIFF = 1e-10; -const double PoseSolver::EQUAL_SQUARED_ERRORS_DIFF = 1e-6; -const double PoseSolver::POINT_VARIANCE_THRESHOLD = 1e-5; -const double PoseSolver::SQRT3 = std::sqrt(3); -const int PoseSolver::SQP_MAX_ITERATION = 15; - -// No checking done here for overflow, since this is not public all call instances -// are assumed to be valid -template -void set( - int row, int col, cv::Matx & dest, - const cv::Matx & source) -{ - for (int y = 0; y < snrows; y++) { - for (int x = 0; x < sncols; x++) { - dest(row + y, col + x) = source(y, x); - } - } -} - -PoseSolver::PoseSolver() : num_null_vectors_(-1), num_solutions_(0) {} - -void PoseSolver::solve( - InputArray objectPoints, InputArray imagePoints, OutputArrayOfArrays rvecs, - OutputArrayOfArrays tvecs) -{ - // Input checking - int objType = objectPoints.getMat().type(); - CV_CheckType( - objType, objType == CV_32FC3 || objType == CV_64FC3, - "Type of objectPoints must be CV_32FC3 or CV_64FC3"); - - int imgType = imagePoints.getMat().type(); - CV_CheckType( - imgType, imgType == CV_32FC2 || imgType == CV_64FC2, - "Type of imagePoints must be CV_32FC2 or CV_64FC2"); - - CV_Assert(objectPoints.rows() == 1 || objectPoints.cols() == 1); - CV_Assert(objectPoints.rows() >= 3 || objectPoints.cols() >= 3); - CV_Assert(imagePoints.rows() == 1 || imagePoints.cols() == 1); - CV_Assert(imagePoints.rows() * imagePoints.cols() == objectPoints.rows() * objectPoints.cols()); - - Mat _imagePoints; - if (imgType == CV_32FC2) { - imagePoints.getMat().convertTo(_imagePoints, CV_64F); - } else { - _imagePoints = imagePoints.getMat(); - } - - Mat _objectPoints; - if (objType == CV_32FC3) { - objectPoints.getMat().convertTo(_objectPoints, CV_64F); - } else { - _objectPoints = objectPoints.getMat(); - } - - num_null_vectors_ = -1; - num_solutions_ = 0; - - computeOmega(_objectPoints, _imagePoints); - solveInternal(); - - int depthRot = rvecs.fixedType() ? rvecs.depth() : CV_64F; - int depthTrans = tvecs.fixedType() ? tvecs.depth() : CV_64F; - - rvecs.create( - num_solutions_, 1, - CV_MAKETYPE(depthRot, rvecs.fixedType() && rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); - tvecs.create( - num_solutions_, 1, - CV_MAKETYPE(depthTrans, tvecs.fixedType() && tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); - - for (int i = 0; i < num_solutions_; i++) { - Mat rvec; - Mat rotation = Mat(solutions_[i].r_hat).reshape(1, 3); - Rodrigues(rotation, rvec); - - rvecs.getMatRef(i) = rvec; - tvecs.getMatRef(i) = Mat(solutions_[i].t); - } -} - -void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) -{ - omega_ = cv::Matx::zeros(); - cv::Matx qa_sum = cv::Matx::zeros(); - - cv::Point2d sum_img(0, 0); - cv::Point3d sum_obj(0, 0, 0); - double sq_norm_sum = 0; - - Mat _imagePoints = imagePoints.getMat(); - Mat _objectPoints = objectPoints.getMat(); - - int n = _objectPoints.cols * _objectPoints.rows; - - for (int i = 0; i < n; i++) { - const cv::Point2d & img_pt = _imagePoints.at(i); - const cv::Point3d & obj_pt = _objectPoints.at(i); - - sum_img += img_pt; - sum_obj += obj_pt; - - const double &x = img_pt.x, &y = img_pt.y; - const double &X = obj_pt.x, &Y = obj_pt.y, &Z = obj_pt.z; - double sq_norm = x * x + y * y; - sq_norm_sum += sq_norm; - - double X2 = X * X, XY = X * Y, XZ = X * Z, Y2 = Y * Y, YZ = Y * Z, Z2 = Z * Z; - - omega_(0, 0) += X2; - omega_(0, 1) += XY; - omega_(0, 2) += XZ; - omega_(1, 1) += Y2; - omega_(1, 2) += YZ; - omega_(2, 2) += Z2; - - // Populating this manually saves operations by only calculating upper triangle - omega_(0, 6) += -x * X2; - omega_(0, 7) += -x * XY; - omega_(0, 8) += -x * XZ; - omega_(1, 7) += -x * Y2; - omega_(1, 8) += -x * YZ; - omega_(2, 8) += -x * Z2; - - omega_(3, 6) += -y * X2; - omega_(3, 7) += -y * XY; - omega_(3, 8) += -y * XZ; - omega_(4, 7) += -y * Y2; - omega_(4, 8) += -y * YZ; - omega_(5, 8) += -y * Z2; - - omega_(6, 6) += sq_norm * X2; - omega_(6, 7) += sq_norm * XY; - omega_(6, 8) += sq_norm * XZ; - omega_(7, 7) += sq_norm * Y2; - omega_(7, 8) += sq_norm * YZ; - omega_(8, 8) += sq_norm * Z2; - - // Compute qa_sum - qa_sum(0, 0) += X; - qa_sum(0, 1) += Y; - qa_sum(0, 2) += Z; - qa_sum(1, 3) += X; - qa_sum(1, 4) += Y; - qa_sum(1, 5) += Z; - - qa_sum(0, 6) += -x * X; - qa_sum(0, 7) += -x * Y; - qa_sum(0, 8) += -x * Z; - qa_sum(1, 6) += -y * X; - qa_sum(1, 7) += -y * Y; - qa_sum(1, 8) += -y * Z; - - qa_sum(2, 0) += -x * X; - qa_sum(2, 1) += -x * Y; - qa_sum(2, 2) += -x * Z; - qa_sum(2, 3) += -y * X; - qa_sum(2, 4) += -y * Y; - qa_sum(2, 5) += -y * Z; - - qa_sum(2, 6) += sq_norm * X; - qa_sum(2, 7) += sq_norm * Y; - qa_sum(2, 8) += sq_norm * Z; - } - - omega_(1, 6) = omega_(0, 7); - omega_(2, 6) = omega_(0, 8); - omega_(2, 7) = omega_(1, 8); - omega_(4, 6) = omega_(3, 7); - omega_(5, 6) = omega_(3, 8); - omega_(5, 7) = omega_(4, 8); - omega_(7, 6) = omega_(6, 7); - omega_(8, 6) = omega_(6, 8); - omega_(8, 7) = omega_(7, 8); - - omega_(3, 3) = omega_(0, 0); - omega_(3, 4) = omega_(0, 1); - omega_(3, 5) = omega_(0, 2); - omega_(4, 4) = omega_(1, 1); - omega_(4, 5) = omega_(1, 2); - omega_(5, 5) = omega_(2, 2); - - // Mirror upper triangle to lower triangle - for (int r = 0; r < 9; r++) { - for (int c = 0; c < r; c++) { - omega_(r, c) = omega_(c, r); - } - } - - cv::Matx q; - q(0, 0) = n; - q(0, 1) = 0; - q(0, 2) = -sum_img.x; - q(1, 0) = 0; - q(1, 1) = n; - q(1, 2) = -sum_img.y; - q(2, 0) = -sum_img.x; - q(2, 1) = -sum_img.y; - q(2, 2) = sq_norm_sum; - - double inv_n = 1.0 / n; - double detQ = n * (n * sq_norm_sum - sum_img.y * sum_img.y - sum_img.x * sum_img.x); - double point_coordinate_variance = detQ * inv_n * inv_n * inv_n; - - CV_Assert(point_coordinate_variance >= POINT_VARIANCE_THRESHOLD); - - Matx q_inv; - analyticalInverse3x3Symm(q, q_inv); - - p_ = -q_inv * qa_sum; - - omega_ += qa_sum.t() * p_; - - cv::SVD omega_svd(omega_, cv::SVD::FULL_UV); - s_ = omega_svd.w; - u_ = cv::Mat(omega_svd.vt.t()); - - CV_Assert(s_(0) >= 1e-7); - - while (s_(7 - num_null_vectors_) < RANK_TOLERANCE) num_null_vectors_++; - - CV_Assert(++num_null_vectors_ <= 6); - - point_mean_ = cv::Vec3d(sum_obj.x / n, sum_obj.y / n, sum_obj.z / n); -} - -void PoseSolver::solveInternal() -{ - double min_sq_err = std::numeric_limits::max(); - int num_eigen_points = num_null_vectors_ > 0 ? num_null_vectors_ : 1; - - for (int i = 9 - num_eigen_points; i < 9; i++) { - const cv::Matx e = SQRT3 * u_.col(i); - double orthogonality_sq_err = orthogonalityError(e); - - SQPSolution solutions[2]; - - // If e is orthogonal, we can skip SQP - if (orthogonality_sq_err < ORTHOGONALITY_SQUARED_ERROR_THRESHOLD) { - solutions[0].r_hat = det3x3(e) * e; - solutions[0].t = p_ * solutions[0].r_hat; - checkSolution(solutions[0], min_sq_err); - } else { - Matx r; - nearestRotationMatrix(e, r); - solutions[0] = runSQP(r); - solutions[0].t = p_ * solutions[0].r_hat; - checkSolution(solutions[0], min_sq_err); - - nearestRotationMatrix(-e, r); - solutions[1] = runSQP(r); - solutions[1].t = p_ * solutions[1].r_hat; - checkSolution(solutions[1], min_sq_err); - } - } - - int c = 1; - - while (min_sq_err > 3 * s_[9 - num_eigen_points - c] && 9 - num_eigen_points - c > 0) { - int index = 9 - num_eigen_points - c; - - const cv::Matx e = u_.col(index); - SQPSolution solutions[2]; - - Matx r; - nearestRotationMatrix(e, r); - solutions[0] = runSQP(r); - solutions[0].t = p_ * solutions[0].r_hat; - checkSolution(solutions[0], min_sq_err); - - nearestRotationMatrix(-e, r); - solutions[1] = runSQP(r); - solutions[1].t = p_ * solutions[1].r_hat; - checkSolution(solutions[1], min_sq_err); - - c++; - } -} - -PoseSolver::SQPSolution PoseSolver::runSQP(const cv::Matx & r0) -{ - cv::Matx r = r0; - - double delta_squared_norm = std::numeric_limits::max(); - cv::Matx delta; - - int step = 0; - while (delta_squared_norm > SQP_SQUARED_TOLERANCE && step++ < SQP_MAX_ITERATION) { - solveSQPSystem(r, delta); - r += delta; - delta_squared_norm = cv::norm(delta, cv::NORM_L2SQR); - } - - SQPSolution solution; - - double det_r = det3x3(r); - if (det_r < 0) { - r = -r; - det_r = -det_r; - } - - if (det_r > SQP_DET_THRESHOLD) { - nearestRotationMatrix(r, solution.r_hat); - } else { - solution.r_hat = r; - } - - return solution; -} - -void PoseSolver::solveSQPSystem(const cv::Matx & r, cv::Matx & delta) -{ - double sqnorm_r1 = r(0) * r(0) + r(1) * r(1) + r(2) * r(2), - sqnorm_r2 = r(3) * r(3) + r(4) * r(4) + r(5) * r(5), - sqnorm_r3 = r(6) * r(6) + r(7) * r(7) + r(8) * r(8); - double dot_r1r2 = r(0) * r(3) + r(1) * r(4) + r(2) * r(5), - dot_r1r3 = r(0) * r(6) + r(1) * r(7) + r(2) * r(8), - dot_r2r3 = r(3) * r(6) + r(4) * r(7) + r(5) * r(8); - - cv::Matx N; - cv::Matx H; - cv::Matx JH; - - computeRowAndNullspace(r, H, N, JH); - - cv::Matx g; - g(0) = 1 - sqnorm_r1; - g(1) = 1 - sqnorm_r2; - g(2) = 1 - sqnorm_r3; - g(3) = -dot_r1r2; - g(4) = -dot_r2r3; - g(5) = -dot_r1r3; - - cv::Matx x; - x(0) = g(0) / JH(0, 0); - x(1) = g(1) / JH(1, 1); - x(2) = g(2) / JH(2, 2); - x(3) = (g(3) - JH(3, 0) * x(0) - JH(3, 1) * x(1)) / JH(3, 3); - x(4) = (g(4) - JH(4, 1) * x(1) - JH(4, 2) * x(2) - JH(4, 3) * x(3)) / JH(4, 4); - x(5) = (g(5) - JH(5, 0) * x(0) - JH(5, 2) * x(2) - JH(5, 3) * x(3) - JH(5, 4) * x(4)) / JH(5, 5); - - delta = H * x; - - cv::Matx nt_omega = N.t() * omega_; - cv::Matx W = nt_omega * N, W_inv; - - analyticalInverse3x3Symm(W, W_inv); - - cv::Matx y = -W_inv * nt_omega * (delta + r); - delta += N * y; -} - -bool PoseSolver::analyticalInverse3x3Symm( - const cv::Matx & Q, cv::Matx & Qinv, const double & threshold) -{ - // 1. Get the elements of the matrix - double a = Q(0, 0), b = Q(1, 0), d = Q(1, 1), c = Q(2, 0), e = Q(2, 1), f = Q(2, 2); - - // 2. Determinant - double t2, t4, t7, t9, t12; - t2 = e * e; - t4 = a * d; - t7 = b * b; - t9 = b * c; - t12 = c * c; - double det = -t4 * f + a * t2 + t7 * f - 2.0 * t9 * e + t12 * d; - - if (fabs(det) < threshold) return false; - - // 3. Inverse - double t15, t20, t24, t30; - t15 = 1.0 / det; - t20 = (-b * f + c * e) * t15; - t24 = (b * e - c * d) * t15; - t30 = (a * e - t9) * t15; - Qinv(0, 0) = (-d * f + t2) * t15; - Qinv(0, 1) = Qinv(1, 0) = -t20; - Qinv(0, 2) = Qinv(2, 0) = -t24; - Qinv(1, 1) = -(a * f - t12) * t15; - Qinv(1, 2) = Qinv(2, 1) = t30; - Qinv(2, 2) = -(t4 - t7) * t15; - - return true; -} - -void PoseSolver::computeRowAndNullspace( - const cv::Matx & r, cv::Matx & H, cv::Matx & N, - cv::Matx & K, const double & norm_threshold) -{ - H = cv::Matx::zeros(); - - // 1. q1 - double norm_r1 = sqrt(r(0) * r(0) + r(1) * r(1) + r(2) * r(2)); - double inv_norm_r1 = norm_r1 > 1e-5 ? 1.0 / norm_r1 : 0.0; - H(0, 0) = r(0) * inv_norm_r1; - H(1, 0) = r(1) * inv_norm_r1; - H(2, 0) = r(2) * inv_norm_r1; - K(0, 0) = 2 * norm_r1; - - // 2. q2 - double norm_r2 = sqrt(r(3) * r(3) + r(4) * r(4) + r(5) * r(5)); - double inv_norm_r2 = 1.0 / norm_r2; - H(3, 1) = r(3) * inv_norm_r2; - H(4, 1) = r(4) * inv_norm_r2; - H(5, 1) = r(5) * inv_norm_r2; - K(1, 0) = 0; - K(1, 1) = 2 * norm_r2; - - // 3. q3 = (r3'*q2)*q2 - (r3'*q1)*q1 ; q3 = q3/norm(q3) - double norm_r3 = sqrt(r(6) * r(6) + r(7) * r(7) + r(8) * r(8)); - double inv_norm_r3 = 1.0 / norm_r3; - H(6, 2) = r(6) * inv_norm_r3; - H(7, 2) = r(7) * inv_norm_r3; - H(8, 2) = r(8) * inv_norm_r3; - K(2, 0) = K(2, 1) = 0; - K(2, 2) = 2 * norm_r3; - - // 4. q4 - double dot_j4q1 = r(3) * H(0, 0) + r(4) * H(1, 0) + r(5) * H(2, 0), - dot_j4q2 = r(0) * H(3, 1) + r(1) * H(4, 1) + r(2) * H(5, 1); - - H(0, 3) = r(3) - dot_j4q1 * H(0, 0); - H(1, 3) = r(4) - dot_j4q1 * H(1, 0); - H(2, 3) = r(5) - dot_j4q1 * H(2, 0); - H(3, 3) = r(0) - dot_j4q2 * H(3, 1); - H(4, 3) = r(1) - dot_j4q2 * H(4, 1); - H(5, 3) = r(2) - dot_j4q2 * H(5, 1); - double inv_norm_j4 = 1.0 / sqrt( - H(0, 3) * H(0, 3) + H(1, 3) * H(1, 3) + H(2, 3) * H(2, 3) + - H(3, 3) * H(3, 3) + H(4, 3) * H(4, 3) + H(5, 3) * H(5, 3)); - - H(0, 3) *= inv_norm_j4; - H(1, 3) *= inv_norm_j4; - H(2, 3) *= inv_norm_j4; - H(3, 3) *= inv_norm_j4; - H(4, 3) *= inv_norm_j4; - H(5, 3) *= inv_norm_j4; - - K(3, 0) = r(3) * H(0, 0) + r(4) * H(1, 0) + r(5) * H(2, 0); - K(3, 1) = r(0) * H(3, 1) + r(1) * H(4, 1) + r(2) * H(5, 1); - K(3, 2) = 0; - K(3, 3) = r(3) * H(0, 3) + r(4) * H(1, 3) + r(5) * H(2, 3) + r(0) * H(3, 3) + r(1) * H(4, 3) + - r(2) * H(5, 3); - - // 5. q5 - double dot_j5q2 = r(6) * H(3, 1) + r(7) * H(4, 1) + r(8) * H(5, 1); - double dot_j5q3 = r(3) * H(6, 2) + r(4) * H(7, 2) + r(5) * H(8, 2); - double dot_j5q4 = r(6) * H(3, 3) + r(7) * H(4, 3) + r(8) * H(5, 3); - - H(0, 4) = -dot_j5q4 * H(0, 3); - H(1, 4) = -dot_j5q4 * H(1, 3); - H(2, 4) = -dot_j5q4 * H(2, 3); - H(3, 4) = r(6) - dot_j5q2 * H(3, 1) - dot_j5q4 * H(3, 3); - H(4, 4) = r(7) - dot_j5q2 * H(4, 1) - dot_j5q4 * H(4, 3); - H(5, 4) = r(8) - dot_j5q2 * H(5, 1) - dot_j5q4 * H(5, 3); - H(6, 4) = r(3) - dot_j5q3 * H(6, 2); - H(7, 4) = r(4) - dot_j5q3 * H(7, 2); - H(8, 4) = r(5) - dot_j5q3 * H(8, 2); - - Matx q4 = H.col(4); - q4 = div(q4, cv::norm(q4)); - set(0, 4, H, q4); - - K(4, 0) = 0; - K(4, 1) = r(6) * H(3, 1) + r(7) * H(4, 1) + r(8) * H(5, 1); - K(4, 2) = r(3) * H(6, 2) + r(4) * H(7, 2) + r(5) * H(8, 2); - K(4, 3) = r(6) * H(3, 3) + r(7) * H(4, 3) + r(8) * H(5, 3); - K(4, 4) = r(6) * H(3, 4) + r(7) * H(4, 4) + r(8) * H(5, 4) + r(3) * H(6, 4) + r(4) * H(7, 4) + - r(5) * H(8, 4); - - // 4. q6 - double dot_j6q1 = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0); - double dot_j6q3 = r(0) * H(6, 2) + r(1) * H(7, 2) + r(2) * H(8, 2); - double dot_j6q4 = r(6) * H(0, 3) + r(7) * H(1, 3) + r(8) * H(2, 3); - double dot_j6q5 = r(0) * H(6, 4) + r(1) * H(7, 4) + r(2) * H(8, 4) + r(6) * H(0, 4) + - r(7) * H(1, 4) + r(8) * H(2, 4); - - H(0, 5) = r(6) - dot_j6q1 * H(0, 0) - dot_j6q4 * H(0, 3) - dot_j6q5 * H(0, 4); - H(1, 5) = r(7) - dot_j6q1 * H(1, 0) - dot_j6q4 * H(1, 3) - dot_j6q5 * H(1, 4); - H(2, 5) = r(8) - dot_j6q1 * H(2, 0) - dot_j6q4 * H(2, 3) - dot_j6q5 * H(2, 4); - - H(3, 5) = -dot_j6q5 * H(3, 4) - dot_j6q4 * H(3, 3); - H(4, 5) = -dot_j6q5 * H(4, 4) - dot_j6q4 * H(4, 3); - H(5, 5) = -dot_j6q5 * H(5, 4) - dot_j6q4 * H(5, 3); - - H(6, 5) = r(0) - dot_j6q3 * H(6, 2) - dot_j6q5 * H(6, 4); - H(7, 5) = r(1) - dot_j6q3 * H(7, 2) - dot_j6q5 * H(7, 4); - H(8, 5) = r(2) - dot_j6q3 * H(8, 2) - dot_j6q5 * H(8, 4); - - Matx q5 = H.col(5); - q5 = div(q5, cv::norm(q5)); - set(0, 5, H, q5); - - K(5, 0) = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0); - K(5, 1) = 0; - K(5, 2) = r(0) * H(6, 2) + r(1) * H(7, 2) + r(2) * H(8, 2); - K(5, 3) = r(6) * H(0, 3) + r(7) * H(1, 3) + r(8) * H(2, 3); - K(5, 4) = r(6) * H(0, 4) + r(7) * H(1, 4) + r(8) * H(2, 4) + r(0) * H(6, 4) + r(1) * H(7, 4) + - r(2) * H(8, 4); - K(5, 5) = r(6) * H(0, 5) + r(7) * H(1, 5) + r(8) * H(2, 5) + r(0) * H(6, 5) + r(1) * H(7, 5) + - r(2) * H(8, 5); - - // Great! Now H is an orthogonalized, sparse basis of the Jacobian row space and K is filled. - // - // Now get a projector onto the null space H: - const cv::Matx Pn = cv::Matx::eye() - (H * H.t()); - - // Now we need to pick 3 columns of P with non-zero norm (> 0.3) and some angle between them (> - // 0.3). - // - // Find the 3 columns of Pn with largest norms - int index1 = 0, index2 = 0, index3 = 0; - double max_norm1 = std::numeric_limits::min(); - double min_dot12 = std::numeric_limits::max(); - double min_dot1323 = std::numeric_limits::max(); - - double col_norms[9]; - for (int i = 0; i < 9; i++) { - col_norms[i] = cv::norm(Pn.col(i)); - if (col_norms[i] >= norm_threshold) { - if (max_norm1 < col_norms[i]) { - max_norm1 = col_norms[i]; - index1 = i; - } - } - } - - Matx v1 = Pn.col(index1); - v1 = div(v1, max_norm1); - set(0, 0, N, v1); - - for (int i = 0; i < 9; i++) { - if (i == index1) continue; - if (col_norms[i] >= norm_threshold) { - double cos_v1_x_col = fabs(Pn.col(i).dot(v1) / col_norms[i]); - - if (cos_v1_x_col <= min_dot12) { - index2 = i; - min_dot12 = cos_v1_x_col; - } - } - } - - Matx v2 = Pn.col(index2); - Matx n0 = N.col(0); - v2 -= v2.dot(n0) * n0; - v2 = div(v2, cv::norm(v2)); - set(0, 1, N, v2); - - for (int i = 0; i < 9; i++) { - if (i == index2 || i == index1) continue; - if (col_norms[i] >= norm_threshold) { - double cos_v1_x_col = fabs(Pn.col(i).dot(v1) / col_norms[i]); - double cos_v2_x_col = fabs(Pn.col(i).dot(v2) / col_norms[i]); - - if (cos_v1_x_col + cos_v2_x_col <= min_dot1323) { - index3 = i; - min_dot1323 = cos_v2_x_col + cos_v2_x_col; - } - } - } - - Matx v3 = Pn.col(index3); - Matx n1 = N.col(1); - v3 -= (v3.dot(n1)) * n1 - (v3.dot(n0)) * n0; - v3 = div(v3, cv::norm(v3)); - set(0, 2, N, v3); -} - -// faster nearest rotation computation based on FOAM (see: -// http://users.ics.forth.gr/~lourakis/publ/2018_iros.pdf ) -/* Solve the nearest orthogonal approximation problem - * i.e., given e, find R minimizing ||R-e||_F - * - * The computation borrows from Markley's FOAM algorithm - * "Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm", J. - * Astronaut. Sci. - * - * See also M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016 - * - * Copyright (C) 2019 Manolis Lourakis (lourakis **at** ics forth gr) - * Institute of Computer Science, Foundation for Research & Technology - Hellas - * Heraklion, Crete, Greece. - */ -void PoseSolver::nearestRotationMatrix(const cv::Matx & e, cv::Matx & r) -{ - int i; - double l, lprev, det_e, e_sq, adj_e_sq, adj_e[9]; - - // e's adjoint - adj_e[0] = e(4) * e(8) - e(5) * e(7); - adj_e[1] = e(2) * e(7) - e(1) * e(8); - adj_e[2] = e(1) * e(5) - e(2) * e(4); - adj_e[3] = e(5) * e(6) - e(3) * e(8); - adj_e[4] = e(0) * e(8) - e(2) * e(6); - adj_e[5] = e(2) * e(3) - e(0) * e(5); - adj_e[6] = e(3) * e(7) - e(4) * e(6); - adj_e[7] = e(1) * e(6) - e(0) * e(7); - adj_e[8] = e(0) * e(4) - e(1) * e(3); - - // det(e), ||e||^2, ||adj(e)||^2 - det_e = e(0) * e(4) * e(8) - e(0) * e(5) * e(7) - e(1) * e(3) * e(8) + e(2) * e(3) * e(7) + - e(1) * e(6) * e(5) - e(2) * e(6) * e(4); - e_sq = e(0) * e(0) + e(1) * e(1) + e(2) * e(2) + e(3) * e(3) + e(4) * e(4) + e(5) * e(5) + - e(6) * e(6) + e(7) * e(7) + e(8) * e(8); - adj_e_sq = adj_e[0] * adj_e[0] + adj_e[1] * adj_e[1] + adj_e[2] * adj_e[2] + adj_e[3] * adj_e[3] + - adj_e[4] * adj_e[4] + adj_e[5] * adj_e[5] + adj_e[6] * adj_e[6] + adj_e[7] * adj_e[7] + - adj_e[8] * adj_e[8]; - - // compute l_max with Newton-Raphson from FOAM's characteristic polynomial, i.e. eq.(23) - (26) - for (i = 200, l = 2.0, lprev = 0.0; fabs(l - lprev) > 1E-12 * fabs(lprev) && i > 0; --i) { - double tmp, p, pp; - - tmp = (l * l - e_sq); - p = (tmp * tmp - 8.0 * l * det_e - 4.0 * adj_e_sq); - pp = 8.0 * (0.5 * tmp * l - det_e); - - lprev = l; - l -= p / pp; - } - - // the rotation matrix equals ((l^2 + e_sq)*e + 2*l*adj(e') - 2*e*e'*e) / (l*(l*l-e_sq) - - // 2*det(e)), i.e. eq.(14) using (18), (19) - { - // compute (l^2 + e_sq)*e - double tmp[9], e_et[9], denom; - const double a = l * l + e_sq; - - // e_et=e*e' - e_et[0] = e(0) * e(0) + e(1) * e(1) + e(2) * e(2); - e_et[1] = e(0) * e(3) + e(1) * e(4) + e(2) * e(5); - e_et[2] = e(0) * e(6) + e(1) * e(7) + e(2) * e(8); - - e_et[3] = e_et[1]; - e_et[4] = e(3) * e(3) + e(4) * e(4) + e(5) * e(5); - e_et[5] = e(3) * e(6) + e(4) * e(7) + e(5) * e(8); - - e_et[6] = e_et[2]; - e_et[7] = e_et[5]; - e_et[8] = e(6) * e(6) + e(7) * e(7) + e(8) * e(8); - - // tmp=e_et*e - tmp[0] = e_et[0] * e(0) + e_et[1] * e(3) + e_et[2] * e(6); - tmp[1] = e_et[0] * e(1) + e_et[1] * e(4) + e_et[2] * e(7); - tmp[2] = e_et[0] * e(2) + e_et[1] * e(5) + e_et[2] * e(8); - - tmp[3] = e_et[3] * e(0) + e_et[4] * e(3) + e_et[5] * e(6); - tmp[4] = e_et[3] * e(1) + e_et[4] * e(4) + e_et[5] * e(7); - tmp[5] = e_et[3] * e(2) + e_et[4] * e(5) + e_et[5] * e(8); - - tmp[6] = e_et[6] * e(0) + e_et[7] * e(3) + e_et[8] * e(6); - tmp[7] = e_et[6] * e(1) + e_et[7] * e(4) + e_et[8] * e(7); - tmp[8] = e_et[6] * e(2) + e_et[7] * e(5) + e_et[8] * e(8); - - // compute R as (a*e + 2*(l*adj(e)' - tmp))*denom; note that adj(e')=adj(e)' - denom = l * (l * l - e_sq) - 2.0 * det_e; - denom = 1.0 / denom; - r(0) = (a * e(0) + 2.0 * (l * adj_e[0] - tmp[0])) * denom; - r(1) = (a * e(1) + 2.0 * (l * adj_e[3] - tmp[1])) * denom; - r(2) = (a * e(2) + 2.0 * (l * adj_e[6] - tmp[2])) * denom; - - r(3) = (a * e(3) + 2.0 * (l * adj_e[1] - tmp[3])) * denom; - r(4) = (a * e(4) + 2.0 * (l * adj_e[4] - tmp[4])) * denom; - r(5) = (a * e(5) + 2.0 * (l * adj_e[7] - tmp[5])) * denom; - - r(6) = (a * e(6) + 2.0 * (l * adj_e[2] - tmp[6])) * denom; - r(7) = (a * e(7) + 2.0 * (l * adj_e[5] - tmp[7])) * denom; - r(8) = (a * e(8) + 2.0 * (l * adj_e[8] - tmp[8])) * denom; - } -} - -double PoseSolver::det3x3(const cv::Matx & e) -{ - return e(0) * e(4) * e(8) + e(1) * e(5) * e(6) + e(2) * e(3) * e(7) - e(6) * e(4) * e(2) - - e(7) * e(5) * e(0) - e(8) * e(3) * e(1); -} - -inline bool PoseSolver::positiveDepth(const SQPSolution & solution) const -{ - const cv::Matx & r = solution.r_hat; - const cv::Matx & t = solution.t; - const cv::Vec3d & mean = point_mean_; - return (r(6) * mean(0) + r(7) * mean(1) + r(8) * mean(2) + t(2) > 0); -} - -void PoseSolver::checkSolution(SQPSolution & solution, double & min_error) -{ - if (positiveDepth(solution)) { - solution.sq_error = (omega_ * solution.r_hat).ddot(solution.r_hat); - if (fabs(min_error - solution.sq_error) > EQUAL_SQUARED_ERRORS_DIFF) { - if (min_error > solution.sq_error) { - min_error = solution.sq_error; - solutions_[0] = solution; - num_solutions_ = 1; - } - } else { - bool found = false; - for (int i = 0; i < num_solutions_; i++) { - if ( - cv::norm(solutions_[i].r_hat - solution.r_hat, cv::NORM_L2SQR) < - EQUAL_VECTORS_SQUARED_DIFF) { - if (solutions_[i].sq_error > solution.sq_error) { - solutions_[i] = solution; - } - found = true; - break; - } - } - - if (!found) { - solutions_[num_solutions_++] = solution; - } - if (min_error > solution.sq_error) min_error = solution.sq_error; - } - } -} - -double PoseSolver::orthogonalityError(const cv::Matx & e) -{ - double sq_norm_e1 = e(0) * e(0) + e(1) * e(1) + e(2) * e(2); - double sq_norm_e2 = e(3) * e(3) + e(4) * e(4) + e(5) * e(5); - double sq_norm_e3 = e(6) * e(6) + e(7) * e(7) + e(8) * e(8); - double dot_e1e2 = e(0) * e(3) + e(1) * e(4) + e(2) * e(5); - double dot_e1e3 = e(0) * e(6) + e(1) * e(7) + e(2) * e(8); - double dot_e2e3 = e(3) * e(6) + e(4) * e(7) + e(5) * e(8); - - return (sq_norm_e1 - 1) * (sq_norm_e1 - 1) + (sq_norm_e2 - 1) * (sq_norm_e2 - 1) + - (sq_norm_e3 - 1) * (sq_norm_e3 - 1) + - 2 * (dot_e1e2 * dot_e1e2 + dot_e1e3 * dot_e1e3 + dot_e2e3 * dot_e2e3); -} - -} // namespace sqpnp -} // namespace cv diff --git a/common/tier4_tag_utils/src/lidartag_filter.cpp b/common/tier4_tag_utils/src/lidartag_filter.cpp index 81f44acf..207e0d29 100644 --- a/common/tier4_tag_utils/src/lidartag_filter.cpp +++ b/common/tier4_tag_utils/src/lidartag_filter.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,23 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - #include #include #include #include +#include +#include +#include + +#include namespace tier4_tag_utils { @@ -39,14 +31,14 @@ LidartagFilter::LidartagFilter(const rclcpp::NodeOptions & options) max_no_observation_time_ = this->declare_parameter("max_no_observation_time"); new_hypothesis_distance_ = this->declare_parameter("new_hypothesis_distance"); - new_hypothesis_transl_ = this->declare_parameter("new_hypothesis_transl"); - new_hypothesis_rot_ = this->declare_parameter("new_hypothesis_rot"); - measurement_noise_transl_ = this->declare_parameter("measurement_noise_transl"); - measurement_noise_rot_ = this->declare_parameter("measurement_noise_rot"); - process_noise_transl_ = this->declare_parameter("process_noise_transl"); - process_noise_transl_dot_ = this->declare_parameter("process_noise_transl_dot"); - process_noise_rot_ = this->declare_parameter("process_noise_rot"); - process_noise_rot_dot_ = this->declare_parameter("process_noise_rot_dot"); + new_hypothesis_translation_ = this->declare_parameter("new_hypothesis_translation"); + new_hypothesis_rotation_ = this->declare_parameter("new_hypothesis_rotation"); + measurement_noise_translation_ = this->declare_parameter("measurement_noise_translation"); + measurement_noise_rotation_ = this->declare_parameter("measurement_noise_rotation"); + process_noise_translation_ = this->declare_parameter("process_noise_translation"); + process_noise_translation_dot_ = this->declare_parameter("process_noise_translation_dot"); + process_noise_rotation_ = this->declare_parameter("process_noise_rotation"); + process_noise_rotation_dot_ = this->declare_parameter("process_noise_rotation_dot"); sub_ = this->create_subscription( "lidartag/detections_array", 1, @@ -103,15 +95,15 @@ void LidartagFilter::updateHypothesis( hypotheses_map_[detection.id] = LidartagHypothesis(detection.id); auto & h = hypotheses_map_[detection.id]; - h.setDynamicsModel(DynamicsModel::Static); h.setMaxNoObservationTime(max_no_observation_time_); h.setMinConvergenceTime(std::numeric_limits::max()); h.setMaxConvergenceThreshold(0.0, 0.0, 0.0, 0.0); - h.setMeasurementNoise(measurement_noise_transl_, measurement_noise_rot_); - h.setNewHypothesisThreshold(new_hypothesis_transl_, new_hypothesis_rot_); + h.setMeasurementNoise(measurement_noise_translation_, measurement_noise_rotation_); + h.setNewHypothesisThreshold(new_hypothesis_translation_, new_hypothesis_rotation_); h.setProcessNoise( - process_noise_transl_, process_noise_transl_dot_, process_noise_rot_, process_noise_rot_dot_); + process_noise_translation_, process_noise_translation_dot_, process_noise_rotation_, + process_noise_rotation_dot_); h.update(translation_cv, rotation_cv, detection.size, timestamp); } else { hypotheses_map_[detection.id].update(translation_cv, rotation_cv, detection.size, timestamp); diff --git a/common/tier4_tag_utils/src/lidartag_hypothesis.cpp b/common/tier4_tag_utils/src/lidartag_hypothesis.cpp index f51d0315..4b2e5cfb 100644 --- a/common/tier4_tag_utils/src/lidartag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/lidartag_hypothesis.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -48,7 +48,7 @@ bool LidartagHypothesis::update( initKalman(pose_translation, pose_rotation); return true; } else if ( - trans_diff > new_hypothesis_transl_ || ang_diff > new_hypothesis_rot_ || + trans_diff > new_hypothesis_translation_ || ang_diff > new_hypothesis_rotation_ || dt > max_no_observation_time_) { first_observation_timestamp_ = stamp; filtered_translation_vector_ = pose_translation; @@ -65,53 +65,25 @@ bool LidartagHypothesis::update( return false; } - if (dynamics_model_ == tier4_tag_utils::DynamicsModel::Static) { - cv::Mat prediction = kalman_filter_.predict(); - cv::Mat observation = toState(pose_translation, pose_rotation); - fixState(prediction, observation); - cv::Mat estimated = kalman_filter_.correct(observation); - fixState(kalman_filter_.statePost); + cv::Mat prediction = kalman_filter_.predict(); + cv::Mat observation = toState(pose_translation, pose_rotation); + fixState(prediction, observation); + cv::Mat estimated = kalman_filter_.correct(observation); + fixState(kalman_filter_.statePost); - filtered_translation_vector_(0) = estimated.at(0); - filtered_translation_vector_(1) = estimated.at(1); - filtered_translation_vector_(2) = estimated.at(2); + filtered_translation_vector_(0) = estimated.at(0); + filtered_translation_vector_(1) = estimated.at(1); + filtered_translation_vector_(2) = estimated.at(2); - cv::Matx31d eulers_estimated; - eulers_estimated(0) = estimated.at(3); - eulers_estimated(1) = estimated.at(4); - eulers_estimated(2) = estimated.at(5); - - filtered_rotation_matrix_ = euler2rot(eulers_estimated); - - double current_speed = cv::norm(pose_translation - filtered_translation_vector_) / dt; - estimated_speed_ = dt > 0.0 ? 0.8 * estimated_speed_ + 0.2 * current_speed : 0.0; - - } else { - // non-fixed timestep - kalman_filter_.transitionMatrix.at(0, 3) = dt; - kalman_filter_.transitionMatrix.at(1, 4) = dt; - kalman_filter_.transitionMatrix.at(2, 5) = dt; - kalman_filter_.transitionMatrix.at(6, 9) = dt; - kalman_filter_.transitionMatrix.at(7, 10) = dt; - kalman_filter_.transitionMatrix.at(8, 11) = dt; - - cv::Mat prediction = kalman_filter_.predict(); - cv::Mat observation = toObservation(pose_translation, pose_rotation); - fixState(prediction, observation); - cv::Mat estimated = kalman_filter_.correct(observation); - fixState(kalman_filter_.statePost); - - filtered_translation_vector_(0) = estimated.at(0); - filtered_translation_vector_(1) = estimated.at(1); - filtered_translation_vector_(2) = estimated.at(2); + cv::Matx31d eulers_estimated; + eulers_estimated(0) = estimated.at(3); + eulers_estimated(1) = estimated.at(4); + eulers_estimated(2) = estimated.at(5); - cv::Matx31d eulers_estimated; - eulers_estimated(0) = estimated.at(6); - eulers_estimated(1) = estimated.at(7); - eulers_estimated(2) = estimated.at(8); + filtered_rotation_matrix_ = euler2rot(eulers_estimated); - filtered_rotation_matrix_ = euler2rot(eulers_estimated); - } + double current_speed = cv::norm(pose_translation - filtered_translation_vector_) / dt; + estimated_speed_ = dt > 0.0 ? 0.8 * estimated_speed_ + 0.2 * current_speed : 0.0; return true; } @@ -194,62 +166,21 @@ double LidartagHypothesis::getTransCov() const { const cv::Mat & cov = kalman_filter_.errorCovPost; - double max_transl_cov = + double max_translation_cov = std::max({cov.at(0, 0), cov.at(1, 1), cov.at(2, 2)}); - return std::sqrt(max_transl_cov); -} - -double LidartagHypothesis::getTransDotCov() const -{ - const cv::Mat & cov = kalman_filter_.errorCovPost; - - double max_transl_dot_cov = - dynamics_model_ == tier4_tag_utils::DynamicsModel::Static - ? 0.0 - : std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); - - return std::sqrt(max_transl_dot_cov); + return std::sqrt(max_translation_cov); } double LidartagHypothesis::getRotCov() const { const cv::Mat & cov = kalman_filter_.errorCovPost; - - double max_rot_cov = - dynamics_model_ == tier4_tag_utils::DynamicsModel::Static - ? std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}) - : std::max({cov.at(6, 6), cov.at(7, 7), cov.at(8, 8)}); - - return std::sqrt(max_rot_cov); + double max_rotation_cov = + std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); + return std::sqrt(max_rotation_cov); } -double LidartagHypothesis::getRotDotCov() const -{ - const cv::Mat & cov = kalman_filter_.errorCovPost; - - double max_rot_dot_cov = - dynamics_model_ == tier4_tag_utils::DynamicsModel::Static - ? 0.0 - : std::max({cov.at(9, 9), cov.at(10, 10), cov.at(11, 11)}); - - return std::sqrt(max_rot_dot_cov); -} - -double LidartagHypothesis::getSpeed() const -{ - const cv::Mat & state = kalman_filter_.statePost; - - if (dynamics_model_ == tier4_tag_utils::DynamicsModel::Static) { - return estimated_speed_; - } - - double vx = state.at(3); - double vy = state.at(3); - double vz = state.at(3); - - return std::sqrt(vx * vx + vy * vy + vz * vz); -} +double LidartagHypothesis::getSpeed() const { return estimated_speed_; } bool LidartagHypothesis::converged() const { @@ -263,24 +194,20 @@ bool LidartagHypothesis::converged() const // decide based on the variance const cv::Mat & cov = kalman_filter_.errorCovPost; - double max_transl_cov = + double max_translation_cov = std::max({cov.at(0, 0), cov.at(1, 1), cov.at(2, 2)}); - double max_transl_dot_cov = - dynamics_model_ == tier4_tag_utils::DynamicsModel::Static - ? 0.0 - : std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); + double max_translation_dot_cov = 0.0; - double max_rot_cov = - dynamics_model_ == tier4_tag_utils::DynamicsModel::Static - ? std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}) - : std::max({cov.at(6, 6), cov.at(7, 7), cov.at(8, 8)}); + double max_rotation_cov = + std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); if ( - std::sqrt(max_transl_cov) > convergence_transl_ || - std::sqrt(max_transl_dot_cov) > convergence_transl_dot_ || - std::sqrt(max_rot_cov) > convergence_rot_ || - std::sqrt(max_transl_dot_cov) > convergence_rot_dot_ || getSpeed() > convergence_transl_dot_) { + std::sqrt(max_translation_cov) > convergence_translation_ || + std::sqrt(max_translation_dot_cov) > convergence_translation_dot_ || + std::sqrt(max_rotation_cov) > convergence_rotation_ || + std::sqrt(max_translation_dot_cov) > convergence_rotation_dot_ || + getSpeed() > convergence_translation_dot_) { return false; } @@ -297,86 +224,72 @@ double LidartagHypothesis::timeSinceLastObservation(const rclcpp::Time & stamp) return (stamp - last_observation_timestamp_).seconds(); } -void LidartagHypothesis::setDynamicsModel(DynamicsModel dynamics_model) -{ - dynamics_model_ = dynamics_model; -} - void LidartagHypothesis::setMinConvergenceTime(double convergence_time) { min_convergence_time_ = convergence_time; } void LidartagHypothesis::setMaxConvergenceThreshold( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { - convergence_transl_ = transl; - convergence_transl_dot_ = transl_dot; - convergence_rot_ = rot; - convergence_rot_dot_ = rot_dot; + convergence_translation_ = translation; + convergence_translation_dot_ = translation_dot; + convergence_rotation_ = rotation; + convergence_rotation_dot_ = rotation_dot; } -void LidartagHypothesis::setNewHypothesisThreshold(double max_transl, double max_rot) +void LidartagHypothesis::setNewHypothesisThreshold(double max_translation, double max_rotation) { - new_hypothesis_transl_ = max_transl; - new_hypothesis_rot_ = max_rot; + new_hypothesis_translation_ = max_translation; + new_hypothesis_rotation_ = max_rotation; } void LidartagHypothesis::setMaxNoObservationTime(double time) { max_no_observation_time_ = time; } -void LidartagHypothesis::setMeasurementNoise(double transl, double rot) +void LidartagHypothesis::setMeasurementNoise(double translation, double rotation) { - measurement_noise_transl_ = transl; - measurement_noise_rot_ = rot; + measurement_noise_translation_ = translation; + measurement_noise_rotation_ = rotation; } void LidartagHypothesis::setProcessNoise( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { - process_noise_transl_ = transl; - process_noise_transl_dot_ = transl_dot; - process_noise_rot_ = rot; - process_noise_rot_dot_ = rot_dot; + process_noise_translation_ = translation; + process_noise_translation_dot_ = translation_dot; + process_noise_rotation_ = rotation; + process_noise_rotation_dot_ = rotation_dot; } void LidartagHypothesis::initKalman( const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix) -{ - if (dynamics_model_ == tier4_tag_utils::DynamicsModel::Static) { - initStaticKalman(translation_vector, rotation_matrix); - } else { - initConstantVelocityKalman(translation_vector, rotation_matrix); - } -} - -void LidartagHypothesis::initStaticKalman( - const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix) { kalman_filter_.init(6, 6, 0, CV_64F); - const double process_cov_transl = process_noise_transl_ * process_noise_transl_; - const double process_cov_rot = process_noise_rot_ * process_noise_rot_; + const double process_cov_translation = process_noise_translation_ * process_noise_translation_; + const double process_cov_rotation = process_noise_rotation_ * process_noise_rotation_; cv::setIdentity(kalman_filter_.processNoiseCov, cv::Scalar::all(1.0)); - kalman_filter_.processNoiseCov.at(0, 0) = process_cov_transl; - kalman_filter_.processNoiseCov.at(1, 1) = process_cov_transl; - kalman_filter_.processNoiseCov.at(2, 2) = process_cov_transl; - kalman_filter_.processNoiseCov.at(3, 3) = process_cov_rot; - kalman_filter_.processNoiseCov.at(4, 4) = process_cov_rot; - kalman_filter_.processNoiseCov.at(5, 5) = process_cov_rot; + kalman_filter_.processNoiseCov.at(0, 0) = process_cov_translation; + kalman_filter_.processNoiseCov.at(1, 1) = process_cov_translation; + kalman_filter_.processNoiseCov.at(2, 2) = process_cov_translation; + kalman_filter_.processNoiseCov.at(3, 3) = process_cov_rotation; + kalman_filter_.processNoiseCov.at(4, 4) = process_cov_rotation; + kalman_filter_.processNoiseCov.at(5, 5) = process_cov_rotation; - const double measurement_cov_transl = measurement_noise_transl_ * measurement_noise_transl_; - const double measurement_cov_rot = measurement_noise_rot_ * measurement_noise_rot_; + const double measurement_cov_translation = + measurement_noise_translation_ * measurement_noise_translation_; + const double measurement_cov_rotation = measurement_noise_rotation_ * measurement_noise_rotation_; cv::setIdentity(kalman_filter_.measurementNoiseCov, cv::Scalar::all(1.0)); - kalman_filter_.measurementNoiseCov.at(0, 0) = measurement_cov_transl; - kalman_filter_.measurementNoiseCov.at(1, 1) = measurement_cov_transl; - kalman_filter_.measurementNoiseCov.at(2, 2) = measurement_cov_transl; - kalman_filter_.measurementNoiseCov.at(3, 3) = measurement_cov_rot; - kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rot; - kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rot; + kalman_filter_.measurementNoiseCov.at(0, 0) = measurement_cov_translation; + kalman_filter_.measurementNoiseCov.at(1, 1) = measurement_cov_translation; + kalman_filter_.measurementNoiseCov.at(2, 2) = measurement_cov_translation; + kalman_filter_.measurementNoiseCov.at(3, 3) = measurement_cov_rotation; + kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rotation; + kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rotation; cv::setIdentity(kalman_filter_.errorCovPost, cv::Scalar::all(1.0)); cv::setIdentity(kalman_filter_.transitionMatrix, cv::Scalar::all(1.0)); @@ -391,37 +304,39 @@ void LidartagHypothesis::initConstantVelocityKalman( kalman_filter_.init(12, 6, 0, CV_64F); double dt = 1.0; - const double process_cov_transl = process_noise_transl_ * process_noise_transl_; - const double process_cov_transl_dot = process_noise_transl_dot_ * process_noise_transl_dot_; - const double process_cov_rot = process_noise_rot_ * process_noise_rot_; - const double process_cov_rot_dot = process_noise_rot_dot_ * process_noise_rot_dot_; + const double process_cov_translation = process_noise_translation_ * process_noise_translation_; + const double process_cov_translation_dot = + process_noise_translation_dot_ * process_noise_translation_dot_; + const double process_cov_rotation = process_noise_rotation_ * process_noise_rotation_; + const double process_cov_rotation_dot = process_noise_rotation_dot_ * process_noise_rotation_dot_; cv::setIdentity(kalman_filter_.processNoiseCov, cv::Scalar::all(1.0)); - kalman_filter_.processNoiseCov.at(0, 0) = process_cov_transl; - kalman_filter_.processNoiseCov.at(1, 1) = process_cov_transl; - kalman_filter_.processNoiseCov.at(2, 2) = process_cov_transl; - kalman_filter_.processNoiseCov.at(3, 3) = process_cov_transl_dot; - kalman_filter_.processNoiseCov.at(4, 4) = process_cov_transl_dot; - kalman_filter_.processNoiseCov.at(5, 5) = process_cov_transl_dot; - kalman_filter_.processNoiseCov.at(6, 6) = process_cov_rot; - kalman_filter_.processNoiseCov.at(7, 7) = process_cov_rot; - kalman_filter_.processNoiseCov.at(8, 8) = process_cov_rot; - kalman_filter_.processNoiseCov.at(9, 9) = process_cov_rot_dot; - kalman_filter_.processNoiseCov.at(10, 10) = process_cov_rot_dot; - kalman_filter_.processNoiseCov.at(11, 11) = process_cov_rot_dot; - - const double measurement_cov_transl = measurement_noise_transl_ * measurement_noise_transl_; - const double measurement_cov_rot = measurement_noise_rot_ * measurement_noise_rot_; + kalman_filter_.processNoiseCov.at(0, 0) = process_cov_translation; + kalman_filter_.processNoiseCov.at(1, 1) = process_cov_translation; + kalman_filter_.processNoiseCov.at(2, 2) = process_cov_translation; + kalman_filter_.processNoiseCov.at(3, 3) = process_cov_translation_dot; + kalman_filter_.processNoiseCov.at(4, 4) = process_cov_translation_dot; + kalman_filter_.processNoiseCov.at(5, 5) = process_cov_translation_dot; + kalman_filter_.processNoiseCov.at(6, 6) = process_cov_rotation; + kalman_filter_.processNoiseCov.at(7, 7) = process_cov_rotation; + kalman_filter_.processNoiseCov.at(8, 8) = process_cov_rotation; + kalman_filter_.processNoiseCov.at(9, 9) = process_cov_rotation_dot; + kalman_filter_.processNoiseCov.at(10, 10) = process_cov_rotation_dot; + kalman_filter_.processNoiseCov.at(11, 11) = process_cov_rotation_dot; + + const double measurement_cov_translation = + measurement_noise_translation_ * measurement_noise_translation_; + const double measurement_cov_rotation = measurement_noise_rotation_ * measurement_noise_rotation_; cv::setIdentity(kalman_filter_.measurementNoiseCov, cv::Scalar::all(1.0)); - kalman_filter_.measurementNoiseCov.at(0, 0) = measurement_cov_transl; - kalman_filter_.measurementNoiseCov.at(1, 1) = measurement_cov_transl; - kalman_filter_.measurementNoiseCov.at(2, 2) = measurement_cov_transl; - kalman_filter_.measurementNoiseCov.at(3, 3) = measurement_cov_rot; - kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rot; - kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rot; + kalman_filter_.measurementNoiseCov.at(0, 0) = measurement_cov_translation; + kalman_filter_.measurementNoiseCov.at(1, 1) = measurement_cov_translation; + kalman_filter_.measurementNoiseCov.at(2, 2) = measurement_cov_translation; + kalman_filter_.measurementNoiseCov.at(3, 3) = measurement_cov_rotation; + kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rotation; + kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rotation; cv::setIdentity(kalman_filter_.errorCovPost, cv::Scalar::all(1.0)); @@ -474,30 +389,16 @@ cv::Mat LidartagHypothesis::toState( { cv::Matx31d euler_angles = rot2euler(rotation_matrix); - if (dynamics_model_ == tier4_tag_utils::DynamicsModel::Static) { - cv::Mat kalman_state(6, 1, CV_64F); - - kalman_state.at(0, 0) = translation_vector(0, 0); - kalman_state.at(1, 0) = translation_vector(1, 0); - kalman_state.at(2, 0) = translation_vector(2, 0); - kalman_state.at(3, 0) = euler_angles(0, 0); - kalman_state.at(4, 0) = euler_angles(1, 0); - kalman_state.at(5, 0) = euler_angles(2, 0); - - return kalman_state; - } else { - cv::Mat kalman_state(12, 1, CV_64F); - kalman_state.setTo(cv::Scalar(0.0)); - - kalman_state.at(0, 0) = translation_vector(0, 0); - kalman_state.at(1, 0) = translation_vector(1, 0); - kalman_state.at(2, 0) = translation_vector(2, 0); - kalman_state.at(6, 0) = euler_angles(0, 0); - kalman_state.at(7, 0) = euler_angles(1, 0); - kalman_state.at(8, 0) = euler_angles(2, 0); - - return kalman_state; - } + cv::Mat kalman_state(6, 1, CV_64F); + + kalman_state.at(0, 0) = translation_vector(0, 0); + kalman_state.at(1, 0) = translation_vector(1, 0); + kalman_state.at(2, 0) = translation_vector(2, 0); + kalman_state.at(3, 0) = euler_angles(0, 0); + kalman_state.at(4, 0) = euler_angles(1, 0); + kalman_state.at(5, 0) = euler_angles(2, 0); + + return kalman_state; } cv::Mat LidartagHypothesis::toObservation( @@ -519,60 +420,33 @@ cv::Mat LidartagHypothesis::toObservation( void LidartagHypothesis::fixState(cv::Mat & old_state, cv::Mat & new_prediction) { - if (dynamics_model_ == tier4_tag_utils::DynamicsModel::Static) { - cv::Mat kalman_state(6, 1, CV_64F); - - double old_x = old_state.at(3, 0); - double old_z = old_state.at(5, 0); - - double & new_x = new_prediction.at(3, 0); - double & new_z = new_prediction.at(5, 0); - - new_x = std::abs(new_x + CV_2PI - old_x) < std::abs(new_x - old_x) ? new_x + CV_2PI - : std::abs(new_x - CV_2PI - old_x) < std::abs(new_x - old_x) ? new_x - CV_2PI - : new_x; - - new_z = std::abs(new_z + CV_2PI - old_z) < std::abs(new_z - old_z) ? new_z + CV_2PI - : std::abs(new_z - CV_2PI - old_z) < std::abs(new_z - old_z) ? new_z - CV_2PI - : new_z; + cv::Mat kalman_state(6, 1, CV_64F); - return; - } else { - double old_x = old_state.at(6, 0); - double old_z = old_state.at(8, 0); + double old_x = old_state.at(3, 0); + double old_z = old_state.at(5, 0); - double & new_x = new_prediction.at(3, 0); - double & new_z = new_prediction.at(5, 0); + double & new_x = new_prediction.at(3, 0); + double & new_z = new_prediction.at(5, 0); - new_x = std::abs(new_x + CV_2PI - old_x) < std::abs(new_x - old_x) ? new_x + CV_2PI - : std::abs(new_x - CV_2PI - old_x) < std::abs(new_x - old_x) ? new_x - CV_2PI - : new_x; + new_x = std::abs(new_x + CV_2PI - old_x) < std::abs(new_x - old_x) ? new_x + CV_2PI + : std::abs(new_x - CV_2PI - old_x) < std::abs(new_x - old_x) ? new_x - CV_2PI + : new_x; - new_z = std::abs(new_z + CV_2PI - old_z) < std::abs(new_z - old_z) ? new_z + CV_2PI - : std::abs(new_z - CV_2PI - old_z) < std::abs(new_z - old_z) ? new_z - CV_2PI - : new_z; + new_z = std::abs(new_z + CV_2PI - old_z) < std::abs(new_z - old_z) ? new_z + CV_2PI + : std::abs(new_z - CV_2PI - old_z) < std::abs(new_z - old_z) ? new_z - CV_2PI + : new_z; - return; - } + return; } void LidartagHypothesis::fixState(cv::Mat & new_state) { - if (dynamics_model_ == tier4_tag_utils::DynamicsModel::Static) { - double & new_x = new_state.at(3, 0); - double & new_z = new_state.at(5, 0); - - new_x = std::min(std::max(-CV_PI, new_x), CV_PI); - new_z = std::min(std::max(-CV_PI, new_z), CV_PI); - return; - } else { - double & new_x = new_state.at(6, 0); - double & new_z = new_state.at(8, 0); - - new_x = std::min(std::max(-CV_PI, new_x), CV_PI); - new_z = std::min(std::max(-CV_PI, new_z), CV_PI); - return; - } + double & new_x = new_state.at(3, 0); + double & new_z = new_state.at(5, 0); + + new_x = std::min(std::max(-CV_PI, new_x), CV_PI); + new_z = std::min(std::max(-CV_PI, new_z), CV_PI); + return; } // Converts a given Rotation Matrix to Euler angles diff --git a/sensor/docs/how_to_extrinsic_interactive.md b/sensor/docs/how_to_extrinsic_interactive.md index c55d24e0..d09f499b 100644 --- a/sensor/docs/how_to_extrinsic_interactive.md +++ b/sensor/docs/how_to_extrinsic_interactive.md @@ -97,7 +97,7 @@ Calibrating a camera-lidar pair (i.e., finding the extrinsics) makes the lidar p The difference between a point in the image (pimage), and the projection (pprojected) in the image of its corresponding object point in lidar coordinates (pobject) is denoted as the reprojection error and can be observed graphically in Figure 3.
- +
Fig 3. Reprojection error
@@ -274,8 +274,8 @@ The `Data collection tools` implement several optional functionalities meant to The `Calibration status` implement shows several numerical results of the calibration process and also implements save/load capabilities. 1. `Calibration points`: The current number of image-object pairs of points. -2. `Reproj error`: The reprojection error of the current pairs of points. On the left, the reprojection of the calibrated extrinsics is displayed, whereas on the right, the reprojection of the extrinsics used for visualization is displayed (this allows, for example, to compare the reprojection error of the current calibration with the initial one). -3. `Inliers`: The number of points that are considered to be inliers for a specific calibration. Same as with `Reproj error`, the inliers for the calibrated and visualization extrinsics are displayed. +2. `Reprojection error`: The reprojection error of the current pairs of points. On the left, the reprojection of the calibrated extrinsics is displayed, whereas on the right, the reprojection of the extrinsics used for visualization is displayed (this allows, for example, to compare the reprojection error of the current calibration with the initial one). +3. `Inliers`: The number of points that are considered to be inliers for a specific calibration. Same as with `Reprojection error`, the inliers for the calibrated and visualization extrinsics are displayed. 4. `Save calibration`: Selects a folder to save the current calibration. It outputs the image-object calibration pairs of points, the camera-lidar extrinsics (not the one requested by the `Calibration tools`, which is usually the `sensor_kit` to `camera`), and the optimized intrinsics (when they are computed). 5. `Load calibration`: Selects a folder to load a previous calibration from the Interactive calibration tool. This only loads the image-object pairs of points (not the intrinsics/extrinsics). diff --git a/sensor/docs/how_to_extrinsic_tag_based.md b/sensor/docs/how_to_extrinsic_tag_based.md index 6a55e8fb..08273b6f 100644 --- a/sensor/docs/how_to_extrinsic_tag_based.md +++ b/sensor/docs/how_to_extrinsic_tag_based.md @@ -131,11 +131,11 @@ Calibrating a camera-lidar pair (i.e., finding the extrinsics) makes the lidar p The difference between a point in the image (pimage), and the projection (pprojected) in the image of its corresponding object point in lidar coordinates (pobject) is denoted as the reprojection error and can be observed graphically in Figure 5.
- +
Fig 5. Reprojection error
-It follows that to calibrate the extrinsics of the camera-lidar pair of sensors, the reprojection error must be minimized in a set of corresponding pairs of points. The acquisition of the calibration pairs of points can be performed either automatically via a computer vision system or manually, and in this case, the pairs are computed automatically from the corners of the lidartag placeds in the field of view of the sensors +It follows that to calibrate the extrinsics of the camera-lidar pair of sensors, the reprojection error must be minimized in a set of corresponding pairs of points. The acquisition of the calibration pairs of points can be performed either automatically via a computer vision system or manually, and in this case, the pairs are computed automatically from the corners of the lidartag placed in the field of view of the sensors ## 5. Tag based Calibration Process @@ -145,11 +145,11 @@ The `apriltag` node detects the corners of the tags in the image, the `lidartag` However, in addition to the automatic calibration process, this tool also launches the Interactive calibrator UI, mainly for visualization purposes, but can also be used to use different optimization options, add additional calibration points, etc. However, the calibrations obtained through the UI can not be sent to the `Calibration manager` and instead must be saved manually (refer to the UI manual for more details). -| ![tagbased-1.jpg](images/camera-lidar/tagbased-1.jpg) | ![tagbased-2.jpg](images/camera-lidar/tagbased-2.jpg) | -| :---------------------------------------------------: | :---------------------------------------------------: | -| Fig 6. Initial calibration | Fig 7. Automatic tag-based calibration | +| ![tag_based-1.jpg](images/camera-lidar/tag_based-1.jpg) | ![tag_based-2.jpg](images/camera-lidar/tag_based-2.jpg) | +| :-----------------------------------------------------: | :-----------------------------------------------------: | +| Fig 6. Initial calibration | Fig 7. Automatic tag-based calibration | -The calibration tool is compatible with one or multiple tags, but a certain amount of camera-lidar detections is needed in order to obtain a correct calibration. In case that the number of tags at hand is not sufficient, the user can move a single tag to multiple locations in order to collect more detections akin to the camera calibration process. The parameters that determine how many detections are required in order for the algorithm to finish and output the final extrinsica are located in the `sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml` launch file and below is an example of the related paramers: +The calibration tool is compatible with one or multiple tags, but a certain amount of camera-lidar detections is needed in order to obtain a correct calibration. In case that the number of tags at hand is not sufficient, the user can move a single tag to multiple locations in order to collect more detections akin to the camera calibration process. The parameters that determine how many detections are required in order for the algorithm to finish and output the final extrinsics are located in the `sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml` launch file and below is an example of the related parameters: ```yaml diff --git a/sensor/docs/images/camera-lidar/reprojerror.svg b/sensor/docs/images/camera-lidar/reprojection_error.svg similarity index 100% rename from sensor/docs/images/camera-lidar/reprojerror.svg rename to sensor/docs/images/camera-lidar/reprojection_error.svg diff --git a/sensor/docs/images/camera-lidar/tagbased-1.jpg b/sensor/docs/images/camera-lidar/tag_based-1.jpg similarity index 100% rename from sensor/docs/images/camera-lidar/tagbased-1.jpg rename to sensor/docs/images/camera-lidar/tag_based-1.jpg diff --git a/sensor/docs/images/camera-lidar/tagbased-2.jpg b/sensor/docs/images/camera-lidar/tag_based-2.jpg similarity index 100% rename from sensor/docs/images/camera-lidar/tagbased-2.jpg rename to sensor/docs/images/camera-lidar/tag_based-2.jpg diff --git a/sensor/extrinsic_calibration_client/CMakeLists.txt b/sensor/extrinsic_calibration_client/CMakeLists.txt deleted file mode 100644 index eb6b61bc..00000000 --- a/sensor/extrinsic_calibration_client/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(extrinsic_calibration_client) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_executable(extrinsic_calibration_client - src/extrinsic_calibration_client.cpp) -ament_target_dependencies(extrinsic_calibration_client) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/sensor/extrinsic_calibration_client/package.xml b/sensor/extrinsic_calibration_client/package.xml deleted file mode 100644 index a43aef55..00000000 --- a/sensor/extrinsic_calibration_client/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - extrinsic_calibration_client - 0.1.0 - The extrinsic calibration client package - Yohei Mishina - Apache License 2.0 - - ament_cmake_auto - - rclcpp - std_msgs - tier4_calibration_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/sensor/extrinsic_calibration_client/src/extrinsic_calibration_client.cpp b/sensor/extrinsic_calibration_client/src/extrinsic_calibration_client.cpp deleted file mode 100644 index 4f89b3d7..00000000 --- a/sensor/extrinsic_calibration_client/src/extrinsic_calibration_client.cpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2023 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. - -#include "rclcpp/rclcpp.hpp" - -#include "tier4_calibration_msgs/srv/extrinsic_calibration_manager.hpp" - -#include -#include - -int main(int argc, char * argv[]) -{ - using std::chrono_literals::operator""s; - - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("extrinsic_calibration_client"); - - auto client = node->create_client( - "extrinsic_calibration_manager"); - - while (!client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service."); - rclcpp::shutdown(); - return 1; - } - RCLCPP_INFO(node->get_logger(), "Waiting for service..."); - } - - auto request = - std::make_shared(); - request->src_path = node->declare_parameter("src_path", ""); - request->dst_path = node->declare_parameter("dst_path", ""); - - auto future = client->async_send_request(request); - - if (rclcpp::spin_until_future_complete(node, future) == rclcpp::FutureReturnCode::SUCCESS) { - auto result = future.get(); - RCLCPP_INFO_STREAM( - node->get_logger(), "Received service message. success " << result.get()->success << " score " - << result.get()->score); - } else { - RCLCPP_ERROR(node->get_logger(), "Problem while waiting for response."); - } - - rclcpp::shutdown(); - return 0; -} diff --git a/sensor/extrinsic_calibration_manager/CMakeLists.txt b/sensor/extrinsic_calibration_manager/CMakeLists.txt deleted file mode 100644 index 35a1d38d..00000000 --- a/sensor/extrinsic_calibration_manager/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(extrinsic_calibration_manager) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_executable(extrinsic_calibration_manager - src/extrinsic_calibration_manager_node.cpp -) - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/sensor/extrinsic_calibration_manager/config/aip_x1/extrinsic_manual_calibration.rviz b/sensor/extrinsic_calibration_manager/config/aip_x1/extrinsic_manual_calibration.rviz deleted file mode 100644 index eacb9e04..00000000 --- a/sensor/extrinsic_calibration_manager/config/aip_x1/extrinsic_manual_calibration.rviz +++ /dev/null @@ -1,416 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1/Frames1 - - /LiDAR11 - - /LiDAR21 - - /Camera1 - - /PointCloud21 - Splitter Ratio: 0.33018869161605835 - Tree Height: 751 - - 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: PointCloud2 -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: 100 - Reference Frame: - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link: - Value: true - camera0/camera_link: - Value: true - camera0/camera_optical_link: - Value: true - camera1/camera_link: - Value: true - camera1/camera_optical_link: - Value: true - camera2/camera_link: - Value: true - camera2/camera_optical_link: - Value: true - camera3/camera_link: - Value: true - camera3/camera_optical_link: - Value: true - camera4/camera_link: - Value: true - camera4/camera_optical_link: - Value: true - camera5/camera_link: - Value: true - camera5/camera_optical_link: - Value: true - front_unit_base_link: - Value: true - gnss_base_link: - Value: true - gnss_left_link: - Value: true - gnss_right)link: - Value: true - gnss_right_link: - Value: true - map: - Value: true - pandar_40p_front: - Value: true - pandar_40p_front_base_link: - Value: true - pandar_40p_left: - Value: true - pandar_40p_left_base_link: - Value: true - pandar_40p_rear: - Value: true - pandar_40p_rear_base_link: - Value: true - pandar_40p_right: - Value: true - pandar_40p_right_base_link: - Value: true - pandar_qt_front: - Value: true - pandar_qt_front_base_link: - Value: true - pandar_qt_left: - Value: true - pandar_qt_left_base_link: - Value: true - pandar_qt_rear: - Value: true - pandar_qt_rear_base_link: - Value: true - pandar_qt_right: - Value: true - pandar_qt_right_base_link: - Value: true - rear_unit_base_link: - Value: true - tamagawa/imu_link: - Value: true - top_unit_base_link: - Value: true - traffic_light_camera/camera_link: - Value: true - traffic_light_camera/camera_optical_link: - Value: true - Marker Scale: 0.5 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - base_link: - front_unit_base_link: - pandar_40p_front_base_link: - pandar_40p_front: - {} - pandar_qt_front_base_link: - pandar_qt_front: - {} - rear_unit_base_link: - camera3/camera_link: - camera3/camera_optical_link: - {} - pandar_40p_rear_base_link: - pandar_40p_rear: - {} - pandar_qt_rear_base_link: - pandar_qt_rear: - {} - top_unit_base_link: - camera0/camera_link: - camera0/camera_optical_link: - {} - camera1/camera_link: - camera1/camera_optical_link: - {} - camera2/camera_link: - camera2/camera_optical_link: - {} - camera4/camera_link: - camera4/camera_optical_link: - {} - camera5/camera_link: - camera5/camera_optical_link: - {} - gnss_left_link: - {} - gnss_right)link: - {} - gnss_right_link: - {} - pandar_40p_left_base_link: - pandar_40p_left: - {} - pandar_40p_right_base_link: - pandar_40p_right: - {} - pandar_qt_left_base_link: - pandar_qt_left: - {} - pandar_qt_right_base_link: - pandar_qt_right: - {} - tamagawa/imu_link: - {} - traffic_light_camera/camera_link: - traffic_light_camera/camera_optical_link: - {} - gnss_base_link: - {} - Update Interval: 0 - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LiDAR1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 124 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LiDAR2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Camera - Enabled: true - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera0/image_raw - Value: true - Visibility: - Grid: true - LiDAR1: true - LiDAR2: true - PointCloud2: true - TF: true - Value: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - 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/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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: 22.56075668334961 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 4.83912467956543 - Y: -0.4213109016418457 - Z: 0.3178406059741974 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4353978633880615 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.238572597503662 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002380000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002160000037afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000204000000a400fffffffb0000000c00430061006d0065007200610100000247000001700000002800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003260000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/sensor/extrinsic_calibration_manager/config/aip_x2/extrinsic_manual_calibration.rviz b/sensor/extrinsic_calibration_manager/config/aip_x2/extrinsic_manual_calibration.rviz deleted file mode 100644 index 2ac32068..00000000 --- a/sensor/extrinsic_calibration_manager/config/aip_x2/extrinsic_manual_calibration.rviz +++ /dev/null @@ -1,590 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1/Frames1 - - /Front Upper1/Topic1 - - /Left Lower1/Topic1 - - /Rear Lower1/Status1 - - /Rear Lower1/Topic1 - - /Rear Upper1/Topic1 - - /Right Lower1/Topic1 - - /Right Upper1/Topic1 - Splitter Ratio: 0.33018869161605835 - Tree Height: 751 - - 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: Front Lower -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: 100 - Reference Frame: - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link: - Value: true - camera0/camera_link: - Value: true - camera0/camera_optical_link: - Value: true - camera1/camera_link: - Value: true - camera1/camera_optical_link: - Value: true - camera2/camera_link: - Value: true - camera2/camera_optical_link: - Value: true - camera3/camera_link: - Value: true - camera3/camera_optical_link: - Value: true - camera4/camera_link: - Value: true - camera4/camera_optical_link: - Value: true - camera5/camera_link: - Value: true - camera5/camera_optical_link: - Value: true - front_unit_base_link: - Value: true - gnss_base_link: - Value: true - gnss_left_link: - Value: true - gnss_right_link: - Value: true - map: - Value: true - pandar_40p_front: - Value: true - pandar_40p_front_base_link: - Value: true - pandar_40p_left: - Value: true - pandar_40p_left_base_link: - Value: true - pandar_40p_rear: - Value: true - pandar_40p_rear_base_link: - Value: true - pandar_40p_right: - Value: true - pandar_40p_right_base_link: - Value: true - pandar_qt_front: - Value: true - pandar_qt_front_base_link: - Value: true - pandar_qt_left: - Value: true - pandar_qt_left_base_link: - Value: true - pandar_qt_rear: - Value: true - pandar_qt_rear_base_link: - Value: true - pandar_qt_right: - Value: true - pandar_qt_right_base_link: - Value: true - rear_unit_base_link: - Value: true - tamagawa/imu_link: - Value: true - top_unit_base_link: - Value: true - traffic_light_camera/camera_link: - Value: true - traffic_light_camera/camera_optical_link: - Value: true - Marker Scale: 0.5 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - base_link: - front_unit_base_link: - pandar_40p_front_base_link: - pandar_40p_front: - {} - pandar_qt_front_base_link: - pandar_qt_front: - {} - rear_unit_base_link: - camera3/camera_link: - camera3/camera_optical_link: - {} - pandar_40p_rear_base_link: - pandar_40p_rear: - {} - pandar_qt_rear_base_link: - pandar_qt_rear: - {} - top_unit_base_link: - camera0/camera_link: - camera0/camera_optical_link: - {} - camera1/camera_link: - camera1/camera_optical_link: - {} - camera2/camera_link: - camera2/camera_optical_link: - {} - camera4/camera_link: - camera4/camera_optical_link: - {} - camera5/camera_link: - camera5/camera_optical_link: - {} - gnss_left_link: - {} - gnss_right_link: - {} - pandar_40p_left_base_link: - pandar_40p_left: - {} - pandar_40p_right_base_link: - pandar_40p_right: - {} - pandar_qt_left_base_link: - pandar_qt_left: - {} - pandar_qt_right_base_link: - pandar_qt_right: - {} - tamagawa/imu_link: - {} - traffic_light_camera/camera_link: - traffic_light_camera/camera_optical_link: - {} - gnss_base_link: - {} - Update Interval: 0 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Left Upper(target) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera0/image_raw - Value: false - Visibility: - Front Lower: true - Front Upper: true - Grid: true - Left Lower: true - Left Upper(target): true - Rear Lower: true - Rear Upper: true - Right Lower: true - Right Upper: true - TF: true - Value: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Front Lower - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 4.584362506866455 - Min Value: -0.04530531167984009 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 6 - Name: Front Upper - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 4.287570476531982 - Min Value: -0.060530662536621094 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 5 - Name: Left Lower - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 26.287418365478516 - Min Value: -0.658060610294342 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Rear Lower - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/rear_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 1.8236280679702759 - Min Value: -0.14003396034240723 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 6 - Name: Rear Upper - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/rear_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 4.034997940063477 - Min Value: -0.09023833274841309 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 5 - Name: Right Lower - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/right_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.91996955871582 - Min Value: -1.3739814758300781 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Right Upper - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/right_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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: 16.576000213623047 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7853981852531433 - Target Frame: - Value: Orbit (rviz_default_plugins) - Yaw: 0.7853981852531433 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002380000037afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002ee000000c9000000000000000000000001000002160000037afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037a000000a400fffffffb0000000c00430061006d0065007200610000000247000001700000002800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005420000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 0 diff --git a/sensor/extrinsic_calibration_manager/config/aip_xx1/extrinsic_manual_calibration.rviz b/sensor/extrinsic_calibration_manager/config/aip_xx1/extrinsic_manual_calibration.rviz deleted file mode 100644 index eacb9e04..00000000 --- a/sensor/extrinsic_calibration_manager/config/aip_xx1/extrinsic_manual_calibration.rviz +++ /dev/null @@ -1,416 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1/Frames1 - - /LiDAR11 - - /LiDAR21 - - /Camera1 - - /PointCloud21 - Splitter Ratio: 0.33018869161605835 - Tree Height: 751 - - 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: PointCloud2 -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: 100 - Reference Frame: - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link: - Value: true - camera0/camera_link: - Value: true - camera0/camera_optical_link: - Value: true - camera1/camera_link: - Value: true - camera1/camera_optical_link: - Value: true - camera2/camera_link: - Value: true - camera2/camera_optical_link: - Value: true - camera3/camera_link: - Value: true - camera3/camera_optical_link: - Value: true - camera4/camera_link: - Value: true - camera4/camera_optical_link: - Value: true - camera5/camera_link: - Value: true - camera5/camera_optical_link: - Value: true - front_unit_base_link: - Value: true - gnss_base_link: - Value: true - gnss_left_link: - Value: true - gnss_right)link: - Value: true - gnss_right_link: - Value: true - map: - Value: true - pandar_40p_front: - Value: true - pandar_40p_front_base_link: - Value: true - pandar_40p_left: - Value: true - pandar_40p_left_base_link: - Value: true - pandar_40p_rear: - Value: true - pandar_40p_rear_base_link: - Value: true - pandar_40p_right: - Value: true - pandar_40p_right_base_link: - Value: true - pandar_qt_front: - Value: true - pandar_qt_front_base_link: - Value: true - pandar_qt_left: - Value: true - pandar_qt_left_base_link: - Value: true - pandar_qt_rear: - Value: true - pandar_qt_rear_base_link: - Value: true - pandar_qt_right: - Value: true - pandar_qt_right_base_link: - Value: true - rear_unit_base_link: - Value: true - tamagawa/imu_link: - Value: true - top_unit_base_link: - Value: true - traffic_light_camera/camera_link: - Value: true - traffic_light_camera/camera_optical_link: - Value: true - Marker Scale: 0.5 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - base_link: - front_unit_base_link: - pandar_40p_front_base_link: - pandar_40p_front: - {} - pandar_qt_front_base_link: - pandar_qt_front: - {} - rear_unit_base_link: - camera3/camera_link: - camera3/camera_optical_link: - {} - pandar_40p_rear_base_link: - pandar_40p_rear: - {} - pandar_qt_rear_base_link: - pandar_qt_rear: - {} - top_unit_base_link: - camera0/camera_link: - camera0/camera_optical_link: - {} - camera1/camera_link: - camera1/camera_optical_link: - {} - camera2/camera_link: - camera2/camera_optical_link: - {} - camera4/camera_link: - camera4/camera_optical_link: - {} - camera5/camera_link: - camera5/camera_optical_link: - {} - gnss_left_link: - {} - gnss_right)link: - {} - gnss_right_link: - {} - pandar_40p_left_base_link: - pandar_40p_left: - {} - pandar_40p_right_base_link: - pandar_40p_right: - {} - pandar_qt_left_base_link: - pandar_qt_left: - {} - pandar_qt_right_base_link: - pandar_qt_right: - {} - tamagawa/imu_link: - {} - traffic_light_camera/camera_link: - traffic_light_camera/camera_optical_link: - {} - gnss_base_link: - {} - Update Interval: 0 - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LiDAR1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 124 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LiDAR2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Camera - Enabled: true - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera0/image_raw - Value: true - Visibility: - Grid: true - LiDAR1: true - LiDAR2: true - PointCloud2: true - TF: true - Value: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - 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/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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: 22.56075668334961 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 4.83912467956543 - Y: -0.4213109016418457 - Z: 0.3178406059741974 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4353978633880615 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.238572597503662 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002380000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002160000037afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000204000000a400fffffffb0000000c00430061006d0065007200610100000247000001700000002800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003260000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/sensor/extrinsic_calibration_manager/config/extrinsic_calibration.rviz b/sensor/extrinsic_calibration_manager/config/extrinsic_calibration.rviz deleted file mode 100644 index 9e6282ea..00000000 --- a/sensor/extrinsic_calibration_manager/config/extrinsic_calibration.rviz +++ /dev/null @@ -1,355 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1/Frames1 - - /LiDAR11 - - /LiDAR21 - - /Camera1 - Splitter Ratio: 0.33018869161605835 - Tree Height: 724 - - 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: "" -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: 100 - Reference Frame: - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link: - Value: true - camera0/camera_link: - Value: true - camera0/camera_optical_link: - Value: true - camera1/camera_link: - Value: true - camera1/camera_optical_link: - Value: true - camera2/camera_link: - Value: true - camera2/camera_optical_link: - Value: true - camera3/camera_link: - Value: true - camera3/camera_optical_link: - Value: true - camera4/camera_link: - Value: true - camera4/camera_optical_link: - Value: true - camera5/camera_link: - Value: true - camera5/camera_optical_link: - Value: true - gnss_link: - Value: true - livox_front_left: - Value: true - livox_front_left_base_link: - Value: true - livox_front_right: - Value: true - livox_front_right_base_link: - Value: true - map: - Value: true - sensor_kit_base_link: - Value: true - tamagawa/imu_link: - Value: true - traffic_light_left_camera/camera_link: - Value: true - traffic_light_left_camera/camera_optical_link: - Value: true - traffic_light_right_camera/camera_link: - Value: true - traffic_light_right_camera/camera_optical_link: - Value: true - velodyne_left: - Value: true - velodyne_left_base_link: - Value: true - velodyne_rear: - Value: true - velodyne_rear_base_link: - Value: true - velodyne_right: - Value: true - velodyne_right_base_link: - Value: true - velodyne_top: - Value: true - velodyne_top_base_link: - Value: true - Marker Scale: 0.5 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - base_link: - livox_front_left_base_link: - livox_front_left: - {} - livox_front_right_base_link: - livox_front_right: - {} - sensor_kit_base_link: - camera0/camera_link: - camera0/camera_optical_link: - {} - camera1/camera_link: - camera1/camera_optical_link: - {} - camera2/camera_link: - camera2/camera_optical_link: - {} - camera3/camera_link: - camera3/camera_optical_link: - {} - camera4/camera_link: - camera4/camera_optical_link: - {} - camera5/camera_link: - camera5/camera_optical_link: - {} - gnss_link: - {} - tamagawa/imu_link: - {} - traffic_light_left_camera/camera_link: - traffic_light_left_camera/camera_optical_link: - {} - traffic_light_right_camera/camera_link: - traffic_light_right_camera/camera_optical_link: - {} - velodyne_left_base_link: - velodyne_left: - {} - velodyne_right_base_link: - velodyne_right: - {} - velodyne_top_base_link: - velodyne_top: - {} - velodyne_rear_base_link: - velodyne_rear: - {} - Update Interval: 0 - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LiDAR1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 124 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LiDAR2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Camera - Enabled: true - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera0/image_raw - Value: true - Visibility: - Grid: true - LiDAR1: true - LiDAR2: true - TF: true - Value: true - Zoom Factor: 1 - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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: 10 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7853981852531433 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.7853981852531433 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002140000035ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002160000035ffc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000001f4000000a400fffffffb0000000c00430061006d0065007200610100000237000001650000002800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000034a0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 27 diff --git a/sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp b/sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp deleted file mode 100644 index 7468a122..00000000 --- a/sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2023 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. - -#ifndef EXTRINSIC_CALIBRATION_MANAGER__EXTRINSIC_CALIBRATION_MANAGER_NODE_HPP_ -#define EXTRINSIC_CALIBRATION_MANAGER__EXTRINSIC_CALIBRATION_MANAGER_NODE_HPP_ - -#include - -#include -#include -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#else -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#endif -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_broadcaster.h" - -#include "tier4_calibration_msgs/srv/extrinsic_calibration_manager.hpp" -#include "tier4_calibration_msgs/srv/extrinsic_calibrator.hpp" - -class ExtrinsicCalibrationManagerNode : public rclcpp::Node -{ -public: - explicit ExtrinsicCalibrationManagerNode(const rclcpp::NodeOptions & options); - void spin(); - -private: - rclcpp::Service::SharedPtr server_; - std::vector::SharedPtr> clients_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - - struct TargetClient - { - std::string parent_frame; - std::string child_frame; - tier4_calibration_msgs::srv::ExtrinsicCalibrator::Request request; - tier4_calibration_msgs::srv::ExtrinsicCalibrator::Response response; - rclcpp::Client::SharedPtr client; - bool estimated = false; - }; - - std::vector target_clients_; - std::string dst_path_; - YAML::Node yaml_node_; - - static constexpr int yaml_precision_ = 6; - - std::string parent_frame_; - std::vector child_frames_; - std::string client_ns_; - double threshold_; - - void calibrationRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response); - - bool createTargetClient( - const YAML::Node & yaml_node, const std::string & parent_frame, const std::string & child_frame, - const std::string & client_ns, const rclcpp::CallbackGroup::SharedPtr & callback_group, - TargetClient & target_client); - geometry_msgs::msg::Pose getPoseFromYaml( - const YAML::Node & yaml_node, const std::string & parent_frame, - const std::string & child_frame); - bool dumpCalibrationConfig( - const std::string & path, const std::vector & target_clients); -}; - -#endif // EXTRINSIC_CALIBRATION_MANAGER__EXTRINSIC_CALIBRATION_MANAGER_NODE_HPP_ diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane.launch.xml deleted file mode 100644 index 7a5e7116..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml deleted file mode 100644 index 59e0b784..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d.launch.xml deleted file mode 100644 index 04f3ef16..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d_sensor_kit.launch.xml deleted file mode 100644 index 065b3eb6..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d_sensor_kit.launch.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/manual.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/manual.launch.xml deleted file mode 100644 index 497673f2..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/manual.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensor_kit.launch.xml deleted file mode 100644 index aaa88125..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensor_kit.launch.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensors.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensors.launch.xml deleted file mode 100644 index 9e5782fe..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensors.launch.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml deleted file mode 100644 index fdbb0bf5..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml deleted file mode 100644 index 9d6961bc..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml +++ /dev/null @@ -1,92 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based.launch.xml deleted file mode 100644 index d979785b..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based_sensor_kit.launch.xml deleted file mode 100644 index a004c3b7..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based_sensor_kit.launch.xml +++ /dev/null @@ -1,108 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane.launch.xml deleted file mode 100644 index 98b334df..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane.launch.xml +++ /dev/null @@ -1,67 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_front_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_front_unit.launch.xml deleted file mode 100644 index b1cf21e3..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_front_unit.launch.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_rear_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_rear_unit.launch.xml deleted file mode 100644 index ccf5efc8..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_rear_unit.launch.xml +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_top_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_top_unit.launch.xml deleted file mode 100644 index 01bf0f4f..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_top_unit.launch.xml +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive.launch.xml deleted file mode 100644 index 4dfc9f14..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive.launch.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_front_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_front_unit.launch.xml deleted file mode 100644 index f0c61249..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_front_unit.launch.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_rear_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_rear_unit.launch.xml deleted file mode 100644 index 0892c331..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_rear_unit.launch.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_top_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_top_unit.launch.xml deleted file mode 100644 index 67724d2f..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_top_unit.launch.xml +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/manual.launch.xml deleted file mode 100644 index a69f80f7..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual.launch.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_front_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_front_unit.launch.xml deleted file mode 100644 index 220fe521..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_front_unit.launch.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_rear_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_rear_unit.launch.xml deleted file mode 100644 index 12d1da9e..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_rear_unit.launch.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_sensors.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_sensors.launch.xml deleted file mode 100644 index 54694ac5..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_sensors.launch.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_top_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_top_unit.launch.xml deleted file mode 100644 index adf8d6ab..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/manual_top_unit.launch.xml +++ /dev/null @@ -1,117 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/mapping_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/mapping_based.launch.xml deleted file mode 100644 index 78484dab..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/mapping_based.launch.xml +++ /dev/null @@ -1,248 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based.launch.xml deleted file mode 100644 index 231587f5..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based.launch.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_front_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_front_unit.launch.xml deleted file mode 100644 index 05d0057d..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_front_unit.launch.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_rear_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_rear_unit.launch.xml deleted file mode 100644 index 9647c3e8..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_rear_unit.launch.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based.launch.xml deleted file mode 100644 index 1bd28815..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based.launch.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_all.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_all.launch.xml deleted file mode 100644 index 9ae0b989..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_all.launch.xml +++ /dev/null @@ -1,690 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_base.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_base.launch.xml deleted file mode 100644 index 6cc2c596..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_base.launch.xml +++ /dev/null @@ -1,155 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_front_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_front_unit.launch.xml deleted file mode 100644 index b699f729..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_front_unit.launch.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_lidars.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_lidars.launch.xml deleted file mode 100644 index 9082a727..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_lidars.launch.xml +++ /dev/null @@ -1,270 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_rear_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_rear_unit.launch.xml deleted file mode 100644 index 6c4b5970..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_rear_unit.launch.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_top_unit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_top_unit.launch.xml deleted file mode 100644 index fc7fe7cb..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_top_unit.launch.xml +++ /dev/null @@ -1,112 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane.launch.xml deleted file mode 100644 index c750f831..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensors.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensors.launch.xml deleted file mode 100644 index 2d37f3e2..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensors.launch.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive.launch.xml deleted file mode 100644 index 98b2fa81..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive_sensor_kit.launch.xml deleted file mode 100644 index a57b73d8..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive_sensor_kit.launch.xml +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual.launch.xml deleted file mode 100644 index af2b2446..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensor_kit.launch.xml deleted file mode 100644 index 2658e33d..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensor_kit.launch.xml +++ /dev/null @@ -1,111 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensors.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensors.launch.xml deleted file mode 100644 index ce000f15..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensors.launch.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml deleted file mode 100644 index b7a0959f..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml deleted file mode 100644 index de138d59..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml deleted file mode 100644 index b0181fca..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/mapping_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/mapping_based.launch.xml deleted file mode 100644 index 489bc6e6..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/mapping_based.launch.xml +++ /dev/null @@ -1,180 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/reflector_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/reflector_based.launch.xml deleted file mode 100644 index 01bedd63..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/reflector_based.launch.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based.launch.xml deleted file mode 100644 index ddf8236d..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_all.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_all.launch.xml deleted file mode 100644 index 14c1bad3..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_all.launch.xml +++ /dev/null @@ -1,579 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_base.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_base.launch.xml deleted file mode 100644 index ca75724a..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_base.launch.xml +++ /dev/null @@ -1,154 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_lidars.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_lidars.launch.xml deleted file mode 100644 index fdffdcce..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_lidars.launch.xml +++ /dev/null @@ -1,250 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_sensor_kit.launch.xml deleted file mode 100644 index 10ba3cbb..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_sensor_kit.launch.xml +++ /dev/null @@ -1,113 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml b/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml deleted file mode 100644 index 5aa9bf92..00000000 --- a/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml +++ /dev/null @@ -1,147 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/launch/camera_republisher.launch.xml b/sensor/extrinsic_calibration_manager/launch/camera_republisher.launch.xml deleted file mode 100644 index 5f03abdc..00000000 --- a/sensor/extrinsic_calibration_manager/launch/camera_republisher.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/sensor/extrinsic_calibration_manager/package.xml b/sensor/extrinsic_calibration_manager/package.xml deleted file mode 100644 index 80ddc959..00000000 --- a/sensor/extrinsic_calibration_manager/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - extrinsic_calibration_manager - 0.1.0 - The extrinsic_calibration_manager package - Yohei Mishina - Apache License 2.0 - - ament_cmake_auto - - autoware_cmake - - rclcpp - tf2 - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_calibration_msgs - yaml_cpp_vendor - - autoware_launch - extrinsic_calibration_client - extrinsic_dummy_calibrator - extrinsic_ground_plane_calibrator - extrinsic_interactive_calibrator - extrinsic_manual_calibrator - extrinsic_tag_based_calibrator - image_proc - individual_params - tunable_static_tf_broadcaster - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp b/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp deleted file mode 100644 index 9d556a6e..00000000 --- a/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp +++ /dev/null @@ -1,221 +0,0 @@ -// Copyright 2023 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. - -#include -#include - -#include -#include -#include -#include -#include - -ExtrinsicCalibrationManagerNode::ExtrinsicCalibrationManagerNode( - const rclcpp::NodeOptions & node_options) -: Node("extrinsic_calibration_manager_node", node_options) -{ - server_ = this->create_service( - "extrinsic_calibration_manager", std::bind( - &ExtrinsicCalibrationManagerNode::calibrationRequestCallback, - this, std::placeholders::_1, std::placeholders::_2)); - - callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - parent_frame_ = this->declare_parameter("parent_frame", ""); - child_frames_ = this->declare_parameter("child_frames", std::vector()); - client_ns_ = this->declare_parameter("client_ns", "extrinsic_calibration"); - threshold_ = this->declare_parameter("fitness_score_threshold", 10.0); -} - -void ExtrinsicCalibrationManagerNode::calibrationRequestCallback( - const std::shared_ptr request, - const std::shared_ptr - response) -{ - using std::chrono_literals::operator""s; - - // open yaml file - auto yaml_node = YAML::LoadFile(request->src_path); - if (yaml_node.IsNull()) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Reading yaml file failed: " << request->src_path); - return; - } - - // create clients - for (const auto & child_frame : child_frames_) { - TargetClient target_client; - if (!createTargetClient( - yaml_node, parent_frame_, child_frame, client_ns_, callback_group_, target_client)) { - rclcpp::shutdown(); - } - target_clients_.push_back(target_client); - } - - // wait for client services - for (auto & target_client : target_clients_) { - while (!target_client.client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for service."); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for service: " << target_client.child_frame); - } - } - - // call client services - for (auto & target_client : target_clients_) { - auto req = std::make_shared(); - *req = target_client.request; - - auto cb = [&](rclcpp::Client::SharedFuture - response_client) { - auto res = response_client.get(); - target_client.response = *res; - target_client.estimated = true; - RCLCPP_INFO_STREAM( - this->get_logger(), "Received service message: " << target_client.child_frame - << "(success = " << res->success - << " score = " << res->score << ")"); - }; - - RCLCPP_INFO_STREAM(this->get_logger(), "Call service: " << target_client.child_frame); - target_client.client->async_send_request(req, cb); - } - - // wait for responses - while (rclcpp::ok()) { - bool done = std::all_of(target_clients_.begin(), target_clients_.end(), [](auto target_client) { - return target_client.estimated; - }); - if (done) { - break; - } - rclcpp::sleep_for(5s); - RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for responses..."); - } - - // dump yaml file - dumpCalibrationConfig(request->dst_path, target_clients_); - - bool success = true; - double max_score = -std::numeric_limits::max(); - for (auto & target_client : target_clients_) { - if (!target_client.response.success) { - success = false; - } - if (target_client.response.score > max_score) { - max_score = target_client.response.score; - } - } - - if (max_score > threshold_) { - success = false; - } - response->success = success; - response->score = max_score; -} - -bool ExtrinsicCalibrationManagerNode::createTargetClient( - const YAML::Node & yaml_node, const std::string & parent_frame, const std::string & child_frame, - const std::string & client_ns, const rclcpp::CallbackGroup::SharedPtr & callback_group, - TargetClient & target_client) -{ - target_client.parent_frame = parent_frame; - target_client.child_frame = child_frame; - - target_client.client = this->create_client( - target_client.parent_frame + "/" + target_client.child_frame + "/" + client_ns, - rmw_qos_profile_default, callback_group); - - target_client.estimated = false; - - try { - target_client.request.initial_pose = getPoseFromYaml(yaml_node, parent_frame, child_frame); - } catch (const std::runtime_error & exception) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Loading parameter from yaml failed: " << exception.what()); - return false; - } - - return true; -} - -geometry_msgs::msg::Pose ExtrinsicCalibrationManagerNode::getPoseFromYaml( - const YAML::Node & yaml_node, const std::string & parent_frame, const std::string & child_frame) -{ - tf2::Vector3 pos( - yaml_node[parent_frame][child_frame]["x"].as(), - yaml_node[parent_frame][child_frame]["y"].as(), - yaml_node[parent_frame][child_frame]["z"].as()); - - tf2::Quaternion quat; - tf2::fromMsg( - tier4_autoware_utils::createQuaternionFromRPY( - yaml_node[parent_frame][child_frame]["roll"].as(), - yaml_node[parent_frame][child_frame]["pitch"].as(), - yaml_node[parent_frame][child_frame]["yaw"].as()), - quat); - return tier4_autoware_utils::transform2pose(toMsg(tf2::Transform(quat, pos))); -} - -bool ExtrinsicCalibrationManagerNode::dumpCalibrationConfig( - const std::string & path, const std::vector & target_clients) -{ - std::ofstream file_out(path.c_str()); - if (file_out.fail()) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Open yaml file failed: " << path); - return false; - } - - YAML::Emitter out; - out << YAML::BeginMap; - out << YAML::Key << parent_frame_ << YAML::Value << YAML::BeginMap; - for (const auto & target_client : target_clients) { - const auto xyz = target_client.response.result_pose.position; - const auto rpy = tier4_autoware_utils::getRPY(target_client.response.result_pose.orientation); - - std::ostringstream xyz_x, xyz_y, xyz_z, rpy_x, rpy_y, rpy_z; - - xyz_x << std::setw(yaml_precision_) << std::fixed << xyz.x; - xyz_y << std::setw(yaml_precision_) << std::fixed << xyz.y; - xyz_z << std::setw(yaml_precision_) << std::fixed << xyz.z; - rpy_x << std::setw(yaml_precision_) << std::fixed << rpy.x; - rpy_y << std::setw(yaml_precision_) << std::fixed << rpy.y; - rpy_z << std::setw(yaml_precision_) << std::fixed << rpy.z; - - out << YAML::Key << target_client.child_frame << YAML::Value << YAML::BeginMap << YAML::Key - << "x" << YAML::Value << xyz_x.str() << YAML::Key << "y" << YAML::Value << xyz_y.str() - << YAML::Key << "z" << YAML::Value << xyz_z.str() << YAML::Key << "roll" << YAML::Value - << rpy_x.str() << YAML::Key << "pitch" << YAML::Value << rpy_y.str() << YAML::Key << "yaw" - << YAML::Value << rpy_z.str() << YAML::EndMap; - } - - out << YAML::EndMap << YAML::EndMap; - file_out << out.c_str(); - file_out.close(); - return true; -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(node_options); - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/sensor/extrinsic_dummy_calibrator/CMakeLists.txt b/sensor/extrinsic_dummy_calibrator/CMakeLists.txt deleted file mode 100644 index e3ea3be3..00000000 --- a/sensor/extrinsic_dummy_calibrator/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(extrinsic_dummy_calibrator) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_executable(extrinsic_dummy_calibrator - src/extrinsic_dummy_calibrator.cpp) -ament_target_dependencies(extrinsic_dummy_calibrator) - -#if(BUILD_TESTING) -# find_package(ament_lint_auto REQUIRED) -# ament_lint_auto_find_test_dependencies() -#endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/sensor/extrinsic_dummy_calibrator/include/extrinsic_dummy_calibrator/extrinsic_dummy_calibrator.hpp b/sensor/extrinsic_dummy_calibrator/include/extrinsic_dummy_calibrator/extrinsic_dummy_calibrator.hpp deleted file mode 100644 index 11a2e457..00000000 --- a/sensor/extrinsic_dummy_calibrator/include/extrinsic_dummy_calibrator/extrinsic_dummy_calibrator.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2023 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. - -#ifndef EXTRINSIC_DUMMY_CALIBRATOR__EXTRINSIC_DUMMY_CALIBRATOR_HPP_ -#define EXTRINSIC_DUMMY_CALIBRATOR__EXTRINSIC_DUMMY_CALIBRATOR_HPP_ - -#include "pcl/PCLPointCloud2.h" -#include "pcl/filters/extract_indices.h" -#include "pcl/filters/voxel_grid.h" -#include "pcl/io/pcd_io.h" -#include "pcl/point_types.h" -#include "pcl/registration/gicp.h" -#include "pcl/segmentation/sac_segmentation.h" -#include "pcl_conversions/pcl_conversions.h" -#include "pcl_ros/transforms.hpp" -#include "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tier4_calibration_msgs/srv/extrinsic_calibrator.hpp" - -#include -#include -#include -#include -#include - -namespace extrinsic_dummy_calibrator -{ - -class ExtrinsicDummyCalibrator : public rclcpp::Node -{ -private: - rclcpp::Service::SharedPtr server_; - - std::mutex mutex_; - std::string parent_frame_; - std::string child_frame_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - -public: - explicit ExtrinsicDummyCalibrator(const rclcpp::NodeOptions & node_options); - void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); -}; - -} // namespace extrinsic_dummy_calibrator -#endif // EXTRINSIC_DUMMY_CALIBRATOR__EXTRINSIC_DUMMY_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_dummy_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_dummy_calibrator/launch/calibrator.launch.xml deleted file mode 100644 index e083e60f..00000000 --- a/sensor/extrinsic_dummy_calibrator/launch/calibrator.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_dummy_calibrator/src/extrinsic_dummy_calibrator.cpp b/sensor/extrinsic_dummy_calibrator/src/extrinsic_dummy_calibrator.cpp deleted file mode 100644 index 725fa262..00000000 --- a/sensor/extrinsic_dummy_calibrator/src/extrinsic_dummy_calibrator.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2023 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. - -#include "extrinsic_dummy_calibrator/extrinsic_dummy_calibrator.hpp" - -#include -#include - -namespace extrinsic_dummy_calibrator -{ -ExtrinsicDummyCalibrator::ExtrinsicDummyCalibrator(const rclcpp::NodeOptions & node_options) -: Node("extrinsic_dummy_calibrator", node_options) -{ - // set launch param - parent_frame_ = this->declare_parameter("parent_frame", ""); - child_frame_ = this->declare_parameter("child_frame", ""); - - callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscription_option = rclcpp::SubscriptionOptions(); - subscription_option.callback_group = callback_group_; - - server_ = this->create_service( - "extrinsic_calibration", std::bind( - &ExtrinsicDummyCalibrator::requestReceivedCallback, this, - std::placeholders::_1, std::placeholders::_2)); -} - -void ExtrinsicDummyCalibrator::requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response) -{ - response->success = true; - response->result_pose = request->initial_pose; - response->score = 1.0; -} - -} // namespace extrinsic_dummy_calibrator - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(node_options); - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/sensor/extrinsic_ground_plane_calibrator/rviz/velodyne_top.rviz b/sensor/extrinsic_ground_plane_calibrator/rviz/velodyne_top.rviz deleted file mode 100644 index 6299e86e..00000000 --- a/sensor/extrinsic_ground_plane_calibrator/rviz/velodyne_top.rviz +++ /dev/null @@ -1,351 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /top1/Topic1 - - /initial_base_link1 - - /top_inliers1/Topic1 - Splitter Ratio: 0.500627338886261 - Tree Height: 725 - - 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 - - /Current View1/Position1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: top -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: 40 - 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: 143 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: top - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: base_link - Radius: 0.10000000149011612 - Reference Frame: base_link - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1 - Name: initial_base_link - Radius: 0.10000000149011612 - Reference Frame: initial_base_link - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: velodyne_top - Radius: 0.10000000149011612 - Reference Frame: velodyne_top - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1.5 - Name: ground_plane - Radius: 0.05000000074505806 - Reference Frame: ground_plane - Value: false - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1.5 - Name: ground_plane_raw - Radius: 0.05000000074505806 - Reference Frame: ground_plane_raw - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/sensor_kit_base_link/markers - Value: false - - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: top_inliers - Position Transformer: XYZ - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/velodyne_top_base_link/inliers - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: left_inliers - Position Transformer: XYZ - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/inliers - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: center_inliers - Position Transformer: XYZ - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/inliers - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: right_inliers - Position Transformer: XYZ - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/inliers - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0 - Position: - X: 2.4723308086395264 - Y: -0.0926784947514534 - Z: 0 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.240023612976074 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000023b0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000003e20000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp b/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp deleted file mode 100644 index 118f4eef..00000000 --- a/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp +++ /dev/null @@ -1,830 +0,0 @@ -// Copyright 2023 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. - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include - -ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::NodeOptions & options) -: Node("extrinsic_ground_plane_calibrator_node", options), - tf_broadcaster_(this), - got_initial_transform_(false), - calibration_done_(false), - first_observation_(true) -{ - tf_buffer_ = std::make_shared(this->get_clock()); - transform_listener_ = std::make_shared(*tf_buffer_); - - base_frame_ = this->declare_parameter("base_frame", "base_link"); - parent_frame_ = this->declare_parameter("parent_frame"); - child_frame_ = this->declare_parameter("child_frame"); - - marker_size_ = this->declare_parameter("marker_size", 20.0); - - use_crop_box_filter_ = this->declare_parameter("use_crop_box_filter", true); - crop_box_min_x_ = this->declare_parameter("crop_box_min_x", -50.0); - crop_box_min_y_ = this->declare_parameter("crop_box_min_y", -50.0); - crop_box_min_z_ = this->declare_parameter("crop_box_min_z", -50.0); - crop_box_max_x_ = this->declare_parameter("crop_box_max_x", 50.0); - crop_box_max_y_ = this->declare_parameter("crop_box_max_y", 50.0); - crop_box_max_z_ = this->declare_parameter("crop_box_max_z", 50.0); - - remove_outliers_ = this->declare_parameter("remove_outliers", true); - remove_outlier_tolerance_ = this->declare_parameter("remove_outlier_tolerance", 0.1); - use_pca_rough_normal_ = this->declare_parameter("use_pca_rough_normal", true); - max_inlier_distance_ = this->declare_parameter("max_inlier_distance", 0.01); - min_plane_points_ = this->declare_parameter("min_plane_points", 500); - min_plane_points_percentage_ = - this->declare_parameter("min_plane_points_percentage", 10.0); - max_cos_distance_ = this->declare_parameter("max_cos_distance", 0.2); - max_iterations_ = this->declare_parameter("max_iterations", 500); - verbose_ = this->declare_parameter("verbose", false); - broadcast_calibration_tf_ = this->declare_parameter("broadcast_calibration_tf", false); - filter_estimations_ = this->declare_parameter("filter_estimations", true); - int ring_buffer_size = this->declare_parameter("ring_buffer_size", 100); - - inlier_observations_.setMaxSize(ring_buffer_size); - - initial_angle_cov_ = this->declare_parameter("initial_angle_cov", 5.0); - initial_translation_cov_ = this->declare_parameter("initial_translation_cov", 0.05); - - angle_measurement_cov_ = this->declare_parameter("angle_measurement_cov", 0.5); - angle_process_cov_ = this->declare_parameter("angle_process_cov", 0.1); - translation_measurement_cov_ = - this->declare_parameter("translation_measurement_cov", 0.005); - translation_process_cov_ = this->declare_parameter("translation_process_cov", 0.001); - - angle_convergence_threshold_ = this->declare_parameter("angle_convergence_threshold", 0.0); - translation_convergence_threshold_ = - this->declare_parameter("translation_convergence_threshold", 0.0); - - initial_angle_cov_ = std::pow(initial_angle_cov_ * M_PI_2 / 180.0, 2); - initial_translation_cov_ = std::pow(initial_translation_cov_, 2); - - angle_measurement_cov_ = std::pow(angle_measurement_cov_ * M_PI_2 / 180.0, 2); - angle_process_cov_ = std::pow(angle_process_cov_ * M_PI_2 / 180.0, 2); - translation_measurement_cov_ = std::pow(translation_measurement_cov_, 2); - translation_process_cov_ = std::pow(translation_process_cov_, 2); - - angle_convergence_threshold_ = std::pow(angle_convergence_threshold_ * M_PI_2 / 180.0, 2); - translation_convergence_threshold_ = std::pow(translation_convergence_threshold_, 2); - - markers_pub_ = this->create_publisher("markers", 10); - inliers_pub_ = this->create_publisher("inliers", 10); - - pointcloud_sub_ = this->create_subscription( - "input_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&ExtrinsicGroundPlaneCalibrator::pointCloudCallback, this, std::placeholders::_1)); - - // The service server runs in a dedicated thread - srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - service_server_ = this->create_service( - "extrinsic_calibration", - std::bind( - &ExtrinsicGroundPlaneCalibrator::requestReceivedCallback, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, srv_callback_group_); - - // Initialize the filter - kalman_filter_.setA(Eigen::DiagonalMatrix(1.0, 1.0, 1.0, 1.0, 1.0, 1.0)); - kalman_filter_.setB(Eigen::DiagonalMatrix(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - kalman_filter_.setC(Eigen::DiagonalMatrix(1.0, 1.0, 1.0, 1.0, 1.0, 1.0)); - kalman_filter_.setQ(Eigen::DiagonalMatrix( - angle_measurement_cov_, angle_measurement_cov_, angle_measurement_cov_, - translation_measurement_cov_, translation_measurement_cov_, translation_measurement_cov_)); - kalman_filter_.setR(Eigen::DiagonalMatrix( - angle_process_cov_, angle_process_cov_, angle_process_cov_, translation_process_cov_, - translation_process_cov_, translation_process_cov_)); -} - -void ExtrinsicGroundPlaneCalibrator::requestReceivedCallback( - __attribute__((unused)) - const std::shared_ptr - request, - const std::shared_ptr response) -{ - // This tool uses several tfs, so for consistency we take the initial calibration using lookups - using std::chrono_literals::operator""s; - - // Loop until the calibration finishes - while (rclcpp::ok()) { - rclcpp::sleep_for(10s); - std::unique_lock lock(mutex_); - - if (calibration_done_) { - break; - } - - RCLCPP_WARN_SKIPFIRST(this->get_logger(), "Waiting for the calibration to end"); - } - - response->success = true; - response->result_pose = output_parent_to_child_msg_; - - RCLCPP_INFO(this->get_logger(), "Calibration result sent"); - - pointcloud_sub_.reset(); -} - -void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( - const sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - lidar_frame_ = msg->header.frame_id; - header_ = msg->header; - - // Make sure we have all the required initial tfs - if (!checkInitialTransforms() || calibration_done_) { - return; - } - - // Convert the pointcloud to PCL - pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); - pcl::PointCloud::Ptr inliers_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *pointcloud); - - // Filter the pointcloud using previous outlier models - if (remove_outliers_) { - std::size_t original_num_points = pointcloud->size(); - for (const auto & outlier_model : outlier_models_) { - pointcloud = removeOutliers(pointcloud, outlier_model, remove_outlier_tolerance_); - } - - RCLCPP_INFO( - this->get_logger(), "Outlier plane removal: %lu -> %lu points", original_num_points, - pointcloud->size()); - } - - // Extract the ground plane model - Eigen::Vector4d ground_plane_model; - if (!extractGroundPlane(pointcloud, ground_plane_model, inliers_pointcloud)) { - return; - } - - // Obtain the model error for the initial and current calibration - evaluateModels(ground_plane_model, inliers_pointcloud); - - // Publish the inliers - sensor_msgs::msg::PointCloud2 inliers_msg; - pcl::toROSMsg(*inliers_pointcloud, inliers_msg); - inliers_msg.header = header_; - inliers_pub_->publish(inliers_msg); - // Create markers to visualize the calibration - visualizeCalibration(ground_plane_model); - - filterCalibration(ground_plane_model, inliers_pointcloud); - - // Obtain the final output tf and publish the lidar -> ground tfs to evaluate the calibration - publishTf(ground_plane_model); -} - -bool ExtrinsicGroundPlaneCalibrator::checkInitialTransforms() -{ - if (lidar_frame_ == "") { - return false; - } - - if (got_initial_transform_) { - return true; - } - - try { - rclcpp::Time t = rclcpp::Time(0); - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - - initial_base_to_lidar_msg_ = - tf_buffer_->lookupTransform(base_frame_, lidar_frame_, t, timeout).transform; - - fromMsg(initial_base_to_lidar_msg_, initial_base_to_lidar_tf2_); - initial_base_to_lidar_eigen_ = tf2::transformToEigen(initial_base_to_lidar_msg_); - - base_to_parent_msg_ = - tf_buffer_->lookupTransform(base_frame_, parent_frame_, t, timeout).transform; - - fromMsg(base_to_parent_msg_, base_to_parent_tf2_); - base_to_parent_eigen_ = tf2::transformToEigen(base_to_parent_msg_); - - child_to_lidar_msg_ = - tf_buffer_->lookupTransform(child_frame_, lidar_frame_, t, timeout).transform; - - fromMsg(child_to_lidar_msg_, child_to_lidar_tf2_); - child_to_lidar_eigen_ = tf2::transformToEigen(child_to_lidar_msg_); - - got_initial_transform_ = true; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "could not get initial tf. %s", ex.what()); - return false; - } - - return true; -} - -bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( - pcl::PointCloud::Ptr & pointcloud, Eigen::Vector4d & model, - pcl::PointCloud::Ptr & inliers_pointcloud) -{ - if (use_crop_box_filter_) { - pcl::CropBox boxFilter; - boxFilter.setMin(Eigen::Vector4f(crop_box_min_x_, crop_box_min_y_, crop_box_min_z_, 1.0)); - boxFilter.setMax(Eigen::Vector4f(crop_box_max_x_, crop_box_max_y_, crop_box_max_z_, 1.0)); - boxFilter.setInputCloud(pointcloud); - boxFilter.filter(*pointcloud); - } - - std::vector models; - Eigen::Vector3f rough_normal; - - if (use_pca_rough_normal_) { - // Obtain an idea of the ground plane using PCA - // under the assumption that the axis with less variance will be the ground plane normal - pcl::PCA pca; - pca.setInputCloud(pointcloud); - Eigen::MatrixXf vectors = pca.getEigenVectors(); - rough_normal = vectors.col(2); - } else { - rough_normal = - (initial_base_to_lidar_eigen_.inverse().rotation() * Eigen::Vector3d(0.0, 0.0, 1.0)) - .cast(); - } - - if (verbose_) { - RCLCPP_INFO( - this->get_logger(), "Rough plane normal. x=%.3f, y=%.3f, z=%.3f", rough_normal.x(), - rough_normal.y(), rough_normal.z()); - } - - // Use RANSAC iteratively until we find the ground plane - // Since walls can have more points, we filter using the PCA-based hypothesis - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - pcl::SACSegmentation seg; - pcl::ExtractIndices extract; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(max_inlier_distance_); - seg.setMaxIterations(max_iterations_); - - pcl::PointCloud::Ptr iteration_cloud = pointcloud; - int iteration_size = iteration_cloud->height * iteration_cloud->width; - - while (iteration_size > min_plane_points_) { - seg.setInputCloud(iteration_cloud); - seg.segment(*inliers, *coefficients); - - if (inliers->indices.size() == 0) { - if (verbose_) { - RCLCPP_WARN(this->get_logger(), "No plane found in the pointcloud"); - } - - break; - } - - Eigen::Vector3f normal( - coefficients->values[0], coefficients->values[1], coefficients->values[2]); - float cos_distance = 1.0 - std::abs(rough_normal.dot(normal)); - - model = Eigen::Vector4d( - coefficients->values[0], coefficients->values[1], coefficients->values[2], - coefficients->values[3]); - - int inlier_size = static_cast(inliers->indices.size()); - double inlier_percentage = 100.0 * inlier_size / pointcloud->size(); - - if ( - inlier_size > min_plane_points_ && inlier_percentage > min_plane_points_percentage_ && - cos_distance < max_cos_distance_) { - if (verbose_) { - RCLCPP_INFO( - this->get_logger(), "Plane found: inliers=%ld (%.3f)", inliers->indices.size(), - inlier_percentage); - RCLCPP_INFO( - this->get_logger(), "Plane model. a=%.3f, b=%.3f, c=%.3f, d=%.3f", model(0), model(1), - model(2), model(3)); - RCLCPP_INFO( - this->get_logger(), "Cos distance: %.3f / %.3f", cos_distance, max_cos_distance_); - } - - // Extract the ground plane inliers - extract.setInputCloud(iteration_cloud); - extract.setIndices(inliers); - extract.setNegative(false); - extract.filter(*inliers_pointcloud); - return true; - } else { - if (remove_outliers_) { - bool accept = true; - - for (const auto & outlier_model : outlier_models_) { - Eigen::Vector3f outlier_normal(outlier_model.x(), outlier_model.y(), outlier_model.z()); - float cos_distance = 1.0 - std::abs(outlier_normal.dot(normal)); - - if ( - cos_distance < max_cos_distance_ && - std::abs(outlier_model.w() - model.w()) < remove_outlier_tolerance_) { - accept = false; - } - } - - if (accept) { - outlier_models_.push_back(model); - RCLCPP_INFO( - this->get_logger(), "New outlier model: a=%.3f, b=%.3f, c=%.3f, d=%.3f", model(0), - model(1), model(2), model(3)); - } - } - - if (verbose_) { - RCLCPP_INFO( - this->get_logger(), - "Iteration failed. model: a=%.3f, b=%.3f, c=%.3f, d=%.3f inliers=%lu inlier " - "percentage=%.2f cos_distance=%.2f", - model(0), model(1), model(2), model(3), inliers->indices.size(), inlier_percentage, - cos_distance); - } - } - - // Extract the inliers from the pointcloud (the detected plane was not the ground plane) - extract.setInputCloud(iteration_cloud); - extract.setIndices(inliers); - extract.setNegative(true); - - pcl::PointCloud next_cloud; - extract.filter(next_cloud); - - iteration_cloud->swap(next_cloud); - iteration_size = iteration_cloud->height * iteration_cloud->width; - } - return false; -} - -void ExtrinsicGroundPlaneCalibrator::evaluateModels( - const Eigen::Vector4d & estimated_model, pcl::PointCloud::Ptr inliers) const -{ - auto modelError = - [](float a, float b, float c, float d, pcl::PointCloud::Ptr pc) -> float { - assert(std::abs(a * a + b * b + c * c - 1.f) < 1e-5); - float sum = 0.f; - for (auto & p : pc->points) { - sum += std::abs(a * p.x + b * p.y + c * p.z + d); - } - return sum / (pc->height * pc->width); - }; - - Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_eigen_.inverse(); - Eigen::Vector4d initial_model = poseToPlaneModel(initial_lidar_base_transform); - - float initial_model_error = - modelError(initial_model(0), initial_model(1), initial_model(2), initial_model(3), inliers); - - float estimated_model_error = modelError( - estimated_model(0), estimated_model(1), estimated_model(2), estimated_model(3), inliers); - - RCLCPP_INFO(this->get_logger(), "Initial calibration error: %3f m", initial_model_error); - RCLCPP_INFO(this->get_logger(), "Estimated calibration error: %3f m", estimated_model_error); -} - -void ExtrinsicGroundPlaneCalibrator::filterCalibration( - const Eigen::Vector4d & ground_plane_model, pcl::PointCloud::Ptr inliers) -{ - // Eigen::Isometry3d lidar_to_ground_pose = modelPlaneToPose(ground_plane_model); - // Eigen::Isometry3d ground_to_lidar_pose = lidar_to_ground_pose.inverse(); - - Eigen::Isometry3d estimated_base_to_lidar_pose = - refineBaseLidarPose(initial_base_to_lidar_eigen_, ground_plane_model); - - Eigen::Isometry3d estimated_parent_to_child_eigen = base_to_parent_eigen_.inverse() * - estimated_base_to_lidar_pose * - child_to_lidar_eigen_.inverse(); - - Eigen::Isometry3d initial_parent_to_child_eigen = base_to_parent_eigen_.inverse() * - initial_base_to_lidar_eigen_ * - child_to_lidar_eigen_.inverse(); - - Eigen::Vector3d estimated_translation; - auto estimated_rpy = - tier4_autoware_utils::getRPY(tf2::toMsg(estimated_parent_to_child_eigen).orientation); - auto initial_rpy = - tier4_autoware_utils::getRPY(tf2::toMsg(initial_parent_to_child_eigen).orientation); - - if (verbose_) { - RCLCPP_INFO( - this->get_logger(), "Initial parent->child euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", - initial_rpy.x, initial_rpy.y, initial_rpy.z); - RCLCPP_INFO( - this->get_logger(), "Estimated parent->child euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", - estimated_rpy.x, estimated_rpy.y, estimated_rpy.z); - - RCLCPP_INFO( - this->get_logger(), "Initial parent->child translation: x=%.3f, y=%.3f, z=%.3f", - initial_parent_to_child_eigen.translation().x(), - initial_parent_to_child_eigen.translation().y(), - initial_parent_to_child_eigen.translation().z()); - RCLCPP_INFO( - this->get_logger(), "Estimated parent->child translation: x=%.3f, y=%.3f, z=%.3f", - estimated_parent_to_child_eigen.translation().x(), - estimated_parent_to_child_eigen.translation().y(), - estimated_parent_to_child_eigen.translation().z()); - } - - // Optional filtering - if (filter_estimations_) { - Eigen::Vector x( - estimated_rpy.x, estimated_rpy.y, estimated_rpy.z, - estimated_parent_to_child_eigen.translation().x(), - estimated_parent_to_child_eigen.translation().y(), - estimated_parent_to_child_eigen.translation().z()); - Eigen::DiagonalMatrix p0( - initial_angle_cov_, initial_angle_cov_, initial_angle_cov_, initial_translation_cov_, - initial_translation_cov_, initial_translation_cov_); - - if (first_observation_) { - kalman_filter_.init(x, p0); - first_observation_ = false; - } else { - kalman_filter_.update(x); - } - - estimated_rpy.x = kalman_filter_.getXelement(0); - estimated_rpy.y = kalman_filter_.getXelement(1); - estimated_rpy.z = kalman_filter_.getXelement(2); - estimated_translation.x() = kalman_filter_.getXelement(3); - estimated_translation.y() = kalman_filter_.getXelement(4); - estimated_translation.z() = kalman_filter_.getXelement(5); - } - - // By detecting the ground plane and fabricating a pose arbitrarily, the x, y, and yaw do not hold - // real meaning, so we instead just use the ones from the initial calibration - geometry_msgs::msg::Quaternion estimated_orientation_msg = - tier4_autoware_utils::createQuaternionFromRPY(estimated_rpy.x, estimated_rpy.y, initial_rpy.z); - Eigen::Quaterniond estimated_orientation_eigen; - - tf2::fromMsg(estimated_orientation_msg, estimated_orientation_eigen); - - output_parent_to_child_eigen_.linear() = estimated_orientation_eigen.toRotationMatrix(); - output_parent_to_child_eigen_.translation() = estimated_translation; - - // We perform basic filtering on the estimated angles - { - std::unique_lock lock(mutex_); - output_parent_to_child_msg_ = tf2::toMsg(output_parent_to_child_eigen_); - - if (filter_estimations_) { - Eigen::MatrixXd p; - kalman_filter_.getP(p); - Eigen::VectorXd diag = p.diagonal(); - std::array thresholds{ - angle_convergence_threshold_, angle_convergence_threshold_, - angle_convergence_threshold_, translation_convergence_threshold_, - translation_convergence_threshold_, translation_convergence_threshold_}; - inlier_observations_.add(inliers); - - bool converged = true; - for (std::size_t index = 0; index < thresholds.size(); index++) { - converged &= diag(index) < thresholds[index]; - } - - RCLCPP_INFO( - this->get_logger(), "Filter cov: roll=%.2e, pitch=%.2e yaw=%.2e, x=%.2e, y=%.2e, z=%.2e", - diag(0), diag(1), diag(2), diag(3), diag(4), diag(5)); - - RCLCPP_INFO( - this->get_logger(), "Convergence thresh: angle=%.2e, translation=%.2e", - angle_convergence_threshold_, translation_convergence_threshold_); - - if (!converged) { - return; - } - - pcl::PointCloud::Ptr augmented_inliers(new pcl::PointCloud); - - for (const auto & inliers : inlier_observations_.get()) { - *augmented_inliers += *inliers; - } - - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr final_inliers(new pcl::PointIndices); - pcl::SACSegmentation seg; - pcl::ExtractIndices extract; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(10 * max_inlier_distance_); - seg.setMaxIterations(max_iterations_); - - seg.setInputCloud(augmented_inliers); - seg.segment(*final_inliers, *coefficients); - Eigen::Vector4d output_model( - coefficients->values[0], coefficients->values[1], coefficients->values[2], - coefficients->values[3]); - - RCLCPP_INFO( - this->get_logger(), - "Final model: a=%.3f, b=%.3f, c=%.3f, d=%.3f final inliers=%lu total.percentage=%.2f", - output_model(0), output_model(1), output_model(2), output_model(3), - final_inliers->indices.size(), - 100.f * final_inliers->indices.size() / augmented_inliers->size()); - - Eigen::Isometry3d output_base_to_lidar_pose = - refineBaseLidarPose(initial_base_to_lidar_eigen_, output_model); - - output_parent_to_child_eigen_ = base_to_parent_eigen_.inverse() * output_base_to_lidar_pose * - child_to_lidar_eigen_.inverse(); - output_parent_to_child_msg_ = tf2::toMsg(output_parent_to_child_eigen_); - - calibration_done_ = true; - } else { - calibration_done_ = true; - } - } -} - -void ExtrinsicGroundPlaneCalibrator::visualizeCalibration( - const Eigen::Vector4d & estimated_ground_model) -{ - visualization_msgs::msg::MarkerArray markers; - - Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_eigen_.inverse(); - - visualizePlaneModel("initial_calibration_pose", initial_lidar_base_transform, markers); - - Eigen::Vector4d fake_model = poseToPlaneModel(initial_lidar_base_transform); - - visualizePlaneModel("initial_calibration_model", fake_model, markers); - - visualizePlaneModel("estimated_model", estimated_ground_model, markers); - - markers_pub_->publish(markers); -} - -void ExtrinsicGroundPlaneCalibrator::visualizePlaneModel( - const std::string & name, Eigen::Vector4d model, visualization_msgs::msg::MarkerArray & markers) -{ - visualizePlaneModel(name, modelPlaneToPose(model), markers); -} - -void ExtrinsicGroundPlaneCalibrator::visualizePlaneModel( - const std::string & name, const Eigen::Isometry3d & lidar_base_pose, - visualization_msgs::msg::MarkerArray & markers) -{ - std::vector corners = { - Eigen::Vector3d(-marker_size_, -marker_size_, 0.0), - Eigen::Vector3d(-marker_size_, marker_size_, 0.0), - Eigen::Vector3d(marker_size_, marker_size_, 0.0), - Eigen::Vector3d(marker_size_, -marker_size_, 0.0)}; - - for (Eigen::Vector3d & corner : corners) { - corner = lidar_base_pose * corner; - } - - std::vector corners_msg; - - for (Eigen::Vector3d & corner : corners) { - geometry_msgs::msg::Point p; - p.x = corner.x(); - p.y = corner.y(); - p.z = corner.z(); - corners_msg.push_back(p); - } - - visualization_msgs::msg::Marker marker; - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.header.frame_id = lidar_frame_; - marker.header.stamp = header_.stamp; - marker.ns = name + "_plane"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.scale.x = 1.0; - marker.scale.y = 1.0; - marker.scale.z = 1.0; - marker.color.a = 0.5; - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - assert(corners_msg.size() == 4); - marker.points.push_back(corners_msg[0]); - marker.points.push_back(corners_msg[2]); - marker.points.push_back(corners_msg[1]); - marker.points.push_back(corners_msg[2]); - marker.points.push_back(corners_msg[0]); - marker.points.push_back(corners_msg[3]); - - markers.markers.push_back(marker); - - marker.ns = name + "_origin"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.01; - marker.scale.y = 0.01; - marker.scale.z = 0.0; - marker.color.a = 1.0; - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - - geometry_msgs::msg::Point p; - marker.points.clear(); - - p.x = 0; - p.y = 0; - p.z = 0; - marker.points.push_back(p); - - p.x = lidar_base_pose.translation().x(); - p.y = lidar_base_pose.translation().y(); - p.z = lidar_base_pose.translation().z(); - marker.points.push_back(p); - - markers.markers.push_back(marker); -} - -void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_plane_model) -{ - geometry_msgs::msg::TransformStamped initial_lidar_to_base_msg = - tf2::eigenToTransform(initial_base_to_lidar_eigen_.inverse()); - initial_lidar_to_base_msg.header.stamp = header_.stamp; - initial_lidar_to_base_msg.header.frame_id = lidar_frame_; - initial_lidar_to_base_msg.child_frame_id = "initial_base_link"; - tf_broadcaster_.sendTransform(initial_lidar_to_base_msg); - - Eigen::Isometry3d raw_lidar_to_base_eigen = modelPlaneToPose(ground_plane_model); - - // The ground_plane_raw tf is only assures us that it lies on the ground plane, but its yaw is - // arbitrary, and the position in the plane is obtained by projecting the lidar origin in the - // plane - geometry_msgs::msg::TransformStamped raw_lidar_to_base_msg = - tf2::eigenToTransform(raw_lidar_to_base_eigen); - - raw_lidar_to_base_msg.header.stamp = header_.stamp; - raw_lidar_to_base_msg.header.frame_id = lidar_frame_; - raw_lidar_to_base_msg.child_frame_id = lidar_frame_ + "_ground_plane_raw"; - tf_broadcaster_.sendTransform(raw_lidar_to_base_msg); - - if (broadcast_calibration_tf_) { - geometry_msgs::msg::TransformStamped output_tf_msg; - output_tf_msg.transform.rotation = output_parent_to_child_msg_.orientation; - output_tf_msg.transform.translation.x = output_parent_to_child_msg_.position.x; - output_tf_msg.transform.translation.y = output_parent_to_child_msg_.position.y; - output_tf_msg.transform.translation.z = output_parent_to_child_msg_.position.z; - output_tf_msg.header.stamp = header_.stamp; - output_tf_msg.header.frame_id = parent_frame_; - output_tf_msg.child_frame_id = child_frame_; - tf_broadcaster_.sendTransform(output_tf_msg); - } - - Eigen::Isometry3d output_base_to_lidar_eigen = - base_to_parent_eigen_ * output_parent_to_child_eigen_ * child_to_lidar_eigen_; - - // The ground_plane tf lies in the plane and is aligned with the initial base_link in the x, y, - // and yaw. The z, pitch, and roll may differ due to the calibration - geometry_msgs::msg::TransformStamped output_lidar_to_base_msg = - tf2::eigenToTransform(output_base_to_lidar_eigen.inverse()); - output_lidar_to_base_msg.header.stamp = header_.stamp; - output_lidar_to_base_msg.header.frame_id = lidar_frame_; - output_lidar_to_base_msg.child_frame_id = lidar_frame_ + "_ground_plane"; - tf_broadcaster_.sendTransform(output_lidar_to_base_msg); -} - -Eigen::Vector4d ExtrinsicGroundPlaneCalibrator::poseToPlaneModel( - const Eigen::Isometry3d & pose) const -{ - Eigen::Vector3d normal_vector_base( - 0.0, 0.0, 1.0); // We use a +z for the normal of the plane. TODO: confirm if PCL does the same - Eigen::Vector3d normal_vector_lidar = pose.rotation() * normal_vector_base; - - Eigen::Vector4d model; // (a, b, c, d) from a*x + b*y + c*z + d = 0 - model(0) = normal_vector_lidar.x(); - model(1) = normal_vector_lidar.y(); - model(2) = normal_vector_lidar.z(); - model(3) = -normal_vector_lidar.dot(pose.translation()); - - return model; -} - -Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::modelPlaneToPose( - const Eigen::Vector4d & model) const -{ - Eigen::Vector3d n(model(0), model(1), model(2)); - n.normalize(); - - Eigen::Vector3d x0 = -n * model(3); - - // To create a real pose we need to invent a basis - Eigen::Vector3d base_x, base_y, base_z; - base_z = n; - - Eigen::Vector3d c1 = Eigen::Vector3d(1.0, 0.0, 0.0).cross(n); - Eigen::Vector3d c2 = Eigen::Vector3d(0.0, 1.0, 0.0).cross(n); - Eigen::Vector3d c3 = Eigen::Vector3d(0.0, 0.0, 1.0).cross(n); - - // Any non-zero would work but we use the one with the highest norm (there has to be a non zero) - if (c1.norm() > c2.norm() && c1.norm() > c3.norm()) { - base_x = c1; - } else if (c2.norm() > c3.norm()) { - base_x = c2; - } else { - base_x = c3; - } - - base_y = base_z.cross(base_x); - - Eigen::Matrix3d rot; - rot.col(0) = base_x.normalized(); - rot.col(1) = base_y.normalized(); - rot.col(2) = base_z.normalized(); - - Eigen::Isometry3d pose; - pose.translation() = x0; - pose.linear() = rot; - - return pose; -} - -Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::refineBaseLidarPose( - const Eigen::Isometry3d & base_lidar_pose, const Eigen::Vector4d & model) const -{ - const Eigen::Isometry3d lidar_base_pose = base_lidar_pose.inverse(); - const Eigen::Isometry3d lidar_ground_pose = modelPlaneToPose(model); - - const Eigen::Isometry3d ground_base = lidar_ground_pose.inverse() * lidar_base_pose; - - Eigen::Vector3d ground_base_projected_translation = ground_base.translation(); - ground_base_projected_translation.z() = 0; - - Eigen::Vector3d ground_base_projected_rotation_x = ground_base.rotation().col(0); - ground_base_projected_rotation_x.z() = 0.0; - ground_base_projected_rotation_x.normalize(); - - Eigen::Matrix3d ground_base_projected_rotation; - ground_base_projected_rotation.col(2) = Eigen::Vector3d(0.0, 0.0, 1.0); - ground_base_projected_rotation.col(0) = ground_base_projected_rotation_x; - ground_base_projected_rotation.col(1) = - ground_base_projected_rotation.col(2).cross(ground_base_projected_rotation.col(0)); - - Eigen::Isometry3d ground_base_projected; - ground_base_projected.translation() = ground_base_projected_translation; - ground_base_projected.linear() = ground_base_projected_rotation; - - return ground_base_projected.inverse() * lidar_ground_pose.inverse(); -} - -pcl::PointCloud::Ptr ExtrinsicGroundPlaneCalibrator::removeOutliers( - pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, - double outlier_tolerance) const -{ - pcl::ExtractIndices extract; - pcl::PointCloud::Ptr inliers(new pcl::PointCloud); - pcl::PointIndices::Ptr outliers(new pcl::PointIndices); - - for (std::size_t index = 0; index < input_pointcloud->size(); index++) { - const auto & p = input_pointcloud->points[index]; - double error = std::abs( - outlier_plane_model.x() * p.x + outlier_plane_model.y() * p.y + - outlier_plane_model.z() * p.z + outlier_plane_model.w()); - - if (error < outlier_tolerance) { - outliers->indices.push_back(index); - } - } - - // Extract the inliers from the pointcloud (the detected plane was not the ground plane) - extract.setInputCloud(input_pointcloud); - extract.setIndices(outliers); - extract.setNegative(true); - extract.filter(*inliers); - - return inliers; -} diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py deleted file mode 100644 index ea2d5e98..00000000 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py +++ /dev/null @@ -1,502 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2020 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 collections import deque -import json -import os -import threading -import time - -import cv2 -from cv_bridge import CvBridge -from extrinsic_interactive_calibrator.utils import decompose_transformation_matrix -from extrinsic_interactive_calibrator.utils import stamp_to_seconds -from extrinsic_interactive_calibrator.utils import tf_message_to_transform_matrix -from extrinsic_interactive_calibrator.utils import transform_matrix_to_tf_message -from extrinsic_interactive_calibrator.utils import transform_points -from geometry_msgs.msg import Point -from geometry_msgs.msg import PointStamped -import numpy as np -import rclpy -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup -from rclpy.duration import Duration -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from rclpy.qos import qos_profile_system_default -import ros2_numpy -from rosidl_runtime_py.convert import message_to_ordereddict -from sensor_msgs.msg import CameraInfo -from sensor_msgs.msg import CompressedImage -from sensor_msgs.msg import Image -from sensor_msgs.msg import PointCloud2 -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster -from tf2_ros.transform_listener import TransformListener -from tier4_calibration_msgs.msg import CalibrationPoints -from tier4_calibration_msgs.srv import ExtrinsicCalibrator -from tier4_calibration_msgs.srv import IntrinsicsOptimizer -import transforms3d -from visualization_msgs.msg import MarkerArray - - -class RosInterface(Node): - def __init__(self): - super().__init__("interactive_calibrator") - - self.lock = threading.RLock() - - self.declare_parameter("camera_parent_frame", rclpy.Parameter.Type.STRING) - self.declare_parameter("camera_frame", rclpy.Parameter.Type.STRING) - self.declare_parameter("use_compressed", True) - self.declare_parameter("timer_period", 1.0) - self.declare_parameter("delay_tolerance", 0.5) - self.declare_parameter("use_calibration_api", True) - self.declare_parameter("can_publish_tf", True) - - self.camera_parent_frame = ( - self.get_parameter("camera_parent_frame").get_parameter_value().string_value - ) - self.camera_frame = self.get_parameter("camera_frame").get_parameter_value().string_value - self.use_compressed = self.get_parameter("use_compressed").get_parameter_value().bool_value - self.timer_period = ( - self.get_parameter("timer_period").get_parameter_value().double_value - ) # 1.0 - self.delay_tolerance = ( - self.get_parameter("delay_tolerance").get_parameter_value().double_value - ) # 0.03 - self.use_calibration_api = ( - self.get_parameter("use_calibration_api").get_parameter_value().bool_value - ) - self.can_publish_tf = self.get_parameter("can_publish_tf").get_parameter_value().bool_value - - self.image_frame = None - self.lidar_frame = None - - self.ros_context = None - self.ros_executor = None - - # State - self.paused = False - - # Data - self.pointcloud_queue = deque([], 5) - self.camera_info_queue = deque([], 5) - self.image_queue = deque([], 5) - - self.pointcloud_sync = None - self.camera_info_sync = None - self.image_sync = None - - self.new_output_tf = False - self.optimize_camera_intrinsics_available = False - self.optimize_camera_intrinsics_future = None - - self.calibration_error = np.inf - self.output_transform_msg = None - self.calibration_api_sent_pending = False - - # ROS Interface configuration - self.publish_tf = None - self.republish_data = None - self.sensor_data_callback = None - self.transform_callback = None - self.object_point_callback = None - self.external_calibration_points_callback = None - self.optimize_camera_intrinsics_status_callback = None - self.optimize_camera_intrinsics_result_callback = None - self.calibration_api_request_received_callback = None - self.calibration_api_request_sent_callback = None - - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - self.tf_publisher = StaticTransformBroadcaster(self) - self.bridge = CvBridge() - - self.lidar_subs = self.create_subscription( - PointCloud2, "pointcloud", self.pointcloud_callback, qos_profile_sensor_data - ) - - if self.use_compressed: - self.image_sub = self.create_subscription( - CompressedImage, "image", self.image_callback, qos_profile_sensor_data - ) - else: - self.image_sub = self.create_subscription( - Image, "image", self.image_callback, qos_profile_sensor_data - ) - - self.camera_info_sub = self.create_subscription( - CameraInfo, "camera_info", self.camera_info_callback, qos_profile_sensor_data - ) - self.point_sub = self.create_subscription( - PointStamped, "/clicked_point", self.point_callback, qos_profile_system_default - ) - self.point_sub = self.create_subscription( - CalibrationPoints, - "calibration_points_input", - self.calibration_points_callback, - qos_profile_system_default, - ) - - self.markers_pub = self.create_publisher(MarkerArray, "markers", qos_profile_sensor_data) - self.image_pub = self.create_publisher(Image, "calibration/image", qos_profile_sensor_data) - self.camera_info_pub = self.create_publisher( - CameraInfo, "calibration/camera_info", qos_profile_sensor_data - ) - self.pointcloud_pub = self.create_publisher( - PointCloud2, "calibration/pointcloud", qos_profile_sensor_data - ) - - self.optimize_camera_intrinsics_client = self.create_client( - IntrinsicsOptimizer, "optimize_intrinsics" - ) - - if self.use_calibration_api: - self.service_callback_group = MutuallyExclusiveCallbackGroup() - self.calibration_api_service_server = self.create_service( - ExtrinsicCalibrator, - "extrinsic_calibration", - self.calibration_api_service_callback, - callback_group=self.service_callback_group, - ) - - self.timer = self.create_timer(self.timer_period, self.timer_callback) - - def send_calibration_api_result(self, calibration_error): - with self.lock: - self.calibration_api_sent_pending = True - self.calibration_error = -calibration_error - - def calibration_api_service_callback(self, request, response): - # Notify the UI that a request was received - self.calibration_api_request_received_callback() - - # Loop until the response arrives - while rclpy.ok(): - with self.lock: - if self.calibration_api_sent_pending: - break - - time.sleep(1.0) - - with self.lock: - assert self.output_transform_msg is not None - - response.success = True - response.result_pose.position.x = self.output_transform_msg.transform.translation.x - response.result_pose.position.y = self.output_transform_msg.transform.translation.y - response.result_pose.position.z = self.output_transform_msg.transform.translation.z - response.result_pose.orientation = self.output_transform_msg.transform.rotation - - response.score = self.calibration_error - - self.calibration_api_request_sent_callback() - - return response - - def set_sensor_data_callback(self, callback): - with self.lock: - self.sensor_data_callback = callback - - def set_transform_callback(self, callback): - with self.lock: - self.transform_callback = callback - - def set_object_point_callback(self, callback): - with self.lock: - self.object_point_callback = callback - - def set_external_calibration_points_callback(self, callback): - with self.lock: - self.external_calibration_points_callback = callback - - def set_optimize_camera_intrinsics_status_callback(self, callback): - self.optimize_camera_intrinsics_status_callback = callback - - def set_optimize_camera_intrinsics_result_callback(self, callback): - self.optimize_camera_intrinsics_result_callback = callback - - def set_calibration_api_request_received_callback(self, callback): - self.calibration_api_request_received_callback = callback - - def set_calibration_api_request_sent_callback(self, callback): - self.calibration_api_request_sent_callback = callback - - def set_republish_data(self, value): - with self.lock: - self.republish_data = value - - def set_publish_tf(self, value): - with self.lock: - self.publish_tf = value - - def is_paused(self): - with self.lock: - return self.paused - - def set_paused(self, value): - with self.lock: - self.paused = value - - def set_camera_lidar_transform(self, camera_optical_lidar_transform): - with self.lock: - optical_axis_to_camera_transform = np.zeros((4, 4)) - optical_axis_to_camera_transform[0, 1] = -1 - optical_axis_to_camera_transform[1, 2] = -1 - optical_axis_to_camera_transform[2, 0] = 1 - optical_axis_to_camera_transform[3, 3] = 1 - - try: - camera_parent_lidar_tf = self.tf_buffer.lookup_transform( - self.camera_parent_frame, - self.lidar_frame, - rclpy.time.Time(), - timeout=Duration(seconds=1.0), - ) - camera_parent_lidar_transform = tf_message_to_transform_matrix( - camera_parent_lidar_tf - ) - except TransformException as ex: - self.get_logger().error( - f"Could not transform {self.camera_parent_frame} to {self.lidar_frame}: {ex}" - ) - return - - camera_camera_parent_transform = ( - np.linalg.inv(optical_axis_to_camera_transform) - @ camera_optical_lidar_transform - @ np.linalg.inv(camera_parent_lidar_transform) - ) - - self.output_transform_msg = transform_matrix_to_tf_message( - np.linalg.inv(camera_camera_parent_transform) - ) - self.output_transform_msg.header.frame_id = self.camera_parent_frame - self.output_transform_msg.child_frame_id = self.camera_frame - self.new_output_tf = True - - def optimize_camera_intrinsics(self, object_points, image_points): - req = IntrinsicsOptimizer.Request() - - for object_point, image_point in zip(object_points, image_points): - point3d = Point() - point3d.x = object_point[0] - point3d.y = object_point[1] - point3d.z = object_point[2] - req.calibration_points.object_points.append(point3d) - - point2d = Point() - point2d.x = image_point[0] - point2d.y = image_point[1] - req.calibration_points.image_points.append(point2d) - - req.initial_camera_info = self.camera_info_sync - - self.optimize_camera_intrinsics_future = self.optimize_camera_intrinsics_client.call_async( - req - ) - - def save_calibration_tfs(self, output_dir): - with self.lock: - d = message_to_ordereddict(self.output_transform_msg) - - q = self.output_transform_msg.transform.rotation - e = transforms3d.euler.quat2euler((q.w, q.x, q.y, q.z)) - - d["roll"] = e[0] - d["pitch"] = e[1] - d["yaw"] = e[2] - - with open(os.path.join(output_dir, "tf.json"), "w") as fout: - fout.write(json.dumps(d, indent=4, sort_keys=False)) - - def pointcloud_callback(self, pointcloud_msg): - self.lidar_frame = pointcloud_msg.header.frame_id - self.pointcloud_queue.append(pointcloud_msg) - self.check_sync() - - def image_callback(self, image_msg): - self.image_queue.append(image_msg) - self.check_sync() - - def camera_info_callback(self, camera_info_msg): - self.camera_info_queue.append(camera_info_msg) - self.image_frame = camera_info_msg.header.frame_id - - def check_sync(self): - with self.lock: - if self.paused: - return - - if ( - len(self.camera_info_queue) == 0 - or len(self.image_queue) == 0 - or len(self.pointcloud_queue) == 0 - ): - return - - found = False - min_delay = 10000 - - for pointcloud_msg in self.pointcloud_queue: - for image_msg in self.image_queue: - current_delay = abs( - stamp_to_seconds(pointcloud_msg.header.stamp) - - stamp_to_seconds(image_msg.header.stamp) - ) - - min_delay = min(min_delay, current_delay) - - if current_delay < self.delay_tolerance: - found = True - break - - if not found: - return - - pc_data = ros2_numpy.numpify(pointcloud_msg) - points = np.zeros(pc_data.shape + (4,)) - points[..., 0] = pc_data["x"] - points[..., 1] = pc_data["y"] - points[..., 2] = pc_data["z"] - points[..., 3] = ( - pc_data["intensity"] - if "intensity" in pc_data.dtype.names - else np.zeros_like(pc_data["x"]) - ) - points = points.reshape(-1, 4) - - with self.lock: - self.camera_info_sync = self.camera_info_queue[-1] - self.image_sync = image_msg - self.pointcloud_sync = pointcloud_msg - - if self.use_compressed: - 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) - # image = cv2.cvtColor(self.raw_image, cv2.COLOR_BGR2RGB) - - self.sensor_data_callback(self.image_sync, self.camera_info_sync, points) - - self.image_queue.clear() # this is suboptical but is only for the gui - self.pointcloud_queue.clear() - - def point_callback(self, point): - point_xyz = np.array([point.point.x, point.point.y, point.point.z]).reshape(1, 3) - - if point.header.frame_id != self.lidar_frame: - try: - lidar_to_point_frame_tf = self.tf_buffer.lookup_transform( - self.lidar_frame, - point.header.frame_id, - rclpy.time.Time(), - timeout=Duration(seconds=1.0), - ) - lidar_to_point_frame_transform = tf_message_to_transform_matrix( - lidar_to_point_frame_tf - ) - translation, rotation = decompose_transformation_matrix( - lidar_to_point_frame_transform - ) - - point_xyz = transform_points(translation, rotation, point_xyz) - except TransformException as ex: - self.get_logger().error( - f"Could not transform {self.lidar_frame} to {point.header.frame_id}: {ex}" - ) - return - - self.object_point_callback(point_xyz) - - def calibration_points_callback(self, calibration_points): - object_points = calibration_points.object_points - image_points = calibration_points.image_points - - assert len(object_points) == len(object_points) - - object_points = [np.array([p.x, p.y, p.z]) for p in object_points] - image_points = [np.array([p.x, p.y]) for p in image_points] - - self.external_calibration_points_callback(object_points, image_points) - - def timer_callback(self): - with self.lock: - service_status = self.optimize_camera_intrinsics_client.service_is_ready() - if ( - service_status != self.optimize_camera_intrinsics_available - and self.camera_info_sync is not None - ): - self.optimize_camera_intrinsics_status_callback(service_status) - self.optimize_camera_intrinsics_available = service_status - - if ( - self.optimize_camera_intrinsics_future is not None - and self.optimize_camera_intrinsics_future.done() - ): - response = self.optimize_camera_intrinsics_future.result() - self.optimize_camera_intrinsics_result_callback(response.optimized_camera_info) - self.optimize_camera_intrinsics_future = None - - if self.image_frame is None or self.lidar_frame is None: - return - - try: - transform = self.tf_buffer.lookup_transform( - self.image_frame, - self.lidar_frame, - rclpy.time.Time(seconds=0, nanoseconds=0), - timeout=Duration(seconds=0.2), - ) - - transform_matrix = tf_message_to_transform_matrix(transform) - self.transform_callback(transform_matrix) - except TransformException as ex: - self.get_logger().error( - f"Could not transform {self.image_frame} to {self.lidar_frame}: {ex}" - ) - - now = self.get_clock().now().to_msg() - - if self.publish_tf and self.new_output_tf and self.can_publish_tf: - self.output_transform_msg.header.stamp = now - self.tf_publisher.sendTransform(self.output_transform_msg) - self.new_output_tf = False - - if self.republish_data and self.image_sync is not None: - assert self.camera_info_sync is not None - assert self.pointcloud_sync is not None - - image_msg = self.bridge.cv2_to_imgmsg(self.image_sync) - - image_msg.header.stamp = now - self.camera_info_sync.header.stamp = now - self.pointcloud_sync.header.stamp = now - - self.image_pub.publish(image_msg) - self.camera_info_pub.publish(self.camera_info_sync) - self.pointcloud_pub.publish(self.pointcloud_sync) - - def spin(self): - self.ros_executor = MultiThreadedExecutor(num_threads=2) - self.ros_executor.add_node(self) - - self.thread = threading.Thread(target=self.executor.spin, args=()) - self.thread.setDaemon(True) - self.thread.start() diff --git a/sensor/extrinsic_interactive_calibrator/setup.cfg b/sensor/extrinsic_interactive_calibrator/setup.cfg deleted file mode 100644 index 469f7bfc..00000000 --- a/sensor/extrinsic_interactive_calibrator/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/extrinsic_interactive_calibrator -[install] -install_scripts=$base/lib/extrinsic_interactive_calibrator diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_lidar_to_lidar_2d_calibrator.hpp b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_lidar_to_lidar_2d_calibrator.hpp deleted file mode 100755 index c20cc355..00000000 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_lidar_to_lidar_2d_calibrator.hpp +++ /dev/null @@ -1,167 +0,0 @@ -// Copyright 2023 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. - -#ifndef EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR__EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ -#define EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR__EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#else -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#endif -#include -#include -#include - -#include -#include -#include -#include -#include - -using PointType = pcl::PointXYZ; - -class LidarToLidar2DCalibrator : public rclcpp::Node -{ -public: - explicit LidarToLidar2DCalibrator(const rclcpp::NodeOptions & options); - -protected: - void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); - - void sourcePointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr pc); - void targetPointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr pc); - void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr pc); - - void calibrationTimerCallback(); - - bool checkInitialTransforms(); - - double getAlignmentError( - pcl::PointCloud::Ptr source_pointcloud, - pcl::PointCloud::Ptr target_pointcloud); - - void findBestTransform( - pcl::PointCloud::Ptr & source_pointcloud_ptr, - pcl::PointCloud::Ptr & target_pointcloud_ptr, - std::vector::Ptr> & registratators, - pcl::PointCloud::Ptr & best_aligned_pointcloud_ptr, Eigen::Matrix4f & best_transform, - float & best_score); - - // Parameters - std::string base_frame_; - std::string sensor_kit_frame_; // the parent for this calibration method must be a sensor kit - std::string lidar_base_frame_; // the child for this calibration method must be a the base of a - // lidar (probably different from the actua lidar tf) - bool broadcast_calibration_tf_; - bool filter_estimations_; - double max_calibration_range_; - double max_corr_distance_; - int max_iterations_; - - double initial_angle_cov_; - double initial_xy_cov_; - double angle_measurement_cov_; - double angle_process_cov_; - double xy_measurement_cov_; - double xy_process_cov_; - double angle_convergence_threshold_; - double xy_convergence_threshold_; - - double min_z_; - double max_z_; - - // ROS Interface - tf2_ros::StaticTransformBroadcaster tf_broadcaster_; - std::shared_ptr tf_buffer_; - std::shared_ptr transform_listener_; - - rclcpp::TimerBase::SharedPtr calib_timer_; - - rclcpp::CallbackGroup::SharedPtr subs_callback_group_; - rclcpp::CallbackGroup::SharedPtr srv_callback_group_; - - rclcpp::Publisher::SharedPtr markers_pub_; - rclcpp::Publisher::SharedPtr source_pub_; - rclcpp::Publisher::SharedPtr target_pub_; - - rclcpp::Subscription::SharedPtr source_pointcloud_sub_; - rclcpp::Subscription::SharedPtr target_pointcloud_sub_; - - rclcpp::Service::SharedPtr service_server_; - - // Threading, sync, and result - std::mutex mutex_; - geometry_msgs::msg::Pose initial_calibration_; - - // ROS Data - sensor_msgs::msg::PointCloud2::SharedPtr source_pointcloud_msg_; - sensor_msgs::msg::PointCloud2::SharedPtr target_pointcloud_msg_; - std_msgs::msg::Header source_pointcloud_header_; - std_msgs::msg::Header target_pointcloud_header_; - std::string source_pointcloud_frame_; - std::string target_pointcloud_frame_; - - // Initial tfs comparable with the one with our method - geometry_msgs::msg::Transform initial_base_to_source_msg_; - geometry_msgs::msg::Transform initial_base_to_target_msg_; - tf2::Transform initial_base_to_source_tf2_; - tf2::Transform initial_base_to_target_tf2_; - Eigen::Affine3d initial_base_to_source_eigen_; - Eigen::Affine3d initial_base_to_target_eigen_; - - // Other tfs to calculate the complete chain. There are constant for our purposes - geometry_msgs::msg::Transform base_to_sensor_kit_msg_; - geometry_msgs::msg::Transform lidar_base_to_source_msg_; - tf2::Transform base_to_sensor_kit_tf2_; - tf2::Transform lidar_base_to_source_tf2_; - Eigen::Affine3d base_to_sensor_kit_eigen_; - Eigen::Affine3d lidar_base_to_source_eigen_; - - std::vector::Ptr> calibration_registrators_; - pcl::GeneralizedIterativeClosestPoint::Ptr calibration_gicp_; - pcl::IterativeClosestPoint::Ptr calibration_icp_, calibration_icp_fine_, - calibration_icp_fine_2_, calibration_icp_fine_3_; - - geometry_msgs::msg::Pose output_calibration_msg_; - - bool got_initial_transform_; - bool received_request_; - bool broadcast_tf_; - bool calibration_done_; - - // Filtering - KalmanFilter kalman_filter_; - bool first_observation_; -}; - -#endif // EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR__EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml deleted file mode 100644 index 9079f8c0..00000000 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/velodyne_top.rviz b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/velodyne_top.rviz deleted file mode 100644 index 6d424c49..00000000 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/velodyne_top.rviz +++ /dev/null @@ -1,494 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.500627338886261 - Tree Height: 725 - - 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: center_source -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: 40 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: base_link - Radius: 0.10000000149011612 - Reference Frame: base_link - Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: velodyne_top - Radius: 0.10000000149011612 - Reference Frame: velodyne_top - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/sensor_kit_base_link/markers - Value: false - - 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: 140 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: top - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: 164 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_left - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_left/livox/lidar - Use Fixed Frame: true - Use rainbow: true - 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: 150 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_center - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_center/livox/lidar - Use Fixed Frame: true - Use rainbow: true - 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: 150 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_right - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_right/livox/lidar - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: left_source - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/source_points_2d - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: left_target - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/target_points_2d - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: center_source - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/source_points_2d - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: center_target - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/target_points_2d - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: right_source - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/source_points_2d - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: right_target - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/target_points_2d - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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: - Angle: -1.5749987363815308 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 20.338735580444336 - Target Frame: - Value: TopDownOrtho (rviz_default_plugins) - X: 4.058230876922607 - Y: 0.42603132128715515 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001990000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004930000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_manual_calibrator/COLCON_IGNORE b/sensor/extrinsic_manual_calibrator/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/sensor/extrinsic_map_based_calibrator/COLCON_IGNORE b/sensor/extrinsic_map_based_calibrator/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/base_lidar_calibrator.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/base_lidar_calibrator.hpp deleted file mode 100644 index 10caddee..00000000 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/base_lidar_calibrator.hpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2023 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. - -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ - -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include - -class BaseLidarCalibrator : public SensorCalibrator -{ -public: - using Ptr = std::shared_ptr; - using PointPublisher = rclcpp::Publisher; - using PointSubscription = rclcpp::Subscription; - using FrameService = rclcpp::Service; - - BaseLidarCalibrator( - CalibrationParameters::Ptr & parameters, MappingData::Ptr & mapping_data, - std::shared_ptr & tf_buffer, tf2_ros::StaticTransformBroadcaster & broadcaster, - PointPublisher::SharedPtr & augmented_pointcloud_pub, - PointPublisher::SharedPtr & ground_pointcloud_pub); - - void calibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response); - - /*! - * Calibrate the lidar - */ - bool calibrate(Eigen::Matrix4f & best_transform, float & best_score) override; - - virtual void singleSensorCalibrationCallback( - __attribute__((unused)) const std::shared_ptr - request, - __attribute__((unused)) const std::shared_ptr - response) - { - } - - virtual void multipleSensorCalibrationCallback( - __attribute__((unused)) const std::shared_ptr - request, - __attribute__((unused)) const std::shared_ptr - response) - { - } - - virtual void configureCalibrators() {} - -protected: - /*! - * Extract the ground plane of a pointcloud - * @param[in] pointcloud Source pointcloud - * @param[in] initial_normal Target pointcloud - * @param[in] model Target pointcloud - * @param[in] inliers_pointcloud Target pointcloud - */ - bool extractGroundPlane( - pcl::PointCloud::Ptr & pointcloud, const Eigen::Vector3f & initial_normal, - Eigen::Vector4d & model, pcl::PointCloud::Ptr & inliers_pointcloud); - - /*! - * Publish the calibration results - */ - void publishResults( - const Eigen::Vector4d & ground_model, - const pcl::PointCloud::Ptr & ground_plane_inliers, - const pcl::PointCloud::Ptr & augmented_pointcloud_ptr); - - Eigen::Isometry3d modelPlaneToPose(const Eigen::Vector4d & model) const; - - tf2_ros::StaticTransformBroadcaster & tf_broadcaster_; - PointPublisher::SharedPtr augmented_pointcloud_pub_; - PointPublisher::SharedPtr ground_pointcloud_pub_; -}; - -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/rviz/xx1.rviz b/sensor/extrinsic_mapping_based_calibrator/rviz/xx1.rviz deleted file mode 100644 index a24f4fba..00000000 --- a/sensor/extrinsic_mapping_based_calibrator/rviz/xx1.rviz +++ /dev/null @@ -1,886 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /mapping velodyne pointcloud1/Topic1 - - /camera2_target1 - Splitter Ratio: 0.5 - Tree Height: 725 - - 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 - - /Current View1/Position1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.10000000149011612 - 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: 100 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: mapping velodyne 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/top/outlier_filtered/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: output map - 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: Reliable - Value: /output_map - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.10000000149011612 - Head Length: 0.12999999523162842 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Frame Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 0; 0 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.019999999552965164 - Shaft Length: 0.05000000074505806 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /frame_path - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 255; 255 - Enabled: true - Head Diameter: 0.07999999821186066 - Head Length: 0.12999999523162842 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Frame Predicted Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 255; 255 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.019999999552965164 - Shaft Length: 0.03999999910593033 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /frame_predicted_path - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.10000000149011612 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: KeyFrame Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 0; 255; 0 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.05999999865889549 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /keyframe_path - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: KeyFrame Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /keyframe_markers - 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; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_left_initial_source_aligned - 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: Reliable - Value: /velodyne_left/initial_source_aligned_map - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_left_calibrated_source_aligned - 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: Reliable - Value: /velodyne_left/calibrated_source_aligned_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_left_target - 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: Reliable - Value: /velodyne_left/target_map - Use Fixed Frame: true - Use rainbow: true - 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; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_right_initial_source_aligned - 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: Reliable - Value: /velodyne_right/initial_source_aligned_map - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_right_calibrated_source_aligned - 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: Reliable - Value: /velodyne_right/calibrated_source_aligned_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_right_target - 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: Reliable - Value: /velodyne_right/target_map - Use Fixed Frame: true - Use rainbow: true - 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; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_rear_initial_source_aligned - 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: Reliable - Value: /velodyne_rear/initial_source_aligned_map - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_rear_calibrated_source_aligned - 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: Reliable - Value: /velodyne_rear/calibrated_source_aligned_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: velodyne_rear_target - 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: Reliable - Value: /velodyne_rear/target_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: camera0_target - 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: Reliable - Value: /camera0/camera_optical_link/target_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: camera1_target - 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: Reliable - Value: /camera1/camera_optical_link/target_map - Use Fixed Frame: true - Use rainbow: true - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: camera2_target - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera2/camera_optical_link/target_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: camera3_target - 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: Reliable - Value: /camera3/camera_optical_link/target_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: camera4_target - 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: Reliable - Value: /camera4/camera_optical_link/target_map - Use Fixed Frame: true - Use rainbow: true - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: camera5_target - 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: Reliable - Value: /camera5/camera_optical_link/target_map - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_0_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera0/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_1_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera1/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_2_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera2/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_3_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera3/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_4_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera4/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_5_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera5/camera_optical_link/target_markers - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: calibration_map - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Position: - X: -30 - Y: -17 - Z: 20 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 0 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002df0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004530000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_mapping_based_calibrator/src/base_lidar_calibrator.cpp b/sensor/extrinsic_mapping_based_calibrator/src/base_lidar_calibrator.cpp deleted file mode 100644 index f64968e5..00000000 --- a/sensor/extrinsic_mapping_based_calibrator/src/base_lidar_calibrator.cpp +++ /dev/null @@ -1,267 +0,0 @@ -// Copyright 2023 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. - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#define UNUSED(x) (void)x; - -BaseLidarCalibrator::BaseLidarCalibrator( - CalibrationParameters::Ptr & parameters, MappingData::Ptr & mapping_data, - std::shared_ptr & tf_buffer, tf2_ros::StaticTransformBroadcaster & broadcaster, - PointPublisher::SharedPtr & augmented_pointcloud_pub, - PointPublisher::SharedPtr & ground_pointcloud_pub) -: SensorCalibrator( - mapping_data->mapping_lidar_frame_, - "base_lidar_calibrator(" + mapping_data->mapping_lidar_frame_ + ")", parameters, mapping_data, - tf_buffer), - tf_broadcaster_(broadcaster), - augmented_pointcloud_pub_(augmented_pointcloud_pub), - ground_pointcloud_pub_(ground_pointcloud_pub) -{ -} - -void BaseLidarCalibrator::calibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) -{ - UNUSED(request); - - Eigen::Matrix4f result; - float score; - response->success = calibrate(result, score); -} - -bool BaseLidarCalibrator::calibrate( - __attribute__((unused)) Eigen::Matrix4f & base_link_transform, - __attribute__((unused)) float & best_score) -{ - auto & last_keyframe = data_->keyframes_and_stopped_.back(); - PointcloudType::Ptr augmented_pointcloud_ptr = getDensePointcloudFromMap( - last_keyframe->pose_, last_keyframe, parameters_->leaf_size_dense_map_, - parameters_->min_calibration_range_, parameters_->max_calibration_range_); - - Eigen::Matrix4f initial_lidar_to_base_transform; - Eigen::Isometry3d initial_lidar_to_base_affine; - - try { - rclcpp::Time t = rclcpp::Time(0); - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - - initial_lidar_to_base_affine = tf2::transformToEigen( - tf_buffer_->lookupTransform(data_->mapping_lidar_frame_, "base_link", t, timeout).transform); - - initial_lidar_to_base_transform = initial_lidar_to_base_affine.matrix().cast(); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "could not get initial tf. %s", ex.what()); - - return false; - } - - Eigen::Vector3f estimated_normal = - initial_lidar_to_base_affine.rotation().cast() * Eigen::Vector3f(0.f, 0.f, 1.f); - - PointcloudType::Ptr augmented_pointcloud_base_ptr(new PointcloudType()); - pcl::transformPointCloud( - *augmented_pointcloud_ptr, *augmented_pointcloud_base_ptr, - initial_lidar_to_base_transform.inverse()); - - pcl::CropBox box_filter; - box_filter.setMin(Eigen::Vector4f( - parameters_->base_lidar_crop_box_min_x_, parameters_->base_lidar_crop_box_min_y_, - parameters_->base_lidar_crop_box_min_z_, 1.0)); - box_filter.setMax(Eigen::Vector4f( - parameters_->base_lidar_crop_box_max_x_, parameters_->base_lidar_crop_box_max_y_, - parameters_->base_lidar_crop_box_max_z_, 1.0)); - box_filter.setInputCloud(augmented_pointcloud_base_ptr); - box_filter.filter(*augmented_pointcloud_base_ptr); - - pcl::transformPointCloud( - *augmented_pointcloud_base_ptr, *augmented_pointcloud_ptr, initial_lidar_to_base_transform); - - PointcloudType::Ptr ground_plane_inliers_ptr(new PointcloudType()); - Eigen::Vector4d ground_plane_model; - extractGroundPlane( - augmented_pointcloud_ptr, estimated_normal, ground_plane_model, ground_plane_inliers_ptr); - - publishResults(ground_plane_model, ground_plane_inliers_ptr, augmented_pointcloud_ptr); - - return true; -} - -bool BaseLidarCalibrator::extractGroundPlane( - pcl::PointCloud::Ptr & pointcloud, const Eigen::Vector3f & initial_normal, - Eigen::Vector4d & model, pcl::PointCloud::Ptr & inliers_pointcloud) -{ - std::vector models; - - RCLCPP_INFO( - rclcpp::get_logger(calibrator_name_), "Rough plane normal. x=%.3f, y=%.3f, z=%.3f", - initial_normal.x(), initial_normal.y(), initial_normal.z()); - - // Use RANSAC iteratively until we find the ground plane - // Since walls can have more points, we filter using the PCA-based hypothesis - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - pcl::SACSegmentation seg; - pcl::ExtractIndices extract; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(parameters_->base_lidar_max_inlier_distance_); - seg.setMaxIterations(parameters_->base_lidar_max_iterations_); - - pcl::PointCloud::Ptr iteration_cloud(new pcl::PointCloud()); - pcl::copyPointCloud(*pointcloud, *iteration_cloud); - int iteration_size = iteration_cloud->height * iteration_cloud->width; - - while (iteration_size > parameters_->base_lidar_min_plane_points_) { - seg.setInputCloud(iteration_cloud); - seg.segment(*inliers, *coefficients); - - if (inliers->indices.size() == 0) { - RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "No plane found in the pointcloud"); - break; - } - - Eigen::Vector3f normal( - coefficients->values[0], coefficients->values[1], coefficients->values[2]); - float cos_distance = 1.0 - std::abs(initial_normal.dot(normal)); - - model = Eigen::Vector4d( - coefficients->values[0], coefficients->values[1], coefficients->values[2], - coefficients->values[3]); - - int inlier_size = static_cast(inliers->indices.size()); - double inlier_percentage = 100.0 * inlier_size / pointcloud->size(); - - if ( - inlier_size > parameters_->base_lidar_min_plane_points_ && - inlier_percentage > parameters_->base_lidar_min_plane_points_percentage_ && - cos_distance < parameters_->base_lidar_max_cos_distance_) { - RCLCPP_INFO( - rclcpp::get_logger(calibrator_name_), "Plane found: inliers=%ld (%.3f)", - inliers->indices.size(), inlier_percentage); - RCLCPP_INFO( - rclcpp::get_logger(calibrator_name_), "Plane model. a=%.3f, b=%.3f, c=%.3f, d=%.3f", - model(0), model(1), model(2), model(3)); - RCLCPP_INFO( - rclcpp::get_logger(calibrator_name_), "Cos distance: %.3f / %.3f", cos_distance, - parameters_->base_lidar_max_cos_distance_); - - // Extract the ground plane inliers - extract.setInputCloud(iteration_cloud); - extract.setIndices(inliers); - extract.setNegative(false); - extract.filter(*inliers_pointcloud); - return true; - } - - // Extract the inliers from the pointcloud (the detected plane was not the ground plane) - extract.setInputCloud(iteration_cloud); - extract.setIndices(inliers); - extract.setNegative(true); - - pcl::PointCloud next_cloud; - extract.filter(next_cloud); - - iteration_cloud->swap(next_cloud); - iteration_size = iteration_cloud->height * iteration_cloud->width; - } - - return false; -} - -void BaseLidarCalibrator::publishResults( - const Eigen::Vector4d & ground_model, - const pcl::PointCloud::Ptr & ground_plane_inliers_ptr, - const pcl::PointCloud::Ptr & augmented_pointcloud_ptr) -{ - RCLCPP_INFO( - rclcpp::get_logger(calibrator_name_), "Estimated model. a=%f, b=%f, c=%f, d=%f", - ground_model.x(), ground_model.y(), ground_model.z(), ground_model.w()); - - sensor_msgs::msg::PointCloud2 ground_plane_inliers_msg, augmented_pointcloud_msg; - ground_plane_inliers_ptr->width = ground_plane_inliers_ptr->points.size(); - ground_plane_inliers_ptr->height = 1; - augmented_pointcloud_ptr->width = augmented_pointcloud_ptr->points.size(); - augmented_pointcloud_ptr->height = 1; - - pcl::toROSMsg(*ground_plane_inliers_ptr, ground_plane_inliers_msg); - pcl::toROSMsg(*augmented_pointcloud_ptr, augmented_pointcloud_msg); - - ground_plane_inliers_msg.header.frame_id = data_->mapping_lidar_frame_; - augmented_pointcloud_msg.header.frame_id = data_->mapping_lidar_frame_; - - augmented_pointcloud_pub_->publish(augmented_pointcloud_msg); - ground_pointcloud_pub_->publish(ground_plane_inliers_msg); - - Eigen::Isometry3d estimated_ground_pose = modelPlaneToPose(ground_model); - auto estimated_ground_msg = tf2::eigenToTransform(estimated_ground_pose); - estimated_ground_msg.header.frame_id = data_->mapping_lidar_frame_; - estimated_ground_msg.child_frame_id = "estimated_ground_pose"; - tf_broadcaster_.sendTransform(estimated_ground_msg); -} - -Eigen::Isometry3d BaseLidarCalibrator::modelPlaneToPose(const Eigen::Vector4d & model) const -{ - Eigen::Vector3d n(model(0), model(1), model(2)); - n.normalize(); - - Eigen::Vector3d x0 = -n * model(3); - - // To create a real pose we need to invent a basis - Eigen::Vector3d base_x, base_y, base_z; - base_z = n; - - Eigen::Vector3d c1 = Eigen::Vector3d(1.0, 0.0, 0.0).cross(n); - Eigen::Vector3d c2 = Eigen::Vector3d(0.0, 1.0, 0.0).cross(n); - Eigen::Vector3d c3 = Eigen::Vector3d(0.0, 0.0, 1.0).cross(n); - - // Any non-zero would work but we use the one with the highest norm (there has to be a non zero) - if (c1.norm() > c2.norm() && c1.norm() > c3.norm()) { - base_x = c1; - } else if (c2.norm() > c3.norm()) { - base_x = c2; - } else { - base_x = c3; - } - - base_y = base_z.cross(base_x); - - Eigen::Matrix3d rot; - rot.col(0) = base_x.normalized(); - rot.col(1) = base_y.normalized(); - rot.col(2) = base_z.normalized(); - - Eigen::Isometry3d pose; - pose.translation() = x0; - pose.linear() = rot; - - return pose; -} diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz deleted file mode 100644 index 98595c18..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz +++ /dev/null @@ -1,419 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /lidar1/Topic1 - - /lidar_background_pointcloud1/Topic1 - - /lidar_colored_clusters1/Topic1 - - /DetectedObjects1/Topic1 - - /text_markers1/Topic1 - Splitter Ratio: 0.5 - Tree Height: 752 - - 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: "" -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 220; 138; 221 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_foreground_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_foreground_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_colored_clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_colored_clusters - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: lidar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_detection_markers - Value: true - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/front_center/detected_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: radar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/radar_detection_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: radar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/radar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: tracking_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/tracking_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: matches_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/matches_markers - Value: true - - 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: 40 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: text_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /front_unit/front_unit_base_link/front_center/radar_link/text_markers - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_front - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.2697972357273102 - Position: - X: 0.5868673324584961 - Y: 18.44519805908203 - Z: 6.774729251861572 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.681333541870117 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000021600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005640000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 2560 - Y: 0 diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_left.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_left.rviz deleted file mode 100644 index 056eeae6..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_left.rviz +++ /dev/null @@ -1,405 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /lidar1/Topic1 - - /lidar_background_pointcloud1/Topic1 - - /lidar_colored_clusters1/Topic1 - - /DetectedObjects1/Topic1 - Splitter Ratio: 0.5 - Tree Height: 1106 - - 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: "" -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/lidar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 220; 138; 221 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_foreground_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/lidar_foreground_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_colored_clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/lidar_colored_clusters - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: lidar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/lidar_detection_markers - Value: true - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: false - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/front_left/detected_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: false - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: radar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/radar_detection_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: radar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/radar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: tracking_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/tracking_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: matches_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_left/radar_link/matches_markers - Value: true - - 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: 30 - Reference Frame: - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_front - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.274797260761261 - Position: - X: -4.431237697601318 - Y: 2.402275562286377 - Z: 3.859571933746338 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 5.75633430480957 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1403 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 2486 - X: 1994 - Y: 0 diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_right.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_right.rviz deleted file mode 100644 index dced29e4..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_right.rviz +++ /dev/null @@ -1,405 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /lidar1/Topic1 - - /lidar_background_pointcloud1/Topic1 - - /lidar_colored_clusters1/Topic1 - - /DetectedObjects1/Topic1 - Splitter Ratio: 0.5 - Tree Height: 1106 - - 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: "" -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/lidar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 220; 138; 221 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_foreground_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/lidar_foreground_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_colored_clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/lidar_colored_clusters - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: lidar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/lidar_detection_markers - Value: true - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: false - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/front_right/detected_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: false - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: radar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/radar_detection_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: radar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/radar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: tracking_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/tracking_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: matches_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_right/radar_link/matches_markers - Value: true - - 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: 40 - Reference Frame: - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_front - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.14479734003543854 - Position: - X: 5.181648254394531 - Y: 2.015692949295044 - Z: 2.1727044582366943 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 3.4863290786743164 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1403 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 2486 - X: 1994 - Y: 0 diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_center.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_center.rviz deleted file mode 100644 index b6ea3f53..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_center.rviz +++ /dev/null @@ -1,405 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /lidar1/Topic1 - - /lidar_background_pointcloud1/Topic1 - - /lidar_colored_clusters1/Topic1 - - /DetectedObjects1/Topic1 - Splitter Ratio: 0.5 - Tree Height: 1106 - - 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: "" -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/rear_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/lidar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 220; 138; 221 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_foreground_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/lidar_foreground_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_colored_clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/lidar_colored_clusters - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: lidar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/lidar_detection_markers - Value: true - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: false - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/rear_center/detected_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: false - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: radar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/radar_detection_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: radar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/radar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: tracking_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/tracking_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: matches_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_center/radar_link/matches_markers - Value: true - - 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: 40 - Reference Frame: - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_rear - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.2697972357273102 - Position: - X: 0.19182920455932617 - Y: 5.7288312911987305 - Z: 3.2564549446105957 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.681333541870117 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1403 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 2486 - X: 1994 - Y: 0 diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_right.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_right.rviz deleted file mode 100644 index bd55fc6f..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_right.rviz +++ /dev/null @@ -1,405 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /lidar1/Topic1 - - /lidar_background_pointcloud1/Topic1 - - /lidar_colored_clusters1/Topic1 - - /DetectedObjects1/Topic1 - Splitter Ratio: 0.5 - Tree Height: 1106 - - 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: "" -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/rear_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/lidar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 220; 138; 221 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_foreground_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/lidar_foreground_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_colored_clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/lidar_colored_clusters - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: lidar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/lidar_detection_markers - Value: true - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: false - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/rear_right/detected_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: false - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: radar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/radar_detection_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: radar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/radar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: tracking_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/tracking_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: matches_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_right/radar_link/matches_markers - Value: true - - 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: 40 - Reference Frame: - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_rear - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.11479736119508743 - Position: - X: -3.1487796306610107 - Y: 3.6161608695983887 - Z: 2.078787088394165 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 5.466327667236328 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1403 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 2486 - X: 1994 - Y: 0 diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/xx1.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/xx1.rviz deleted file mode 100644 index ed97f5b4..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/xx1.rviz +++ /dev/null @@ -1,387 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /lidar1/Topic1 - - /lidar_background_pointcloud1/Topic1 - - /lidar_colored_clusters1/Topic1 - - /DetectedObjects1/Topic1 - Splitter Ratio: 0.7603448033332825 - Tree Height: 746 - - 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: lidar -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - 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: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - 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 - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/ars408/lidar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 220; 138; 221 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_foreground_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/ars408/lidar_foreground_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: lidar_colored_clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07000000029802322 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/ars408/lidar_colored_clusters - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: lidar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/ars408/lidar_detection_markers - Value: true - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/front_center/debug/detected_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: radar_detections - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/ars408/radar_detection_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: radar_background_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/radar_background_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: tracking_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/ars408/tracking_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: matches_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: " /matches_markers" - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.39479711651802063 - Position: - X: -8.984928131103516 - Y: 0.34768491983413696 - Z: 6.239386081695557 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 0.0381561815738678 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000020200000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000052e0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1846 - X: 74 - Y: 0 diff --git a/sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml deleted file mode 100644 index f2f222f8..00000000 --- a/sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml +++ /dev/null @@ -1,191 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_tag_based_base_calibrator/rviz/x2.rviz b/sensor/extrinsic_tag_based_base_calibrator/rviz/x2.rviz deleted file mode 100644 index f39289dc..00000000 --- a/sensor/extrinsic_tag_based_base_calibrator/rviz/x2.rviz +++ /dev/null @@ -1,894 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /calibration markers1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 746 - - 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 - - /Current View1/Position1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: pandar_40p_front_pc -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.009999999776482582 - Reference Frame: - Value: false - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: pandar_40p_left_pc - 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/left_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: pandar_40p_right_pc - 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/right_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: pandar_40p_front_pc - 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/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 0.4000000059604645 - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: pandar_40p_rear_pc - 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/rear_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: calibration markers - Namespaces: - initial_base_link: true - initial_connections: false - initial_estimations: false - initial_ground_plane: false - optimized_base_link: true - optimized_connections: false - optimized_estimations: true - optimized_ground_plane: false - raw_detections: false - raw_detections_s0_e0: false - raw_detections_s0_e1: false - raw_detections_s0_e10: false - raw_detections_s0_e11: false - raw_detections_s0_e12: false - raw_detections_s0_e13: false - raw_detections_s0_e14: false - raw_detections_s0_e15: false - raw_detections_s0_e16: false - raw_detections_s0_e17: false - raw_detections_s0_e18: false - raw_detections_s0_e19: false - raw_detections_s0_e2: false - raw_detections_s0_e20: false - raw_detections_s0_e21: false - raw_detections_s0_e22: false - raw_detections_s0_e23: false - raw_detections_s0_e24: false - raw_detections_s0_e25: false - raw_detections_s0_e26: false - raw_detections_s0_e27: false - raw_detections_s0_e28: false - raw_detections_s0_e29: false - raw_detections_s0_e3: false - raw_detections_s0_e30: false - raw_detections_s0_e31: false - raw_detections_s0_e32: false - raw_detections_s0_e33: false - raw_detections_s0_e34: false - raw_detections_s0_e35: false - raw_detections_s0_e36: false - raw_detections_s0_e37: false - raw_detections_s0_e38: false - raw_detections_s0_e39: false - raw_detections_s0_e4: false - raw_detections_s0_e40: false - raw_detections_s0_e41: false - raw_detections_s0_e42: false - raw_detections_s0_e43: false - raw_detections_s0_e44: false - raw_detections_s0_e45: false - raw_detections_s0_e46: false - raw_detections_s0_e47: false - raw_detections_s0_e48: false - raw_detections_s0_e49: false - raw_detections_s0_e5: false - raw_detections_s0_e50: false - raw_detections_s0_e51: false - raw_detections_s0_e52: false - raw_detections_s0_e53: false - raw_detections_s0_e54: false - raw_detections_s0_e55: false - raw_detections_s0_e56: false - raw_detections_s0_e57: false - raw_detections_s0_e58: false - raw_detections_s0_e59: false - raw_detections_s0_e6: false - raw_detections_s0_e60: false - raw_detections_s0_e61: false - raw_detections_s0_e62: false - raw_detections_s0_e63: false - raw_detections_s0_e64: false - raw_detections_s0_e65: false - raw_detections_s0_e66: false - raw_detections_s0_e67: false - raw_detections_s0_e68: false - raw_detections_s0_e69: false - raw_detections_s0_e7: false - raw_detections_s0_e70: false - raw_detections_s0_e71: false - raw_detections_s0_e72: false - raw_detections_s0_e73: false - raw_detections_s0_e74: false - raw_detections_s0_e75: false - raw_detections_s0_e76: false - raw_detections_s0_e77: false - raw_detections_s0_e78: false - raw_detections_s0_e79: false - raw_detections_s0_e8: false - raw_detections_s0_e80: false - raw_detections_s0_e81: false - raw_detections_s0_e82: false - raw_detections_s0_e83: false - raw_detections_s0_e84: false - raw_detections_s0_e85: false - raw_detections_s0_e86: false - raw_detections_s0_e87: false - raw_detections_s0_e88: false - raw_detections_s0_e89: false - raw_detections_s0_e9: false - raw_detections_s0_e90: false - raw_detections_s0_e91: false - raw_detections_s0_e92: false - raw_detections_s0_e93: false - raw_detections_s0_e94: false - raw_detections_s0_e95: false - raw_detections_s0_e96: false - raw_detections_s0_e97: false - raw_detections_s0_e98: false - raw_detections_s0_e99: false - raw_detections_s1_e0: false - raw_detections_s1_e1: false - raw_detections_s1_e10: false - raw_detections_s1_e11: false - raw_detections_s1_e12: false - raw_detections_s1_e13: false - raw_detections_s1_e14: false - raw_detections_s1_e15: false - raw_detections_s1_e16: false - raw_detections_s1_e17: false - raw_detections_s1_e18: false - raw_detections_s1_e19: false - raw_detections_s1_e2: false - raw_detections_s1_e20: false - raw_detections_s1_e21: false - raw_detections_s1_e22: false - raw_detections_s1_e23: false - raw_detections_s1_e24: false - raw_detections_s1_e25: false - raw_detections_s1_e26: false - raw_detections_s1_e27: false - raw_detections_s1_e28: false - raw_detections_s1_e29: false - raw_detections_s1_e3: false - raw_detections_s1_e30: false - raw_detections_s1_e31: false - raw_detections_s1_e32: false - raw_detections_s1_e33: false - raw_detections_s1_e34: false - raw_detections_s1_e35: false - raw_detections_s1_e36: false - raw_detections_s1_e37: false - raw_detections_s1_e38: false - raw_detections_s1_e39: false - raw_detections_s1_e4: false - raw_detections_s1_e40: false - raw_detections_s1_e5: false - raw_detections_s1_e6: false - raw_detections_s1_e7: false - raw_detections_s1_e8: false - raw_detections_s1_e9: false - raw_detections_s2_e0: false - raw_detections_s2_e1: false - raw_detections_s2_e10: false - raw_detections_s2_e11: false - raw_detections_s2_e12: false - raw_detections_s2_e13: false - raw_detections_s2_e14: false - raw_detections_s2_e15: false - raw_detections_s2_e16: false - raw_detections_s2_e17: false - raw_detections_s2_e18: false - raw_detections_s2_e19: false - raw_detections_s2_e2: false - raw_detections_s2_e20: false - raw_detections_s2_e21: false - raw_detections_s2_e22: false - raw_detections_s2_e23: false - raw_detections_s2_e24: false - raw_detections_s2_e25: false - raw_detections_s2_e26: false - raw_detections_s2_e27: false - raw_detections_s2_e28: false - raw_detections_s2_e29: false - raw_detections_s2_e3: false - raw_detections_s2_e30: false - raw_detections_s2_e31: false - raw_detections_s2_e32: false - raw_detections_s2_e33: false - raw_detections_s2_e34: false - raw_detections_s2_e35: false - raw_detections_s2_e36: false - raw_detections_s2_e37: false - raw_detections_s2_e38: false - raw_detections_s2_e39: false - raw_detections_s2_e4: false - raw_detections_s2_e5: false - raw_detections_s2_e6: false - raw_detections_s2_e7: false - raw_detections_s2_e8: false - raw_detections_s2_e9: false - raw_detections_s3_e0: false - raw_detections_s3_e1: false - raw_detections_s3_e10: false - raw_detections_s3_e11: false - raw_detections_s3_e12: false - raw_detections_s3_e13: false - raw_detections_s3_e14: false - raw_detections_s3_e15: false - raw_detections_s3_e16: false - raw_detections_s3_e17: false - raw_detections_s3_e18: false - raw_detections_s3_e19: false - raw_detections_s3_e2: false - raw_detections_s3_e20: false - raw_detections_s3_e21: false - raw_detections_s3_e22: false - raw_detections_s3_e23: false - raw_detections_s3_e24: false - raw_detections_s3_e25: false - raw_detections_s3_e26: false - raw_detections_s3_e27: false - raw_detections_s3_e28: false - raw_detections_s3_e29: false - raw_detections_s3_e3: false - raw_detections_s3_e30: false - raw_detections_s3_e31: false - raw_detections_s3_e4: false - raw_detections_s3_e5: false - raw_detections_s3_e6: false - raw_detections_s3_e7: false - raw_detections_s3_e8: false - raw_detections_s3_e9: false - raw_detections_s4_e0: false - raw_detections_s4_e1: false - raw_detections_s4_e10: false - raw_detections_s4_e11: false - raw_detections_s4_e12: false - raw_detections_s4_e13: false - raw_detections_s4_e14: false - raw_detections_s4_e15: false - raw_detections_s4_e16: false - raw_detections_s4_e17: false - raw_detections_s4_e18: false - raw_detections_s4_e19: false - raw_detections_s4_e2: false - raw_detections_s4_e20: false - raw_detections_s4_e21: false - raw_detections_s4_e22: false - raw_detections_s4_e23: false - raw_detections_s4_e24: false - raw_detections_s4_e25: false - raw_detections_s4_e26: false - raw_detections_s4_e27: false - raw_detections_s4_e28: false - raw_detections_s4_e29: false - raw_detections_s4_e3: false - raw_detections_s4_e30: false - raw_detections_s4_e31: false - raw_detections_s4_e32: false - raw_detections_s4_e33: false - raw_detections_s4_e34: false - raw_detections_s4_e35: false - raw_detections_s4_e36: false - raw_detections_s4_e37: false - raw_detections_s4_e38: false - raw_detections_s4_e39: false - raw_detections_s4_e4: false - raw_detections_s4_e40: false - raw_detections_s4_e41: false - raw_detections_s4_e42: false - raw_detections_s4_e43: false - raw_detections_s4_e5: false - raw_detections_s4_e6: false - raw_detections_s4_e7: false - raw_detections_s4_e8: false - raw_detections_s4_e9: false - raw_detections_s5_e0: false - raw_detections_s5_e1: false - raw_detections_s5_e10: false - raw_detections_s5_e11: false - raw_detections_s5_e12: false - raw_detections_s5_e13: false - raw_detections_s5_e14: false - raw_detections_s5_e15: false - raw_detections_s5_e16: false - raw_detections_s5_e17: false - raw_detections_s5_e18: false - raw_detections_s5_e19: false - raw_detections_s5_e2: false - raw_detections_s5_e20: false - raw_detections_s5_e21: false - raw_detections_s5_e22: false - raw_detections_s5_e23: false - raw_detections_s5_e24: false - raw_detections_s5_e25: false - raw_detections_s5_e26: false - raw_detections_s5_e27: false - raw_detections_s5_e28: false - raw_detections_s5_e29: false - raw_detections_s5_e3: false - raw_detections_s5_e30: false - raw_detections_s5_e31: false - raw_detections_s5_e32: false - raw_detections_s5_e33: false - raw_detections_s5_e34: false - raw_detections_s5_e35: false - raw_detections_s5_e36: false - raw_detections_s5_e37: false - raw_detections_s5_e38: false - raw_detections_s5_e39: false - raw_detections_s5_e4: false - raw_detections_s5_e40: false - raw_detections_s5_e41: false - raw_detections_s5_e42: false - raw_detections_s5_e43: false - raw_detections_s5_e5: false - raw_detections_s5_e6: false - raw_detections_s5_e7: false - raw_detections_s5_e8: false - raw_detections_s5_e9: false - raw_detections_s6_e0: false - raw_detections_s6_e1: false - raw_detections_s6_e10: false - raw_detections_s6_e11: false - raw_detections_s6_e12: false - raw_detections_s6_e13: false - raw_detections_s6_e14: false - raw_detections_s6_e15: false - raw_detections_s6_e16: false - raw_detections_s6_e17: false - raw_detections_s6_e18: false - raw_detections_s6_e19: false - raw_detections_s6_e2: false - raw_detections_s6_e20: false - raw_detections_s6_e21: false - raw_detections_s6_e22: false - raw_detections_s6_e23: false - raw_detections_s6_e24: false - raw_detections_s6_e25: false - raw_detections_s6_e26: false - raw_detections_s6_e27: false - raw_detections_s6_e28: false - raw_detections_s6_e29: false - raw_detections_s6_e3: false - raw_detections_s6_e30: false - raw_detections_s6_e31: false - raw_detections_s6_e32: false - raw_detections_s6_e33: false - raw_detections_s6_e34: false - raw_detections_s6_e35: false - raw_detections_s6_e36: false - raw_detections_s6_e37: false - raw_detections_s6_e38: false - raw_detections_s6_e39: false - raw_detections_s6_e4: false - raw_detections_s6_e40: false - raw_detections_s6_e41: false - raw_detections_s6_e42: false - raw_detections_s6_e43: false - raw_detections_s6_e44: false - raw_detections_s6_e45: false - raw_detections_s6_e46: false - raw_detections_s6_e47: false - raw_detections_s6_e48: false - raw_detections_s6_e49: false - raw_detections_s6_e5: false - raw_detections_s6_e6: false - raw_detections_s6_e7: false - raw_detections_s6_e8: false - raw_detections_s6_e9: false - raw_detections_s7_e0: false - raw_detections_s7_e1: false - raw_detections_s7_e10: false - raw_detections_s7_e11: false - raw_detections_s7_e12: false - raw_detections_s7_e13: false - raw_detections_s7_e14: false - raw_detections_s7_e15: false - raw_detections_s7_e16: false - raw_detections_s7_e17: false - raw_detections_s7_e18: false - raw_detections_s7_e19: false - raw_detections_s7_e2: false - raw_detections_s7_e20: false - raw_detections_s7_e21: false - raw_detections_s7_e22: false - raw_detections_s7_e23: false - raw_detections_s7_e24: false - raw_detections_s7_e25: false - raw_detections_s7_e26: false - raw_detections_s7_e27: false - raw_detections_s7_e28: false - raw_detections_s7_e29: false - raw_detections_s7_e3: false - raw_detections_s7_e30: false - raw_detections_s7_e31: false - raw_detections_s7_e32: false - raw_detections_s7_e33: false - raw_detections_s7_e34: false - raw_detections_s7_e35: false - raw_detections_s7_e36: false - raw_detections_s7_e37: false - raw_detections_s7_e38: false - raw_detections_s7_e39: false - raw_detections_s7_e4: false - raw_detections_s7_e40: false - raw_detections_s7_e41: false - raw_detections_s7_e42: false - raw_detections_s7_e43: false - raw_detections_s7_e5: false - raw_detections_s7_e6: false - raw_detections_s7_e7: false - raw_detections_s7_e8: false - raw_detections_s7_e9: false - raw_detections_s8_e0: false - raw_detections_s8_e1: false - raw_detections_s8_e10: false - raw_detections_s8_e11: false - raw_detections_s8_e12: false - raw_detections_s8_e13: false - raw_detections_s8_e14: false - raw_detections_s8_e15: false - raw_detections_s8_e16: false - raw_detections_s8_e17: false - raw_detections_s8_e18: false - raw_detections_s8_e19: false - raw_detections_s8_e2: false - raw_detections_s8_e20: false - raw_detections_s8_e21: false - raw_detections_s8_e22: false - raw_detections_s8_e23: false - raw_detections_s8_e24: false - raw_detections_s8_e25: false - raw_detections_s8_e26: false - raw_detections_s8_e27: false - raw_detections_s8_e28: false - raw_detections_s8_e29: false - raw_detections_s8_e3: false - raw_detections_s8_e30: false - raw_detections_s8_e31: false - raw_detections_s8_e32: false - raw_detections_s8_e33: false - raw_detections_s8_e34: false - raw_detections_s8_e35: false - raw_detections_s8_e36: false - raw_detections_s8_e37: false - raw_detections_s8_e38: false - raw_detections_s8_e39: false - raw_detections_s8_e4: false - raw_detections_s8_e40: false - raw_detections_s8_e41: false - raw_detections_s8_e42: false - raw_detections_s8_e43: false - raw_detections_s8_e44: false - raw_detections_s8_e45: false - raw_detections_s8_e46: false - raw_detections_s8_e47: false - raw_detections_s8_e48: false - raw_detections_s8_e49: false - raw_detections_s8_e5: false - raw_detections_s8_e50: false - raw_detections_s8_e51: false - raw_detections_s8_e52: false - raw_detections_s8_e53: false - raw_detections_s8_e54: false - raw_detections_s8_e6: false - raw_detections_s8_e7: false - raw_detections_s8_e8: false - raw_detections_s8_e9: false - raw_detections_s9_e0: false - raw_detections_s9_e1: false - raw_detections_s9_e10: false - raw_detections_s9_e11: false - raw_detections_s9_e12: false - raw_detections_s9_e13: false - raw_detections_s9_e14: false - raw_detections_s9_e15: false - raw_detections_s9_e16: false - raw_detections_s9_e17: false - raw_detections_s9_e18: false - raw_detections_s9_e19: false - raw_detections_s9_e2: false - raw_detections_s9_e20: false - raw_detections_s9_e21: false - raw_detections_s9_e22: false - raw_detections_s9_e23: false - raw_detections_s9_e24: false - raw_detections_s9_e25: false - raw_detections_s9_e26: false - raw_detections_s9_e27: false - raw_detections_s9_e28: false - raw_detections_s9_e29: false - raw_detections_s9_e3: false - raw_detections_s9_e30: false - raw_detections_s9_e31: false - raw_detections_s9_e32: false - raw_detections_s9_e33: false - raw_detections_s9_e34: false - raw_detections_s9_e35: false - raw_detections_s9_e36: false - raw_detections_s9_e37: false - raw_detections_s9_e38: false - raw_detections_s9_e39: false - raw_detections_s9_e4: false - raw_detections_s9_e40: false - raw_detections_s9_e41: false - raw_detections_s9_e42: false - raw_detections_s9_e43: false - raw_detections_s9_e44: false - raw_detections_s9_e45: false - raw_detections_s9_e46: false - raw_detections_s9_e47: false - raw_detections_s9_e48: false - raw_detections_s9_e49: false - raw_detections_s9_e5: false - raw_detections_s9_e50: false - raw_detections_s9_e51: false - raw_detections_s9_e52: false - raw_detections_s9_e53: false - raw_detections_s9_e54: false - raw_detections_s9_e55: false - raw_detections_s9_e56: false - raw_detections_s9_e57: false - raw_detections_s9_e6: false - raw_detections_s9_e7: false - raw_detections_s9_e8: false - raw_detections_s9_e9: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /markers - Value: true - - Alpha: 0.5 - Cell Size: 0.10000000149011612 - Class: rviz_default_plugins/Grid - Color: 171; 171; 171 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: base_link_grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 200 - Reference Frame: base_link - Value: false - - Alpha: 0.5 - Cell Size: 0.10000000149011612 - Class: rviz_default_plugins/Grid - Color: 0; 255; 127 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: estimated_base_link_grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 80 - Reference Frame: estimated_base_link - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: base link - Radius: 0.009999999776482582 - Reference Frame: base_link - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1 - Name: estimated_base link - Radius: 0.009999999776482582 - Reference Frame: estimated_base_link - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: pandar_40p_left - Radius: 0.009999999776482582 - Reference Frame: pandar_40p_left - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: pandar_40p_left_detections - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /left_upper/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: pandar_40p_right_detections - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /right_upper/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: pandar_40p_front_detections - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_lower/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: pandar_40p_rear_detections - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_lower/lidartag/tag_frame - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_left - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.5647963285446167 - Position: - X: 2.4623332023620605 - Y: 0.46027514338493347 - Z: 15.705755233764648 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 1.5453146696090698 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000027600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1846 - X: 74 - Y: 0 diff --git a/sensor/extrinsic_tag_based_base_calibrator/rviz/xx1.rviz b/sensor/extrinsic_tag_based_base_calibrator/rviz/xx1.rviz deleted file mode 100644 index d5320dcd..00000000 --- a/sensor/extrinsic_tag_based_base_calibrator/rviz/xx1.rviz +++ /dev/null @@ -1,563 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /calibration markers1/Namespaces1 - - /base_link_grid1 - Splitter Ratio: 0.5 - Tree Height: 746 - - 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 - - /Current View1/Position1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: left -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.009999999776482582 - Reference Frame: - Value: false - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: top - 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/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: left - 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/left/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: right - 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/right/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: rear - 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/rear/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: calibration markers - Namespaces: - initial_base_link: true - initial_connections: false - initial_estimations: true - initial_ground_plane: true - optimized_base_link: true - optimized_connections: false - optimized_estimations: true - optimized_ground_plane: true - raw_detections: true - raw_detections_s0_e0: false - raw_detections_s0_e1: true - raw_detections_s0_e10: false - raw_detections_s0_e11: false - raw_detections_s0_e12: false - raw_detections_s0_e13: false - raw_detections_s0_e14: false - raw_detections_s0_e15: false - raw_detections_s0_e16: false - raw_detections_s0_e17: false - raw_detections_s0_e18: false - raw_detections_s0_e19: false - raw_detections_s0_e2: false - raw_detections_s0_e20: false - raw_detections_s0_e21: false - raw_detections_s0_e22: false - raw_detections_s0_e23: false - raw_detections_s0_e24: false - raw_detections_s0_e25: false - raw_detections_s0_e26: false - raw_detections_s0_e27: false - raw_detections_s0_e28: false - raw_detections_s0_e29: false - raw_detections_s0_e3: false - raw_detections_s0_e30: false - raw_detections_s0_e31: false - raw_detections_s0_e32: false - raw_detections_s0_e33: false - raw_detections_s0_e34: false - raw_detections_s0_e35: false - raw_detections_s0_e36: false - raw_detections_s0_e37: false - raw_detections_s0_e38: false - raw_detections_s0_e39: false - raw_detections_s0_e4: false - raw_detections_s0_e5: false - raw_detections_s0_e6: false - raw_detections_s0_e7: false - raw_detections_s0_e8: false - raw_detections_s0_e9: false - raw_detections_s1_e0: false - raw_detections_s1_e1: false - raw_detections_s1_e10: false - raw_detections_s1_e11: false - raw_detections_s1_e12: false - raw_detections_s1_e13: false - raw_detections_s1_e14: false - raw_detections_s1_e15: false - raw_detections_s1_e16: false - raw_detections_s1_e17: false - raw_detections_s1_e18: false - raw_detections_s1_e19: false - raw_detections_s1_e2: false - raw_detections_s1_e20: false - raw_detections_s1_e21: false - raw_detections_s1_e22: false - raw_detections_s1_e23: false - raw_detections_s1_e24: false - raw_detections_s1_e3: false - raw_detections_s1_e4: false - raw_detections_s1_e5: false - raw_detections_s1_e6: false - raw_detections_s1_e7: false - raw_detections_s1_e8: false - raw_detections_s1_e9: false - raw_detections_s2_e0: false - raw_detections_s2_e1: false - raw_detections_s2_e10: false - raw_detections_s2_e11: false - raw_detections_s2_e12: false - raw_detections_s2_e13: false - raw_detections_s2_e14: false - raw_detections_s2_e15: false - raw_detections_s2_e16: false - raw_detections_s2_e17: false - raw_detections_s2_e18: false - raw_detections_s2_e19: false - raw_detections_s2_e2: false - raw_detections_s2_e3: false - raw_detections_s2_e4: false - raw_detections_s2_e5: false - raw_detections_s2_e6: false - raw_detections_s2_e7: false - raw_detections_s2_e8: false - raw_detections_s2_e9: false - raw_detections_s3_e0: false - raw_detections_s3_e1: false - raw_detections_s3_e10: false - raw_detections_s3_e11: false - raw_detections_s3_e12: false - raw_detections_s3_e13: false - raw_detections_s3_e14: false - raw_detections_s3_e15: false - raw_detections_s3_e2: false - raw_detections_s3_e3: false - raw_detections_s3_e4: false - raw_detections_s3_e5: false - raw_detections_s3_e6: false - raw_detections_s3_e7: false - raw_detections_s3_e8: false - raw_detections_s3_e9: false - raw_detections_s4_e0: false - raw_detections_s4_e1: false - raw_detections_s4_e10: false - raw_detections_s4_e11: false - raw_detections_s4_e12: false - raw_detections_s4_e13: false - raw_detections_s4_e14: false - raw_detections_s4_e15: false - raw_detections_s4_e16: false - raw_detections_s4_e17: false - raw_detections_s4_e18: false - raw_detections_s4_e19: false - raw_detections_s4_e2: false - raw_detections_s4_e20: false - raw_detections_s4_e21: false - raw_detections_s4_e22: false - raw_detections_s4_e23: false - raw_detections_s4_e24: false - raw_detections_s4_e25: false - raw_detections_s4_e26: false - raw_detections_s4_e3: false - raw_detections_s4_e4: false - raw_detections_s4_e5: false - raw_detections_s4_e6: false - raw_detections_s4_e7: false - raw_detections_s4_e8: false - raw_detections_s4_e9: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /markers - Value: true - - Alpha: 0.5 - Cell Size: 0.10000000149011612 - Class: rviz_default_plugins/Grid - Color: 171; 171; 171 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: base_link_grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 80 - Reference Frame: base_link - Value: true - - Alpha: 0.5 - Cell Size: 0.10000000149011612 - Class: rviz_default_plugins/Grid - Color: 0; 255; 127 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: estimated_base_link_grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 80 - Reference Frame: estimated_base_link - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: base link - Radius: 0.009999999776482582 - Reference Frame: base_link - Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: estimated_base link - Radius: 0.009999999776482582 - Reference Frame: estimated_base_link - Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: velodyne_top - Radius: 0.009999999776482582 - Reference Frame: velodyne_top - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: rear_detections_frames - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: left_detections_frames - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /left/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: right_detections_frames - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /right/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: top_detections_frames - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: rear_detections_ids - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear/lidartag/id_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: left_detections_ids - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /left/lidartag/id_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: right_detections_ids - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /right/lidartag/id_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: top_detections_ids - Namespaces: - Text0: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top/lidartag/id_markers - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6899994015693665 - Position: - X: 0.07426369190216064 - Y: -8.048012733459473 - Z: 4.948040962219238 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 1.6139578819274902 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001cd00000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044e0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1846 - X: 74 - Y: 0 diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/main.cpp b/sensor/extrinsic_tag_based_base_calibrator/src/main.cpp deleted file mode 100644 index 8e4e92f0..00000000 --- a/sensor/extrinsic_tag_based_base_calibrator/src/main.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2023 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. - -#include -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::NodeOptions node_options; - std::shared_ptr node = - std::make_shared( - node_options); - executor.add_node(node); - executor.spin(); - - rclcpp::shutdown(); - - return 0; -} diff --git a/sensor/extrinsic_tag_based_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_calibrator/launch/calibrator.launch.xml deleted file mode 100644 index 7a2a2a7e..00000000 --- a/sensor/extrinsic_tag_based_calibrator/launch/calibrator.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml b/sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml deleted file mode 100755 index 93022c15..00000000 --- a/sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz deleted file mode 100644 index 6bab776f..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera0/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera0/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/right_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera0/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_right - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5747967958450317 - Position: - X: -7.506234169006348 - Y: 0.7994357943534851 - Z: 7.922142028808594 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.282007217407227 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz deleted file mode 100644 index e08c4580..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera0/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera0/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.385 - Position: - X: 0.0 - Y: 8.0 - Z: 5.0 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.71 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz deleted file mode 100644 index 192d09da..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz +++ /dev/null @@ -1,1222 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.5865139961242676 - Tree Height: 803 - - 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: Points of Interest -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera1/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera1/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/right_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text0: true - Text1: true - Text2: true - Text3: true - Text4: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_apriltag_frame: true - active_apriltag_id: true - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - calibration_zone: true - converged_apriltag_frame: true - converged_apriltag_id: true - converged_center: true - converged_lidartag_frame: true - converged_lidartag_id: true - lidartag_ccs: false - lidartag_ccs_0_corner_id: false - lidartag_ccs_1_corner_id: false - lidartag_ccs_2_corner_id: false - lidartag_ccs_3_corner_id: false - lidartag_ccs_4_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_0_corner_id: false - lidartag_ics_1_corner_id: false - lidartag_ics_2_corner_id: false - lidartag_ics_3_corner_id: false - lidartag_ics_4_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera1/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_right - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5747967958450317 - Position: - X: -8.07607650756836 - Y: 0.8628609776496887 - Z: 7.040873050689697 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.252006530761719 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650100000383000000160000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz deleted file mode 100644 index 72f7f84a..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera1/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera1/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera1/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.52 - Position: - X: 0.0 - Y: -5.0 - Z: 4.3 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 1.57 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz deleted file mode 100644 index d02bfc84..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz +++ /dev/null @@ -1,1222 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.5865139961242676 - Tree Height: 803 - - 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: Points of Interest -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera2/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera2/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/right_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text0: true - Text1: true - Text2: true - Text3: true - Text4: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_apriltag_frame: true - active_apriltag_id: true - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - calibration_zone: true - converged_apriltag_frame: true - converged_apriltag_id: true - converged_center: true - converged_lidartag_frame: true - converged_lidartag_id: true - lidartag_ccs: false - lidartag_ccs_0_corner_id: false - lidartag_ccs_1_corner_id: false - lidartag_ccs_2_corner_id: false - lidartag_ccs_3_corner_id: false - lidartag_ccs_4_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_0_corner_id: false - lidartag_ics_1_corner_id: false - lidartag_ics_2_corner_id: false - lidartag_ics_3_corner_id: false - lidartag_ics_4_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera2/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_right - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5747967958450317 - Position: - X: -8.07607650756836 - Y: 0.8628609776496887 - Z: 7.040873050689697 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.252006530761719 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650100000383000000160000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz deleted file mode 100644 index c0df02dd..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera2/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera2/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera2/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7 - Position: - X: -3.2 - Y: 1.6 - Z: 5.0 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 5.87 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz deleted file mode 100644 index a910a521..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz +++ /dev/null @@ -1,1222 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.5865139961242676 - Tree Height: 803 - - 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: Points of Interest -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera3/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera3/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/rear_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text0: true - Text1: true - Text2: true - Text3: true - Text4: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_apriltag_frame: true - active_apriltag_id: true - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - calibration_zone: true - converged_apriltag_frame: true - converged_apriltag_id: true - converged_center: true - converged_lidartag_frame: true - converged_lidartag_id: true - lidartag_ccs: false - lidartag_ccs_0_corner_id: false - lidartag_ccs_1_corner_id: false - lidartag_ccs_2_corner_id: false - lidartag_ccs_3_corner_id: false - lidartag_ccs_4_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_0_corner_id: false - lidartag_ics_1_corner_id: false - lidartag_ics_2_corner_id: false - lidartag_ics_3_corner_id: false - lidartag_ics_4_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/camera3/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_rear - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5747967958450317 - Position: - X: -8.07607650756836 - Y: 0.8628609776496887 - Z: 7.040873050689697 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.252006530761719 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650100000383000000160000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz deleted file mode 100644 index 69433bbb..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera3/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera3/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera3/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.385 - Position: - X: 0.0 - Y: 8.0 - Z: 5.0 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.71 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz deleted file mode 100644 index 1714ac3e..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz +++ /dev/null @@ -1,1222 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.5865139961242676 - Tree Height: 803 - - 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: Points of Interest -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera4/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera4/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text0: true - Text1: true - Text2: true - Text3: true - Text4: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_apriltag_frame: true - active_apriltag_id: true - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - calibration_zone: true - converged_apriltag_frame: true - converged_apriltag_id: true - converged_center: true - converged_lidartag_frame: true - converged_lidartag_id: true - lidartag_ccs: false - lidartag_ccs_0_corner_id: false - lidartag_ccs_1_corner_id: false - lidartag_ccs_2_corner_id: false - lidartag_ccs_3_corner_id: false - lidartag_ccs_4_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_0_corner_id: false - lidartag_ics_1_corner_id: false - lidartag_ics_2_corner_id: false - lidartag_ics_3_corner_id: false - lidartag_ics_4_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera4/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_left - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5747967958450317 - Position: - X: -8.07607650756836 - Y: 0.8628609776496887 - Z: 7.040873050689697 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.252006530761719 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650100000383000000160000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz deleted file mode 100644 index bfa1af16..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera4/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera4/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera4/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.385 - Position: - X: 0.0 - Y: 8.0 - Z: 5.0 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.71 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz deleted file mode 100644 index c9ac66c6..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz +++ /dev/null @@ -1,1222 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.5865139961242676 - Tree Height: 803 - - 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: Points of Interest -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera5/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera5/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text0: true - Text1: true - Text2: true - Text3: true - Text4: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_apriltag_frame: true - active_apriltag_id: true - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - calibration_zone: true - converged_apriltag_frame: true - converged_apriltag_id: true - converged_center: true - converged_lidartag_frame: true - converged_lidartag_id: true - lidartag_ccs: false - lidartag_ccs_0_corner_id: false - lidartag_ccs_1_corner_id: false - lidartag_ccs_2_corner_id: false - lidartag_ccs_3_corner_id: false - lidartag_ccs_4_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_0_corner_id: false - lidartag_ics_1_corner_id: false - lidartag_ics_2_corner_id: false - lidartag_ics_3_corner_id: false - lidartag_ics_4_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera5/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_left - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5747967958450317 - Position: - X: -8.07607650756836 - Y: 0.8628609776496887 - Z: 7.040873050689697 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.252006530761719 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650100000383000000160000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz deleted file mode 100644 index 1b124fdb..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera5/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera5/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera5/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.385 - Position: - X: 0.0 - Y: 8.0 - Z: 5.0 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.71 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz deleted file mode 100644 index a7326ed1..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz +++ /dev/null @@ -1,1197 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.5865139961242676 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera6/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera6/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text0: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - calibration_status: true - lidartag_ccs: false - lidartag_ccs_0_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_0_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/camera6/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_front - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.37479695677757263 - Position: - X: 0.10891041159629822 - Y: 8.847746849060059 - Z: 5.501904487609863 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.711989402770996 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650100000383000000160000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera7_pandar_40p_left.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera7_pandar_40p_left.rviz deleted file mode 100644 index 21e58b9e..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera7_pandar_40p_left.rviz +++ /dev/null @@ -1,1222 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.5865139961242676 - Tree Height: 803 - - 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: Points of Interest -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/camera7/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera7/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/left_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text0: true - Text1: true - Text2: true - Text3: true - Text4: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_apriltag_frame: true - active_apriltag_id: true - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - calibration_zone: true - converged_apriltag_frame: true - converged_apriltag_id: true - converged_center: true - converged_lidartag_frame: true - converged_lidartag_id: true - lidartag_ccs: false - lidartag_ccs_0_corner_id: false - lidartag_ccs_1_corner_id: false - lidartag_ccs_2_corner_id: false - lidartag_ccs_3_corner_id: false - lidartag_ccs_4_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_0_corner_id: false - lidartag_ics_1_corner_id: false - lidartag_ics_2_corner_id: false - lidartag_ics_3_corner_id: false - lidartag_ics_4_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /top_unit/top_unit_base_link/camera7/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_left - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5747967958450317 - Position: - X: -8.07607650756836 - Y: 0.8628609776496887 - Z: 7.040873050689697 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.252006530761719 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d0061006700650100000383000000160000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz b/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz deleted file mode 100644 index 0c9824d9..00000000 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz +++ /dev/null @@ -1,1203 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Image1/Topic1 - - /(Optimized) Binary Transformed Points1/Topic1 - - "/Cluster info: detail code1/Topic1" - - "/Cluster info: detail code1/Namespaces1" - - /Marker1/Topic1 - - /Marker2/Topic1 - - /Marker3/Topic1 - - /Marker5/Topic1 - - /Marker6/Topic1 - - /Marker7/Topic1 - - /Marker8/Topic1 - - /Tag calib markers (filtered)1 - - /Tag calib markers (filtered)1/Namespaces1 - Splitter Ratio: 0.6812933087348938 - Tree Height: 803 - - 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: Raw Pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - 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: 20 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 0.13500000536441803 - Class: rviz_default_plugins/Grid - Color: 226; 226; 226 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid Template - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: YZ - Plane Cell Count: 6 - Reference Frame: - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/camera/traffic_light_left_camera/image_raw - Value: false - - Class: rviz_default_plugins/Camera - Enabled: false - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/traffic_light_left_camera/image_raw - Value: false - Visibility: - (Before Transformed) Edge Pointcloud: true - (Optimized) Binary Transformed Points: true - (Optimized) Transformed Point: true - Axes: true - Boundary Points: true - "Cluster info: detail code": true - "Cluster info: size": true - Clusters: true - Colored Cluster: true - Estimated Corners (PCA): true - Filled Cluster B&W: true - Filled Clusters: true - Grid: true - Grid Template: true - ID: true - Image: true - Initial Corners: true - Initial Transformed Points: true - Initial guess Corners: true - Intersection Markers: true - Marker: true - MarkerArray (Unused): true - PointCloud2: true - Points of Interest: true - Raw Pointcloud: true - Tag Frame: true - Tag calib markers (filtered): true - Tag calib markers (unfiltered): true - Template Frame: true - Template Points: true - Value: true - edges1: true - edges2: true - edges3: true - edges4: true - Zoom Factor: 1 - - 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Raw Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 78 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points of Interest - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/whole_edged_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 49 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/cluster_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.20000000298023224 - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 89 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: Filled Clusters - Position Transformer: XYZ - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/detected_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Clusters - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/boundary_marker - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Filled Cluster B&W - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/cluster_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 77 - Min Color: 0; 0; 0 - Min Intensity: 27 - Name: Boundary Points - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/boundary_pts - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Estimated Corners (PCA) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/transformed_points_tag - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Initial guess Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 93 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/initial_template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Tag Frame - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/tag_frame - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ID - Namespaces: - Text-1: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/id_markers - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 98 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: (Optimized) Transformed Point - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/template_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 95 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Optimized) Binary Transformed Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/template_points_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 200 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: Template Points - 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: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/associated_pattern_3d - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Template Frame - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/ideal_frame - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray (Unused) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/detail_valid_marker - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: (Before Transformed) Edge Pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.029999999329447746 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/before_transformed_edge_pc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/intesection_markers - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/lidartag_cluster_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/lidartag_cluster_edge_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: detail code" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/cluster_buff_index_number_markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: "Cluster info: size" - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/cluster_buff_points_size_markers - Value: false - - 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: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Colored Cluster - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/colored_cluster_buff - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/top_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/top_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/left_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/left_boundary_corner - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 50 - Min Color: 0; 0; 0 - Min Intensity: 50 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/transformed_points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/down_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/down_boundary_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/right_corner - Value: false - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/right_boundary_corner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tag calib markers (unfiltered) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/current_projections - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Tag calib markers (filtered) - Namespaces: - active_center: true - active_lidartag_frame: true - active_lidartag_id: true - active_lidartag_status: true - apriltag_0_corner_id_ccs: false - apriltag_0_corner_id_ics: false - apriltag_ccs: false - apriltag_ics: false - apriltag_id_ics: false - calibration_status: true - lidartag_ccs: false - lidartag_ccs_-1_corner_id: false - lidartag_ccs_id: false - lidartag_ics: false - lidartag_ics_-1_corner_id: false - lidartag_ics_id: false - lidartag_lcs: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/filtered_projections - 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: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/edge_group_1 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/edge_group_2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/edge_group_3 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: edges4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/edge_group_4 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial Corners - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/traffic_light_left_camera/camera_link/lidartag/initial_corners - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: velodyne_top - 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/FPS - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.385 - Position: - X: 0.0 - Y: 8.0 - Z: 5.0 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 4.71 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b30000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000002800fffffffb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000002800fffffffb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004790000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/sensor/extrinsic_ground_plane_calibrator/CMakeLists.txt b/sensor/ground_plane_calibrator/CMakeLists.txt similarity index 61% rename from sensor/extrinsic_ground_plane_calibrator/CMakeLists.txt rename to sensor/ground_plane_calibrator/CMakeLists.txt index ea62c3eb..5d02746d 100644 --- a/sensor/extrinsic_ground_plane_calibrator/CMakeLists.txt +++ b/sensor/ground_plane_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_ground_plane_calibrator) +project(ground_plane_calibrator) find_package(autoware_cmake REQUIRED) @@ -12,12 +12,12 @@ ament_export_include_directories( # COMPILE THE SOURCE #======================================================================== -ament_auto_add_executable(extrinsic_ground_plane_calibrator - src/extrinsic_ground_plane_calibrator.cpp +ament_auto_add_executable(ground_plane_calibrator + src/ground_plane_calibrator.cpp src/main.cpp ) -target_link_libraries(extrinsic_ground_plane_calibrator +target_link_libraries(ground_plane_calibrator ) ament_auto_package( diff --git a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp b/sensor/ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp similarity index 52% rename from sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp rename to sensor/ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp index fde4a276..44ac2406 100644 --- a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp +++ b/sensor/ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,41 +12,61 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ -#define EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ +#ifndef GROUND_PLANE_CALIBRATOR__GROUND_PLANE_CALIBRATOR_HPP_ +#define GROUND_PLANE_CALIBRATOR__GROUND_PLANE_CALIBRATOR_HPP_ -#define PCL_NO_PRECOMPILE #include -#include +#include #include #include #include +#include #include +#include +#include #include #include -#include #include #include +#include #include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#else -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#endif - #include #include #include #include +#include #include +namespace ground_plane_calibrator +{ + using PointType = pcl::PointXYZ; +using tier4_ground_plane_utils::GroundPlaneExtractorParameters; + +/** + * A base-lidar calibrator. + * + * This calibrator assumes that the area around the vehicle consists on a flat surface and consist + * essentially on a plane estimation algorithm, + * + * Once the plane has been estimated, the create an arbitrary coordinate system in the estimated + * ground (gcs = ground coordinate system). Then, we proceed to estimate the initial base link pose + * in gcs, and proceed to project it into the new "ground", that is dropping the z component. Once + * that has been done, we can recompute the "calibrated" base lidar transform. + * + * Note: Although the result of this algorithm is a full 3D pose, not all the parameters were really + * calibrated. With only the ground, we can only calibrate roll, pitch, and z. That is despite the + * fact that the initial and output transforms may have different x, y, and yaw values. This is due + * to the fact once we obtain the ground plane, we only know for certain the normal and distance of + * the plane with respect to the lidar. All other values and transformation are derived from the + * initial base lidar calibration + */ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node { @@ -54,6 +74,13 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node explicit ExtrinsicGroundPlaneCalibrator(const rclcpp::NodeOptions & options); protected: + /*! + * External interface to start the calibration process and retrieve the result. + * The call gets blocked until the calibration finishes + * + * @param request An empty service request + * @param response A vector of calibration results + */ void requestReceivedCallback( const std::shared_ptr request, const std::shared_ptr response); @@ -66,36 +93,17 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node /*! * Checks that all the needed tfs are available - * @retval wether or not all the needed tfs are available + * @return wether or not all the needed tfs are available */ bool checkInitialTransforms(); - /*! - * Extracts the ground plane from a pointcloud - * @param[in] pointcloud the input pointcloud - * @param[in] model the estimated ground plane model - * @param[in] inliers the inliers of the current estimated model - * @retval wether or not th calibration plane was found - */ - bool extractGroundPlane( - pcl::PointCloud::Ptr & pointcloud, Eigen::Vector4d & model, - pcl::PointCloud::Ptr & inliers); - - /*! - * Computes the fitting error of an estimated model and the initial one - * @param[in] estimated_model the estimated model - * @param[in] inliers the inliers of the current estimated model - */ - void evaluateModels( - const Eigen::Vector4d & estimated_model, pcl::PointCloud::Ptr inliers) const; - /*! * Filter individual calibration plane estimations and accumulate the inliers for a final * regression * @param[in] estimated_model the estimated model * @param[in] inliers the inliers of the current estimated model */ - void filterCalibration( + void filterGroundModelEstimation( const Eigen::Vector4d & estimated_model, pcl::PointCloud::Ptr inliers); /*! @@ -129,69 +137,14 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node */ void publishTf(const Eigen::Vector4d & ground_plane_model); - /*! - * Computes a plane model given a pose. - * The normal of the plane is given by the z-axis of the rotation of the pose - * @param[in] pointcloud Point cloud to crop - * @param[in] max_range Range to crop the pointcloud to - * @retval the plane model - */ - Eigen::Vector4d poseToPlaneModel(const Eigen::Isometry3d & pose) const; - - /*! - * Compute a pose from a plane model a*x + b*y +c*z +d = 0 - * The pose lies has its origin on the z-projection of the plane - * @param[in] model Point cloud to crop - * @retval the plane pose - */ - Eigen::Isometry3d modelPlaneToPose(const Eigen::Vector4d & model) const; - - /*! - * Refine a lidar-base pose given an estimated ground plane - * Projects the initial base lidar pose into the ground plane. - * @param[in] base_lidar_pose Initial base lidar pose - * @param[in] ground_plane_model ground plane model - * @retval the refined base lidar pose - */ - Eigen::Isometry3d refineBaseLidarPose( - const Eigen::Isometry3d & base_lidar_pose, const Eigen::Vector4d & model) const; - - /*! - * Removes the point that are consistent with an input plane from the pointcloud - * @param[in] input_pointcloud the pointcloud to filter - * @param[in] outlier_model the model that represents the outliers - * @param[in] outlier_tolerance the tolerance with which a point is still considered an outlier - * @retval the refined base lidar pose - */ - pcl::PointCloud::Ptr removeOutliers( - pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, - double outlier_tolerance) const; - // Parameters - // We perform base-lidar pose estimation but the output are frames in between - // base -> parent -> child -> lidar std::string base_frame_; - std::string parent_frame_; - std::string child_frame_; + std::string lidar_frame_; + GroundPlaneExtractorParameters ground_plane_extractor_parameters_; double marker_size_; - bool use_crop_box_filter_; - double crop_box_min_x_; - double crop_box_min_y_; - double crop_box_min_z_; - double crop_box_max_x_; - double crop_box_max_y_; - double crop_box_max_z_; - bool remove_outliers_; - double remove_outlier_tolerance_; - bool use_pca_rough_normal_; - double max_inlier_distance_; - int min_plane_points_; - int min_plane_points_percentage_; - double max_cos_distance_; - int max_iterations_; bool verbose_; - bool broadcast_calibration_tf_; + bool overwrite_xy_yaw_; bool filter_estimations_; double initial_angle_cov_; double initial_translation_cov_; @@ -222,34 +175,24 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node // ROS Data std_msgs::msg::Header header_; - std::string lidar_frame_; // Initial tfs comparable with the one with our method - geometry_msgs::msg::Transform initial_base_to_lidar_msg_; - tf2::Transform initial_base_to_lidar_tf2_; - Eigen::Isometry3d initial_base_to_lidar_eigen_; + geometry_msgs::msg::TransformStamped initial_base_to_lidar_transform_msg_; + Eigen::Isometry3d initial_base_to_lidar_transform_; - // Other tfs to calculate the complete chain. There are constant for our purposes - geometry_msgs::msg::Transform base_to_parent_msg_; - tf2::Transform base_to_parent_tf2_; - Eigen::Isometry3d base_to_parent_eigen_; + Eigen::Isometry3d calibrated_base_to_lidar_transform_; - geometry_msgs::msg::Transform child_to_lidar_msg_; - tf2::Transform child_to_lidar_tf2_; - Eigen::Isometry3d child_to_lidar_eigen_; - - geometry_msgs::msg::Pose output_parent_to_child_msg_; - Eigen::Isometry3d output_parent_to_child_eigen_; - - bool got_initial_transform_; - bool broadcast_tf_; - bool calibration_done_; + bool got_initial_transform_{false}; + bool received_request_{false}; + bool calibration_done_{false}; // Filtering KalmanFilter kalman_filter_; - bool first_observation_; + bool first_observation_{true}; RingBuffer::Ptr> inlier_observations_; std::vector outlier_models_; }; -#endif // EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ +} // namespace ground_plane_calibrator + +#endif // GROUND_PLANE_CALIBRATOR__GROUND_PLANE_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/utils.hpp b/sensor/ground_plane_calibrator/include/ground_plane_calibrator/utils.hpp similarity index 84% rename from sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/utils.hpp rename to sensor/ground_plane_calibrator/include/ground_plane_calibrator/utils.hpp index 166ef753..fa2ecb10 100644 --- a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/utils.hpp +++ b/sensor/ground_plane_calibrator/include/ground_plane_calibrator/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_GROUND_PLANE_CALIBRATOR__UTILS_HPP_ -#define EXTRINSIC_GROUND_PLANE_CALIBRATOR__UTILS_HPP_ +#ifndef GROUND_PLANE_CALIBRATOR__UTILS_HPP_ +#define GROUND_PLANE_CALIBRATOR__UTILS_HPP_ #include @@ -43,4 +43,4 @@ class RingBuffer int max_size_{0}; }; -#endif // EXTRINSIC_GROUND_PLANE_CALIBRATOR__UTILS_HPP_ +#endif // GROUND_PLANE_CALIBRATOR__UTILS_HPP_ diff --git a/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml b/sensor/ground_plane_calibrator/launch/calibrator.launch.xml similarity index 53% rename from sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml rename to sensor/ground_plane_calibrator/launch/calibrator.launch.xml index 06b74ee7..4d5bbf87 100644 --- a/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml +++ b/sensor/ground_plane_calibrator/launch/calibrator.launch.xml @@ -1,10 +1,22 @@ - + + - - + + + + + + + + + + + + + @@ -15,23 +27,35 @@ - + + - + + + + - - + - - + + - - - - - + + + + + + + + + + + + + + @@ -49,17 +73,10 @@ - - - - - - - - - - - + + + + diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/package.xml b/sensor/ground_plane_calibrator/package.xml old mode 100755 new mode 100644 similarity index 87% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/package.xml rename to sensor/ground_plane_calibrator/package.xml index b2dfffe4..65738128 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/package.xml +++ b/sensor/ground_plane_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_lidar_to_lidar_2d_calibrator + ground_plane_calibrator 0.0.1 - The extrinsic_lidar_to_lidar_2d_calibrator package + The ground_plane_calibrator package Kenzo Lobos Tsunekawa BSD @@ -28,6 +28,7 @@ tf2_ros tier4_autoware_utils tier4_calibration_msgs + tier4_ground_plane_utils visualization_msgs diff --git a/sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz b/sensor/ground_plane_calibrator/rviz/default.rviz similarity index 64% rename from sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz rename to sensor/ground_plane_calibrator/rviz/default.rviz index e7ad85a9..69a218db 100644 --- a/sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz +++ b/sensor/ground_plane_calibrator/rviz/default.rviz @@ -6,12 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /top1/Topic1 - - /initial_base_link1 - - /top_inliers1 - - /top_inliers1/Topic1 + - /lidar1/Topic1 Splitter Ratio: 0.500627338886261 - Tree Height: 719 + Tree Height: 746 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -30,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: top + SyncSource: lidar Visualization Manager: Class: "" Displays: @@ -67,10 +64,10 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 255 + Max Intensity: 207 Min Color: 0; 0; 0 Min Intensity: 0 - Name: top + Name: lidar Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -82,57 +79,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw + Value: /pointcloud Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: base_link - Radius: 0.10000000149011612 - Reference Frame: base_link - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1 - Name: initial_base_link - Radius: 0.10000000149011612 - Reference Frame: initial_base_link - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: velodyne_top - Radius: 0.10000000149011612 - Reference Frame: velodyne_top - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1.5 - Name: ground_plane - Radius: 0.05000000074505806 - Reference Frame: ground_plane - Value: false - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1.5 - Name: ground_plane_raw - Radius: 0.05000000074505806 - Reference Frame: ground_plane_raw - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/sensor_kit_base_link/markers - Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -151,7 +101,7 @@ Visualization Manager: Max Intensity: 100 Min Color: 0; 0; 0 Min Intensity: 1 - Name: top_inliers + Name: inliers Position Transformer: XYZ Selectable: true Size (Pixels): 5 @@ -163,116 +113,61 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensors/base_link/sensor_kit_base_link/inliers + Value: /inliers Use Fixed Frame: true Use rainbow: true 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: FlatColor - Decay Time: 0 + - Class: rviz_default_plugins/Axes Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: left_inliers - Position Transformer: XYZ - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_left_base_link/inliers - Use Fixed Frame: true - Use rainbow: true + Length: 1 + Name: initial_base_link + Radius: 0.10000000149011612 + Reference Frame: base_link 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: FlatColor - Decay Time: 0 + - Class: rviz_default_plugins/Axes Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: center_inliers - Position Transformer: XYZ - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_center_base_link/inliers - Use Fixed Frame: true - Use rainbow: true + Length: 1 + Name: estimated_base_link + Radius: 0.10000000149011612 + Reference Frame: estimated_base_link 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: FlatColor - Decay Time: 0 + - Class: rviz_default_plugins/Axes Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: right_inliers - Position Transformer: XYZ - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points + Length: 1 + Name: lidar + Radius: 0.10000000149011612 + Reference Frame: lidar_frame + Value: true + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 1.5 + Name: ground_plane + Radius: 0.05000000074505806 + Reference Frame: ground_plane + Value: false + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 1.5 + Name: ground_plane_raw + Radius: 0.05000000074505806 + Reference Frame: ground_plane_raw + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensor_kit/sensor_kit_base_link/livox_front_right_base_link/inliers - Use Fixed Frame: true - Use rainbow: true - Value: true + Reliability Policy: Reliable + Value: /base_link/sensor_kit_base_link/markers + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: estimated_base_link Frame Rate: 30 Name: root Tools: @@ -323,22 +218,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.294999599456787 + Pitch: 0 Position: - X: -6.224511623382568 - Y: 0.10581792891025543 - Z: 25.368898391723633 + X: 2.4723308086395264 + Y: -0.0926784947514534 + Z: 0 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 0.051838066428899765 + Yaw: 6.240023612976074 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1043 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000023b0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e00000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000023b00000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e00000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -349,4 +244,4 @@ Window Geometry: collapsed: false Width: 1846 X: 74 - Y: 27 + Y: 0 diff --git a/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp new file mode 100644 index 00000000..63d74df5 --- /dev/null +++ b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp @@ -0,0 +1,537 @@ +// 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. + +#include +#include +#include + +#include + +#include + +namespace ground_plane_calibrator +{ + +ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::NodeOptions & options) +: Node("ground_plane_calibrator_node", options), tf_broadcaster_(this) +{ + tf_buffer_ = std::make_shared(this->get_clock()); + transform_listener_ = std::make_shared(*tf_buffer_); + + base_frame_ = this->declare_parameter("base_frame", "base_link"); + lidar_frame_ = this->declare_parameter("lidar_frame"); + + marker_size_ = this->declare_parameter("marker_size", 20.0); + + auto & parameters = ground_plane_extractor_parameters_; + parameters.use_crop_box_filter_ = this->declare_parameter("use_crop_box_filter", true); + parameters.crop_box_min_x_ = this->declare_parameter("crop_box_min_x", -50.0); + parameters.crop_box_min_y_ = this->declare_parameter("crop_box_min_y", -50.0); + parameters.crop_box_min_z_ = this->declare_parameter("crop_box_min_z", -50.0); + parameters.crop_box_max_x_ = this->declare_parameter("crop_box_max_x", 50.0); + parameters.crop_box_max_y_ = this->declare_parameter("crop_box_max_y", 50.0); + parameters.crop_box_max_z_ = this->declare_parameter("crop_box_max_z", 50.0); + + parameters.remove_outliers_ = this->declare_parameter("remove_outliers", true); + parameters.remove_outlier_tolerance_ = + this->declare_parameter("remove_outlier_tolerance", 0.1); + parameters.use_pca_rough_normal_ = this->declare_parameter("use_pca_rough_normal", true); + parameters.max_inlier_distance_ = this->declare_parameter("max_inlier_distance", 0.01); + parameters.min_plane_points_ = this->declare_parameter("min_plane_points", 500); + parameters.min_plane_points_percentage_ = + this->declare_parameter("min_plane_points_percentage", 10.0); + parameters.max_cos_distance_ = this->declare_parameter("max_cos_distance", 0.2); + parameters.max_iterations_ = this->declare_parameter("max_iterations", 500); + verbose_ = this->declare_parameter("verbose", false); + parameters.verbose_ = verbose_; + overwrite_xy_yaw_ = this->declare_parameter("overwrite_xy_yaw", false); + filter_estimations_ = this->declare_parameter("filter_estimations", true); + int ring_buffer_size = this->declare_parameter("ring_buffer_size", 100); + + inlier_observations_.setMaxSize(ring_buffer_size); + + initial_angle_cov_ = this->declare_parameter("initial_angle_cov", 5.0); + initial_translation_cov_ = this->declare_parameter("initial_translation_cov", 0.05); + + angle_measurement_cov_ = this->declare_parameter("angle_measurement_cov", 0.5); + angle_process_cov_ = this->declare_parameter("angle_process_cov", 0.1); + translation_measurement_cov_ = + this->declare_parameter("translation_measurement_cov", 0.005); + translation_process_cov_ = this->declare_parameter("translation_process_cov", 0.001); + + angle_convergence_threshold_ = this->declare_parameter("angle_convergence_threshold", 0.0); + translation_convergence_threshold_ = + this->declare_parameter("translation_convergence_threshold", 0.0); + + initial_angle_cov_ = std::pow(initial_angle_cov_ * M_PI_2 / 180.0, 2); + initial_translation_cov_ = std::pow(initial_translation_cov_, 2); + + angle_measurement_cov_ = std::pow(angle_measurement_cov_ * M_PI_2 / 180.0, 2); + angle_process_cov_ = std::pow(angle_process_cov_ * M_PI_2 / 180.0, 2); + translation_measurement_cov_ = std::pow(translation_measurement_cov_, 2); + translation_process_cov_ = std::pow(translation_process_cov_, 2); + + angle_convergence_threshold_ = std::pow(angle_convergence_threshold_ * M_PI_2 / 180.0, 2); + translation_convergence_threshold_ = std::pow(translation_convergence_threshold_, 2); + + markers_pub_ = this->create_publisher("markers", 10); + inliers_pub_ = this->create_publisher("inliers", 10); + + pointcloud_sub_ = this->create_subscription( + "input_pointcloud", rclcpp::SensorDataQoS(), + std::bind(&ExtrinsicGroundPlaneCalibrator::pointCloudCallback, this, std::placeholders::_1)); + + // The service server runs in a dedicated thread + srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + service_server_ = this->create_service( + "extrinsic_calibration", + std::bind( + &ExtrinsicGroundPlaneCalibrator::requestReceivedCallback, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, srv_callback_group_); + + // Initialize the filter + kalman_filter_.setA(Eigen::DiagonalMatrix(1.0, 1.0, 1.0, 1.0, 1.0, 1.0)); + kalman_filter_.setB(Eigen::DiagonalMatrix(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + kalman_filter_.setC(Eigen::DiagonalMatrix(1.0, 1.0, 1.0, 1.0, 1.0, 1.0)); + kalman_filter_.setQ(Eigen::DiagonalMatrix( + angle_measurement_cov_, angle_measurement_cov_, angle_measurement_cov_, + translation_measurement_cov_, translation_measurement_cov_, translation_measurement_cov_)); + kalman_filter_.setR(Eigen::DiagonalMatrix( + angle_process_cov_, angle_process_cov_, angle_process_cov_, translation_process_cov_, + translation_process_cov_, translation_process_cov_)); +} + +void ExtrinsicGroundPlaneCalibrator::requestReceivedCallback( + [[maybe_unused]] const std::shared_ptr + request, + const std::shared_ptr response) +{ + // This tool uses several tfs, so for consistency we take the initial calibration using lookups + using std::chrono_literals::operator""s; + + { + std::unique_lock lock(mutex_); + received_request_ = true; + } + + // Loop until the calibration finishes + while (rclcpp::ok()) { + rclcpp::sleep_for(1s); + std::unique_lock lock(mutex_); + + if (calibration_done_) { + break; + } + + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 10000, "Waiting for the calibration to end"); + } + + tier4_calibration_msgs::msg::CalibrationResult result; + result.transform_stamped = tf2::eigenToTransform(calibrated_base_to_lidar_transform_); + result.transform_stamped.header.frame_id = base_frame_; + result.transform_stamped.child_frame_id = lidar_frame_; + result.score = 0.f; + result.success = true; + result.message.data = + "Calibration succeeded with convergence criteria. However, no metric is available for this " + "tool"; + + if (overwrite_xy_yaw_) { + result.transform_stamped = tier4_ground_plane_utils::overwriteXYYawValues( + initial_base_to_lidar_transform_msg_, result.transform_stamped); + } + + response->results.emplace_back(result); + + RCLCPP_INFO(this->get_logger(), "Calibration result sent"); + + // Forcefully unsubscribe from the pointcloud topic + pointcloud_sub_.reset(); +} + +void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + bool received_request; + { + std::unique_lock lock(mutex_); + received_request = received_request_; + } + + if (lidar_frame_ != msg->header.frame_id) { + RCLCPP_WARN( + this->get_logger(), + "Received pointcloud's frame does not match the expected one (received=%s vs. expected=%s)", + msg->header.frame_id.c_str(), lidar_frame_.c_str()); + return; + } + + header_ = msg->header; + + // Make sure we have all the required initial tfs + if (!received_request || !checkInitialTransforms() || calibration_done_) { + return; + } + + // Convert the pointcloud to PCL + pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *pointcloud); + + // Filter the pointcloud using previous outlier models + if (ground_plane_extractor_parameters_.remove_outliers_) { + std::size_t original_num_points = pointcloud->size(); + for (const auto & outlier_model : outlier_models_) { + pointcloud = tier4_ground_plane_utils::removeOutliers( + pointcloud, outlier_model, ground_plane_extractor_parameters_.remove_outlier_tolerance_); + } + + RCLCPP_INFO( + this->get_logger(), "Outlier plane removal: %lu -> %lu points", original_num_points, + pointcloud->size()); + } + + // Extract the ground plane model + ground_plane_extractor_parameters_.initial_base_to_lidar_transform_ = + initial_base_to_lidar_transform_; + auto [ground_plane_result, ground_plane_model, inliers_pointcloud] = + tier4_ground_plane_utils::extractGroundPlane( + pointcloud, ground_plane_extractor_parameters_, outlier_models_); + + if (!ground_plane_result) { + return; + } + + // Obtain the model error for the initial and current calibration + Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_transform_.inverse(); + Eigen::Vector4d initial_ground_plane_model = + tier4_ground_plane_utils::poseToPlaneModel(initial_lidar_base_transform); + + tier4_ground_plane_utils::evaluateModels( + initial_ground_plane_model, ground_plane_model, inliers_pointcloud); + + // Publish the inliers + sensor_msgs::msg::PointCloud2 inliers_msg; + pcl::toROSMsg(*inliers_pointcloud, inliers_msg); + inliers_msg.header = header_; + inliers_pub_->publish(inliers_msg); + + // Create markers to visualize the calibration + visualizeCalibration(ground_plane_model); + + filterGroundModelEstimation(ground_plane_model, inliers_pointcloud); + + // Obtain the final output tf and publish the lidar -> ground tfs to evaluate the calibration + publishTf(ground_plane_model); +} + +bool ExtrinsicGroundPlaneCalibrator::checkInitialTransforms() +{ + if (lidar_frame_ == "") { + RCLCPP_ERROR(this->get_logger(), "The lidar frame can not be empty !"); + return false; + } + + if (got_initial_transform_) { + return true; + } + + try { + rclcpp::Time t = rclcpp::Time(0); + rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); + + geometry_msgs::msg::Transform initial_base_to_lidar_transform_msg_ = + tf_buffer_->lookupTransform(base_frame_, lidar_frame_, t, timeout).transform; + + initial_base_to_lidar_transform_ = tf2::transformToEigen(initial_base_to_lidar_transform_msg_); + + got_initial_transform_ = true; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "Could not get initial tf. %s", ex.what()); + return false; + } + + return true; +} + +void ExtrinsicGroundPlaneCalibrator::filterGroundModelEstimation( + const Eigen::Vector4d & ground_plane_model, pcl::PointCloud::Ptr inliers) +{ + Eigen::Isometry3d estimated_base_to_lidar_transform = + tier4_ground_plane_utils::estimateBaseLidarTransform( + initial_base_to_lidar_transform_, ground_plane_model); + + Eigen::Vector3d estimated_translation; + auto estimated_rpy = + tier4_autoware_utils::getRPY(tf2::toMsg(estimated_base_to_lidar_transform).orientation); + auto initial_rpy = + tier4_autoware_utils::getRPY(tf2::toMsg(initial_base_to_lidar_transform_).orientation); + + if (verbose_) { + RCLCPP_INFO( + this->get_logger(), "Initial base->lidar euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", + initial_rpy.x, initial_rpy.y, initial_rpy.z); + RCLCPP_INFO( + this->get_logger(), "Estimated base->lidar euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", + estimated_rpy.x, estimated_rpy.y, estimated_rpy.z); + + RCLCPP_INFO( + this->get_logger(), "Initial base->lidar translation: x=%.3f, y=%.3f, z=%.3f", + initial_base_to_lidar_transform_.translation().x(), + initial_base_to_lidar_transform_.translation().y(), + initial_base_to_lidar_transform_.translation().z()); + RCLCPP_INFO( + this->get_logger(), "Estimated base->lidar translation: x=%.3f, y=%.3f, z=%.3f", + estimated_base_to_lidar_transform.translation().x(), + estimated_base_to_lidar_transform.translation().y(), + estimated_base_to_lidar_transform.translation().z()); + } + + // If filtering is disabled no further processing is needed + if (!filter_estimations_) { + std::unique_lock lock(mutex_); + calibrated_base_to_lidar_transform_ = estimated_base_to_lidar_transform; + calibration_done_ = true; + return; + } + + // Optional filtering: + // 1) Use linear kalman filter to determine convergence in the estimation + // 2) The liner kalman filter does not correctly filter rotations, instead use all the estimations + // so far to calibrate once more + + // Kalman step + Eigen::Vector x( + estimated_rpy.x, estimated_rpy.y, estimated_rpy.z, + estimated_base_to_lidar_transform.translation().x(), + estimated_base_to_lidar_transform.translation().y(), + estimated_base_to_lidar_transform.translation().z()); + Eigen::DiagonalMatrix p0( + initial_angle_cov_, initial_angle_cov_, initial_angle_cov_, initial_translation_cov_, + initial_translation_cov_, initial_translation_cov_); + + if (first_observation_) { + kalman_filter_.init(x, p0); + first_observation_ = false; + } else { + kalman_filter_.update(x); + } + + // cSpell:ignore getXelement + estimated_rpy.x = kalman_filter_.getXelement(0); + estimated_rpy.y = kalman_filter_.getXelement(1); + estimated_rpy.z = kalman_filter_.getXelement(2); + estimated_translation.x() = kalman_filter_.getXelement(3); + estimated_translation.y() = kalman_filter_.getXelement(4); + estimated_translation.z() = kalman_filter_.getXelement(5); + + // Filtering convergence criteria + Eigen::MatrixXd p; + kalman_filter_.getP(p); + Eigen::VectorXd diag = p.diagonal(); + std::array thresholds{ + angle_convergence_threshold_, angle_convergence_threshold_, + angle_convergence_threshold_, translation_convergence_threshold_, + translation_convergence_threshold_, translation_convergence_threshold_}; + + bool converged = true; + for (std::size_t index = 0; index < thresholds.size(); index++) { + converged &= diag(index) < thresholds[index]; + } + + RCLCPP_INFO( + this->get_logger(), "Filter cov: roll=%.2e, pitch=%.2e yaw=%.2e, x=%.2e, y=%.2e, z=%.2e", + diag(0), diag(1), diag(2), diag(3), diag(4), diag(5)); + + RCLCPP_INFO( + this->get_logger(), "Convergence thresh: angle=%.2e, translation=%.2e", + angle_convergence_threshold_, translation_convergence_threshold_); + + // Save the inliers for later refinement + inlier_observations_.add(inliers); + + if (!converged) { + return; + } + + // Integrate all the inliers so far and refine the estimation + pcl::PointCloud::Ptr augmented_inliers(new pcl::PointCloud); + + for (const auto & inliers : inlier_observations_.get()) { + *augmented_inliers += *inliers; + } + + auto [final_model, final_inliers] = tier4_ground_plane_utils::extractPlane( + augmented_inliers, 10 * ground_plane_extractor_parameters_.max_inlier_distance_, + ground_plane_extractor_parameters_.max_iterations_); + + RCLCPP_INFO( + this->get_logger(), + "Final model: a=%.3f, b=%.3f, c=%.3f, d=%.3f final inliers=%lu total.percentage=%.2f", + final_model(0), final_model(1), final_model(2), final_model(3), final_inliers->indices.size(), + 100.f * final_inliers->indices.size() / augmented_inliers->size()); + + std::unique_lock lock(mutex_); + calibrated_base_to_lidar_transform_ = tier4_ground_plane_utils::estimateBaseLidarTransform( + initial_base_to_lidar_transform_, final_model); + calibration_done_ = true; +} + +void ExtrinsicGroundPlaneCalibrator::visualizeCalibration( + const Eigen::Vector4d & estimated_ground_model) +{ + visualization_msgs::msg::MarkerArray markers; + + Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_transform_.inverse(); + + visualizePlaneModel("initial_calibration_pose", initial_lidar_base_transform, markers); + + Eigen::Vector4d fake_model = + tier4_ground_plane_utils::poseToPlaneModel(initial_lidar_base_transform); + + visualizePlaneModel("initial_calibration_model", fake_model, markers); + + visualizePlaneModel("estimated_model", estimated_ground_model, markers); + + markers_pub_->publish(markers); +} + +void ExtrinsicGroundPlaneCalibrator::visualizePlaneModel( + const std::string & name, Eigen::Vector4d model, visualization_msgs::msg::MarkerArray & markers) +{ + visualizePlaneModel(name, tier4_ground_plane_utils::modelPlaneToPose(model), markers); +} + +void ExtrinsicGroundPlaneCalibrator::visualizePlaneModel( + const std::string & name, const Eigen::Isometry3d & lidar_base_pose, + visualization_msgs::msg::MarkerArray & markers) +{ + std::vector corners = { + Eigen::Vector3d(-marker_size_, -marker_size_, 0.0), + Eigen::Vector3d(-marker_size_, marker_size_, 0.0), + Eigen::Vector3d(marker_size_, marker_size_, 0.0), + Eigen::Vector3d(marker_size_, -marker_size_, 0.0)}; + + for (Eigen::Vector3d & corner : corners) { + corner = lidar_base_pose * corner; + } + + std::vector corners_msg; + + for (Eigen::Vector3d & corner : corners) { + geometry_msgs::msg::Point p; + p.x = corner.x(); + p.y = corner.y(); + p.z = corner.z(); + corners_msg.push_back(p); + } + + visualization_msgs::msg::Marker marker; + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + marker.header.frame_id = lidar_frame_; + marker.header.stamp = header_.stamp; + marker.ns = name + "_plane"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.a = 0.5; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + + assert(corners_msg.size() == 4); + marker.points.push_back(corners_msg[0]); + marker.points.push_back(corners_msg[2]); + marker.points.push_back(corners_msg[1]); + marker.points.push_back(corners_msg[2]); + marker.points.push_back(corners_msg[0]); + marker.points.push_back(corners_msg[3]); + + markers.markers.push_back(marker); + + marker.ns = name + "_origin"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; + marker.scale.y = 0.01; + marker.scale.z = 0.0; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + + geometry_msgs::msg::Point p; + marker.points.clear(); + + p.x = 0; + p.y = 0; + p.z = 0; + marker.points.push_back(p); + + p.x = lidar_base_pose.translation().x(); + p.y = lidar_base_pose.translation().y(); + p.z = lidar_base_pose.translation().z(); + marker.points.push_back(p); + + markers.markers.push_back(marker); +} + +void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_plane_model) +{ + geometry_msgs::msg::TransformStamped initial_lidar_to_base_msg = + tf2::eigenToTransform(initial_base_to_lidar_transform_.inverse()); + initial_lidar_to_base_msg.header.stamp = header_.stamp; + initial_lidar_to_base_msg.header.frame_id = lidar_frame_; + initial_lidar_to_base_msg.child_frame_id = "initial_base_link"; + tf_broadcaster_.sendTransform(initial_lidar_to_base_msg); + + Eigen::Isometry3d raw_lidar_to_base_eigen = + tier4_ground_plane_utils::modelPlaneToPose(ground_plane_model); + + geometry_msgs::msg::TransformStamped raw_lidar_to_base_msg = + tf2::eigenToTransform(raw_lidar_to_base_eigen); + + raw_lidar_to_base_msg.header.stamp = header_.stamp; + raw_lidar_to_base_msg.header.frame_id = lidar_frame_; + raw_lidar_to_base_msg.child_frame_id = "ground_plane_raw"; + tf_broadcaster_.sendTransform(raw_lidar_to_base_msg); + + Eigen::Isometry3d calibrated_base_to_lidar_transform = + calibration_done_ ? calibrated_base_to_lidar_transform_ + : tier4_ground_plane_utils::estimateBaseLidarTransform( + initial_base_to_lidar_transform_, ground_plane_model); + geometry_msgs::msg::TransformStamped calibrated_base_to_lidar_transform_msg = + tf2::eigenToTransform(calibrated_base_to_lidar_transform); + + if (overwrite_xy_yaw_) { + calibrated_base_to_lidar_transform_msg = tier4_ground_plane_utils::overwriteXYYawValues( + initial_base_to_lidar_transform_msg_, calibrated_base_to_lidar_transform_msg); + } + + geometry_msgs::msg::TransformStamped lidar_to_calibrated_base_transform_msg = + tf2::eigenToTransform(tf2::transformToEigen(calibrated_base_to_lidar_transform_msg).inverse()); + lidar_to_calibrated_base_transform_msg.header.stamp = header_.stamp; + lidar_to_calibrated_base_transform_msg.header.frame_id = lidar_frame_; + lidar_to_calibrated_base_transform_msg.child_frame_id = "estimated_base_link"; + tf_broadcaster_.sendTransform(lidar_to_calibrated_base_transform_msg); +} + +} // namespace ground_plane_calibrator diff --git a/sensor/extrinsic_ground_plane_calibrator/src/main.cpp b/sensor/ground_plane_calibrator/src/main.cpp similarity index 75% rename from sensor/extrinsic_ground_plane_calibrator/src/main.cpp rename to sensor/ground_plane_calibrator/src/main.cpp index 547ba88a..f5b62376 100644 --- a/sensor/extrinsic_ground_plane_calibrator/src/main.cpp +++ b/sensor/ground_plane_calibrator/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -23,8 +23,8 @@ int main(int argc, char ** argv) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::NodeOptions node_options; - std::shared_ptr node = - std::make_shared(node_options); + std::shared_ptr node = + std::make_shared(node_options); executor.add_node(node); executor.spin(); diff --git a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/__init__.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/calibrator.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py similarity index 88% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/calibrator.py rename to sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py index 7c4fe170..c9d51bba 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/calibrator.py +++ b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2020 Tier IV, Inc. +# 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. @@ -14,11 +14,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging +from typing import List +from typing import Optional + import cv2 -from extrinsic_interactive_calibrator.utils import cv_to_transformation_matrix -from extrinsic_interactive_calibrator.utils import tf_message_to_transform_matrix -from extrinsic_interactive_calibrator.utils import transform_matrix_to_cv import numpy as np +from tier4_calibration_views.utils import cv_to_transformation_matrix +from tier4_calibration_views.utils import tf_message_to_transform_matrix +from tier4_calibration_views.utils import transform_matrix_to_cv class Calibrator: @@ -33,7 +37,6 @@ def __init__(self): # Camera parameters self.k = None self.d = None - pass def set_min_points(self, min_points): self.min_points = min_points @@ -81,7 +84,7 @@ def calibrate(self, object_points, image_points): object_points, image_points, self.k, self.d, flags=self.flags ) except Exception as e: - print(e) + logging.error(e) camera_to_lidar_transform = cv_to_transformation_matrix(tvec, rvec) @@ -105,10 +108,10 @@ def calibrate_ransac(self, object_points, image_points): object_points_iter, image_points_iter, self.k, self.d, flags=self.flags ) except Exception as e: - print(e) + logging.error(e) continue - reproj_error_iter, inliers = self.calculate_reproj_error( + reproj_error_iter, inliers = self.calculate_reproj_error( # cSpell:ignore reproj object_points, image_points, tvec=iter_tvec, rvec=iter_rvec ) @@ -125,7 +128,13 @@ def calibrate_ransac(self, object_points, image_points): return camera_to_lidar_transform def calculate_reproj_error( - self, object_points, image_points, tvec=None, rvec=None, tf_msg=None, transform_matrix=None + self, + object_points: List[np.array], + image_points: List[np.array], + tvec: Optional[np.array] = None, + rvec: Optional[np.array] = None, + tf_msg=None, + transform_matrix=None, ): if isinstance(object_points, list) and isinstance(image_points, list): if len(object_points) == 0: diff --git a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/image_view.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/image_view.py new file mode 100644 index 00000000..896c458e --- /dev/null +++ b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/image_view.py @@ -0,0 +1,827 @@ +#!/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. + + +import copy +import logging +import threading + +from PySide2.QtCore import QObject +from PySide2.QtCore import QPoint +from PySide2.QtCore import QRectF +from PySide2.QtCore import QSize +from PySide2.QtCore import QThread +from PySide2.QtCore import Qt +from PySide2.QtCore import Signal +from PySide2.QtGui import QColor +from PySide2.QtGui import QPainter +from PySide2.QtGui import QPen +from PySide2.QtGui import QPixmap +from PySide2.QtWidgets import QGraphicsItem +from PySide2.QtWidgets import QGraphicsView +import cv2 +import matplotlib.pyplot as plt +import numpy as np +from tier4_calibration_views.utils import decompose_transformation_matrix +from tier4_calibration_views.utils import transform_points + + +def intensity_to_rainbow_qcolor(value, alpha=1.0): + h = value * 5.0 + 1.0 + i = h // 1 # floor(h) + f = h - i + if i % 2 == 0: + f = 1.0 - f + + n = 1 - f + + if i <= 1: + r, g, b = n, 0, 1.0 + elif i == 2: + r, g, b = 0.0, n, 1.0 + elif i == 3: + r, g, b = 0.0, 1.0, n + elif i == 4: + r, g, b = n, 1.0, 0.0 + elif i >= 5: + r, g, b = 1.0, n, 0 + + return QColor(int(255 * r), int(255 * g), int(255 * b), int(255 * alpha)) + + +class RenderingData: + def __init__(self): + self.image_to_lidar_transform = None + self.image_to_lidar_translation = None + self.image_to_lidar_rotation = None + + self.draw_calibration_points_flag = False + self.draw_pointcloud_flag = False + self.draw_inliers_flag = False + self.marker_size_pixels = None + self.marker_size_meters = None + self.color_channel = None + self.marker_units = None + self.marker_type = None + self.rendering_alpha = None + self.subsample_factor = None + self.rainbow_distance = None + self.rainbow_offset = 0 + self.min_rendering_distance = 0.0 + self.max_rendering_distance = 100.0 + + self.current_object_point = None + self.object_points = None + self.image_points = None + self.external_object_points = None + self.external_image_points = None + self.pointcloud_xyz = None + self.pointcloud_intensity = None + + self.widget_size = None + + self.k = None + self.d = None + + +class CustomQGraphicsView(QGraphicsView): + def __init__(self, parent=None): + super(CustomQGraphicsView, self).__init__(parent) + + def resizeEvent(self, event): + super().resizeEvent(event) + + for item in self.scene().items(): + item.prepareGeometryChange() + item.update() + + def wheelEvent(self, event): + zoom_in_factor = 1.25 + zoom_out_factor = 1 / zoom_in_factor + + for item in self.scene().items(): + item.prepareGeometryChange() + item.update() + + self.setTransformationAnchor(QGraphicsView.NoAnchor) + self.setResizeAnchor(QGraphicsView.NoAnchor) + + old_pos = self.mapToScene(event.pos()) + + # Zoom + if event.delta() > 0: + zoom_factor = zoom_in_factor + else: + zoom_factor = zoom_out_factor + self.scale(zoom_factor, zoom_factor) + + # Get the new position + new_pos = self.mapToScene(event.pos()) + + # Move scene to old position + delta = new_pos - old_pos + self.translate(delta.x(), delta.y()) + + +class Renderer(QObject): + def __init__(self, image_view): + super().__init__() + self.image_view = image_view + + def render(self): + self.image_view.paintEventThread() + + +class ImageView(QGraphicsItem, QObject): + clicked_signal = Signal(float, float) + render_request_signal = Signal() + rendered_signal = Signal() + + def __init__(self, parent=None): + QGraphicsItem.__init__(self, parent) + QObject.__init__(self, parent) + + self.pix = QPixmap() + self.image_width = None + self.image_height = None + self.display_width = None + self.display_height = None + + self.data_ui = RenderingData() + self.data_renderer = RenderingData() + + self.thread = QThread() + self.thread.start() + self.lock = threading.RLock() + self.renderer = Renderer(self) + self.renderer.moveToThread(self.thread) + # To debug rendering, consider not moving it into another thread + + self.rendered_signal.connect(self.update2) + self.render_request_signal.connect(self.renderer.render) + + self.line_pen = QPen() + self.line_pen.setWidth(2) + self.line_pen.setBrush(Qt.white) + + self.magenta_pen = QPen() + self.magenta_pen.setWidth(2) + self.magenta_pen.setBrush(Qt.magenta) + + self.cyan_pen = QPen() + self.cyan_pen.setWidth(2) + self.cyan_pen.setBrush(Qt.cyan) + + self.inlier_line_pen = QPen() + self.inlier_line_pen.setWidth(2) + self.inlier_line_pen.setBrush(Qt.green) + + self.outlier_line_pen = QPen() + self.outlier_line_pen.setWidth(2) + self.outlier_line_pen.setBrush(Qt.red) + + self.red_pen = QPen(Qt.red) + self.red_pen.setBrush(Qt.red) + + self.green_pen = QPen(Qt.green) + self.green_pen.setBrush(Qt.green) + + colormap_name = "hsv" + self.colormap_bins = 100 + self.colormap = plt.get_cmap(colormap_name, self.colormap_bins) + self.colormap = [ + ( + int(255 * self.colormap(i)[2]), + int(255 * self.colormap(i)[1]), + int(255 * self.colormap(i)[0]), + ) + for i in range(self.colormap_bins) + ] + + # self.setMinimumWidth(300) + + self.update_count = 0 + self.render_count = 0 + self.unprocessed_rendered_requests = 0 + self.rendering = False + self.rendered_image = None + + def update(self): + with self.lock: + self.update_count += 1 + self.unprocessed_rendered_requests += 1 + self.render_request_signal.emit() + + def update2(self): + super().update() + + def set_draw_calibration_points(self, value): + with self.lock: + self.data_ui.draw_calibration_points_flag = value + self.update() + + def set_draw_pointcloud(self, value): + with self.lock: + self.data_ui.draw_pointcloud_flag = value + self.update() + + def set_marker_size_pixels(self, value): + with self.lock: + self.data_ui.marker_size_pixels = value + self.update() + + def set_marker_size_meters(self, value): + with self.lock: + self.data_ui.marker_size_meters = value + self.update() + + def set_rainbow_distance(self, value): + with self.lock: + self.data_ui.rainbow_distance = value + self.update() + + def set_rainbow_offset(self, value): + with self.lock: + self.data_ui.rainbow_offset = value + self.update() + + def set_rendering_alpha(self, value): + with self.lock: + self.data_ui.rendering_alpha = value + self.update() + + def set_marker_type(self, value): + with self.lock: + self.data_ui.marker_type = value.lower() + self.update() + + def set_marker_units(self, value): + with self.lock: + value = value.lower() + assert value == "meters" or value == "pixels" + self.data_ui.marker_units = value + self.update() + + def set_color_channel(self, value): + with self.lock: + self.data_ui.color_channel = value.lower() + self.update() + + def set_draw_inliers(self, value): + with self.lock: + self.data_ui.draw_inliers_flag = value + self.update() + + def set_inlier_distance(self, value): + with self.lock: + self.data_ui.inlier_distance = value + self.update() + + def set_min_rendering_distance(self, value): + with self.lock: + self.data_ui.min_rendering_distance = value + self.update() + + def set_max_rendering_distance(self, value): + with self.lock: + self.data_ui.max_rendering_distance = value + self.update() + + def set_current_point(self, point): + with self.lock: + self.data_ui.current_object_point = None if point is None else point.reshape(1, 3) + self.update() + + def set_transform(self, transform): + with self.lock: + self.data_ui.image_to_lidar_transform = transform + ( + self.data_ui.image_to_lidar_translation, + self.data_ui.image_to_lidar_rotation, + ) = decompose_transformation_matrix(transform) + self.update() + + def pixmap(self): + with self.lock: + return self.pix + + def set_pixmap(self, pixmap): + with self.lock: + if self.pix is None or self.pix.size() != pixmap.size(): + self.prepareGeometryChange() + + self.pix = pixmap + + self.image_width = float(self.pix.size().width()) + self.image_height = float(self.pix.size().height()) + + def set_subsample_factor(self, value): + with self.lock: + self.data_ui.subsample_factor = int(value) + self.update() + + def set_pointcloud(self, pointcloud): + with self.lock: + self.data_ui.pointcloud_xyz = pointcloud[:, 0:3] + self.data_ui.pointcloud_intensity = pointcloud[:, 3] + + subsample = self.data_ui.subsample_factor + + self.data_ui.pointcloud_xyz = self.data_ui.pointcloud_xyz[::subsample, :] + self.data_ui.pointcloud_intensity = self.data_ui.pointcloud_intensity[::subsample] + + def set_camera_info(self, k, d): + with self.lock: + self.data_ui.k = np.copy(k).reshape((3, 3)) + self.data_ui.d = np.copy(d).reshape((-1,)) + + def set_calibration_points(self, object_points, image_points): + with self.lock: + self.data_ui.object_points = object_points + self.data_ui.image_points = image_points + self.update() + + def set_external_calibration_points(self, object_points, image_points): + with self.lock: + self.data_ui.external_object_points = object_points + self.data_ui.external_image_points = image_points + self.update() + + def minimumSizeHint(self): + return QSize(1000, 400) + + def sizeHint(self): + return QSize(1000, 1000) + + def take_screenshot(self): + with self.lock: + return self.rendered_image.copy() + + def paint(self, painter, option, widget): + with self.lock: + self.data_ui.widget_size = widget.size() + painter.setRenderHint(QPainter.Antialiasing) + painter.drawPixmap(QPoint(), self.rendered_image) + + def boundingRect(self): + with self.lock: + if self.rendered_image is None: + return QRectF(0, 0, 500, 500) + + return QRectF( + 0, 0, self.rendered_image.size().width(), self.rendered_image.size().height() + ) + + def paintEventThread(self): + with self.lock: + self.render_count += 1 + self.unprocessed_rendered_requests -= 1 + + if self.pix.isNull(): + return + + if self.unprocessed_rendered_requests > 0: + self.rendered.emit() + return + + # Copy the data into the thread + self.data_renderer = copy.deepcopy(self.data_ui) + + if self.data_renderer.widget_size is None: + return + + scaled_pix_size = self.pix.size() + scaled_pix_size.scale(self.data_renderer.widget_size, Qt.KeepAspectRatio) + + rendered_image = self.pix.scaled( + scaled_pix_size, Qt.KeepAspectRatio, Qt.SmoothTransformation + ) + + painter = QPainter(rendered_image) + painter.setRenderHint(QPainter.Antialiasing) + + painter.setPen(Qt.red) + + painter.drawLine(QPoint(0, 0), QPoint(0, scaled_pix_size.height())) + painter.drawLine( + QPoint(0, scaled_pix_size.height()), + QPoint(scaled_pix_size.width(), scaled_pix_size.height()), + ) + painter.drawLine( + QPoint(scaled_pix_size.width(), scaled_pix_size.height()), + QPoint(scaled_pix_size.width(), 0), + ) + painter.drawLine(QPoint(scaled_pix_size.width(), 0), QPoint(0, 0)) + + self.width_image_to_widget_factor = float(scaled_pix_size.width()) / self.image_width + self.height_image_to_widget_factor = float(scaled_pix_size.height()) / self.image_height + self.image_to_widget_factor = np.array( + [self.width_image_to_widget_factor, self.height_image_to_widget_factor] + ) + + if self.data_renderer.draw_pointcloud_flag: + self.draw_pointcloud(painter) + + if self.data_renderer.draw_calibration_points_flag: + self.draw_calibration_points(painter) + self.draw_external_calibration_points(painter) + + self.draw_current_point(painter) + + painter.end() + + with self.lock: + self.rendered_image = rendered_image + self.rendered_signal.emit() + + def draw_pointcloud(self, painter): + if ( + self.data_renderer.image_to_lidar_translation is None + or self.data_renderer.image_to_lidar_rotation is None + ): + return + + # Note: ccs=camera coordinate system. ics=image coordinate system + pointcloud_ccs = transform_points( + self.data_renderer.image_to_lidar_translation, + self.data_renderer.image_to_lidar_rotation, + self.data_renderer.pointcloud_xyz, + ) + + if pointcloud_ccs.shape[0] == 0: + return + + # Transform to the image coordinate system + tvec = np.zeros((3, 1)) + rvec = np.zeros((3, 1)) + + pointcloud_ics, _ = cv2.projectPoints( + pointcloud_ccs, rvec, tvec, self.data_renderer.k, self.data_renderer.d + ) + + pointcloud_ics = pointcloud_ics.reshape(-1, 2) + + indexes = np.logical_and( + np.logical_and( + np.logical_and(pointcloud_ics[:, 0] >= 0, pointcloud_ics[:, 0] < self.image_width), + np.logical_and(pointcloud_ics[:, 1] >= 0, pointcloud_ics[:, 1] < self.image_height), + ), + np.logical_and( + pointcloud_ccs[:, 2] >= self.data_renderer.min_rendering_distance, + pointcloud_ccs[:, 2] < self.data_renderer.max_rendering_distance, + ), + ) + + # Transform (rescale) into the widget coordinate system + pointcloud_z = pointcloud_ccs[indexes, 2] + pointcloud_i = self.data_renderer.pointcloud_intensity[indexes] + + if self.data_renderer.marker_units == "meters": + factor = ( + self.data_renderer.k[0, 0] + * self.data_renderer.marker_size_meters + * self.width_image_to_widget_factor + ) + scale_px = factor / pointcloud_z + else: + factor = self.data_renderer.marker_size_pixels * self.width_image_to_widget_factor + scale_px = factor * np.ones_like(pointcloud_z) + + pointcloud_wcs = pointcloud_ics[indexes, :] * self.image_to_widget_factor + + indexes2 = scale_px >= 1 + pointcloud_wcs = pointcloud_wcs[indexes2, :] + scale_px = scale_px[indexes2] + + if pointcloud_wcs.shape[0] == 0: + return + + try: + if self.data_renderer.color_channel == "x": + color_scalars = pointcloud_ccs[indexes, 0][indexes2] + elif self.data_renderer.color_channel == "y": + color_scalars = pointcloud_ccs[indexes, 1][indexes2] + elif self.data_renderer.color_channel == "z": + color_scalars = pointcloud_z[indexes2] + elif self.data_renderer.color_channel == "intensity": + color_scalars = pointcloud_i[indexes2] + min_value = color_scalars.min() + max_value = color_scalars.max() + if min_value == max_value: + color_scalars = np.ones_like(color_scalars) + else: + color_scalars = 1.0 - (color_scalars - min_value) / (max_value - min_value) + else: + raise NotImplementedError + except Exception as e: + logging.error(e) + + line_pen = QPen() + line_pen.setWidth(2) + line_pen.setBrush(Qt.white) + + painter.setPen(Qt.blue) + painter.setBrush(Qt.blue) + + draw_marker_f = ( + painter.drawEllipse if self.data_renderer.marker_type == "circles" else painter.drawRect + ) + + # print(f"Drawing pointcloud size: {scale_px.shape[0]}") + + for point, radius, color_channel in zip(pointcloud_wcs, scale_px, color_scalars): + if self.data_renderer.color_channel == "intensity": + color = intensity_to_rainbow_qcolor( + color_channel, self.data_renderer.rendering_alpha + ) + else: + color_index = int( + self.colormap_bins + * ( + ( + self.data_renderer.rainbow_offset + + (color_channel / self.data_renderer.rainbow_distance) + ) + % 1.0 + ) + ) + color = self.colormap[color_index] + color = QColor( + color[0], color[1], color[2], int(255 * self.data_renderer.rendering_alpha) + ) + + painter.setPen(color) + painter.setBrush(color) + draw_marker_f(point[0] - 0.5 * radius, point[1] - 0.5 * radius, radius, radius) + + def draw_calibration_points(self, painter): + if ( + self.data_renderer.image_points is None + or self.data_renderer.object_points is None + or len(self.data_renderer.image_points) == 0 + or len(self.data_renderer.object_points) == 0 + or self.data_renderer.image_to_lidar_translation is None + or self.data_renderer.image_to_lidar_rotation is None + ): + return + + image_points = np.array(self.data_renderer.image_points) + + # Note: lcs=lidar coordinate system. ccs=camera coordinate system + object_points_lcs = np.array(self.data_renderer.object_points) + + object_points_ccs = transform_points( + self.data_renderer.image_to_lidar_translation, + self.data_renderer.image_to_lidar_rotation, + object_points_lcs, + ) + + # Transform to the image coordinate system + tvec = np.zeros((3, 1)) + rvec = np.zeros((3, 1)) + object_points_ics, _ = cv2.projectPoints( + object_points_ccs, rvec, tvec, self.data_renderer.k, self.data_renderer.d + ) + object_points_ics = object_points_ics.reshape(-1, 2) + + repr_err = np.linalg.norm(object_points_ics - image_points, axis=1) + + # Transform (rescale) into the widget coordinate system + object_points_wcs = object_points_ics * self.image_to_widget_factor + + radius = 10 * self.width_image_to_widget_factor + + object_pen = self.red_pen + image_pen = self.green_pen + line_pen = self.line_pen + + for object_point_wcs, image_point, d in zip( + object_points_wcs, self.data_renderer.image_points, repr_err + ): + image_point_wcs = image_point * self.image_to_widget_factor + + if self.data_renderer.draw_inliers_flag: + if d < self.data_renderer.inlier_distance: + line_pen = self.inlier_line_pen + object_pen = self.green_pen + image_pen = self.green_pen + else: + line_pen = self.outlier_line_pen + object_pen = self.red_pen + image_pen = self.red_pen + + painter.setPen(line_pen) + painter.drawLine( + object_point_wcs[0], object_point_wcs[1], image_point_wcs[0], image_point_wcs[1] + ) + + painter.setPen(object_pen) + painter.setBrush(object_pen.brush()) + painter.drawEllipse( + object_point_wcs[0] - 0.5 * radius, + object_point_wcs[1] - 0.5 * radius, + radius, + radius, + ) + + painter.setPen(image_pen) + painter.setBrush(image_pen.brush()) + painter.drawEllipse( + image_point_wcs[0] - 0.5 * radius, image_point_wcs[1] - 0.5 * radius, radius, radius + ) + + def draw_external_calibration_points(self, painter): + if ( + self.data_renderer.external_image_points is None + or self.data_renderer.external_object_points is None + or len(self.data_renderer.external_image_points) == 0 + or len(self.data_renderer.external_object_points) == 0 + or self.data_renderer.image_to_lidar_translation is None + or self.data_renderer.image_to_lidar_rotation is None + ): + return + + object_points_lcs = np.array(self.data_renderer.external_object_points) + + # Note: lcs=lidar coordinate system. ccs=camera coordinate system. ics=image coordinate system. wcs=widget coordinate system + object_points_ccs = transform_points( + self.data_renderer.image_to_lidar_translation, + self.data_renderer.image_to_lidar_rotation, + object_points_lcs, + ) + + # Transform to the image coordinate system + tvec = np.zeros((3, 1)) + rvec = np.zeros((3, 1)) + object_points_ics, _ = cv2.projectPoints( + object_points_ccs, rvec, tvec, self.data_renderer.k, self.data_renderer.d + ) + object_points_ics = object_points_ics.reshape(-1, 2) + + # Transform (rescale) into the widget coordinate system + object_points_wcs = object_points_ics * self.image_to_widget_factor + + radius = 10 * self.width_image_to_widget_factor + + object_pen = self.red_pen + image_pen = self.green_pen + line_pen = self.line_pen + object_line_pen = self.magenta_pen + image_line_pen = self.cyan_pen + + # Draw tag borders + image_points = self.data_renderer.external_image_points + + scaled_pix_size = self.pix.size() + scaled_pix_size.scale(self.data_renderer.widget_size, Qt.KeepAspectRatio) + + for i1 in range(len(image_points)): + tag_index = i1 // 4 + i2 = 4 * tag_index + ((i1 + 1) % 4) + + image_point_1_wcs = image_points[i1] * self.image_to_widget_factor + image_point_2_wcs = image_points[i2] * self.image_to_widget_factor + + object_point_1_wcs = object_points_wcs[i1] + object_point_2_wcs = object_points_wcs[i2] + + painter.setPen(image_line_pen) + painter.drawLine( + image_point_1_wcs[0], + image_point_1_wcs[1], + image_point_2_wcs[0], + image_point_2_wcs[1], + ) + + if ( + np.any(np.isnan(object_point_1_wcs)) + or np.any(np.isnan(object_point_2_wcs)) + or object_point_1_wcs[0] < 0 + or object_point_1_wcs[0] > scaled_pix_size.width() + or object_point_1_wcs[1] < 0 + or object_point_1_wcs[1] > scaled_pix_size.height() + or object_point_2_wcs[0] < 0 + or object_point_2_wcs[0] > scaled_pix_size.width() + or object_point_2_wcs[1] < 0 + or object_point_2_wcs[1] > scaled_pix_size.height() + ): + continue + + painter.setPen(object_line_pen) + painter.drawLine( + object_point_1_wcs[0], + object_point_1_wcs[1], + object_point_2_wcs[0], + object_point_2_wcs[1], + ) + + # Draw normal points + for object_point_wcs, image_point in zip( + object_points_wcs, self.data_renderer.external_image_points + ): + image_point_wcs = image_point * self.image_to_widget_factor + + painter.setPen(line_pen) + painter.drawLine( + object_point_wcs[0], object_point_wcs[1], image_point_wcs[0], image_point_wcs[1] + ) + + painter.setPen(object_pen) + painter.setBrush(object_pen.brush()) + painter.drawEllipse( + object_point_wcs[0] - 0.5 * radius, + object_point_wcs[1] - 0.5 * radius, + radius, + radius, + ) + + painter.setPen(image_pen) + painter.setBrush(image_pen.brush()) + painter.drawEllipse( + image_point_wcs[0] - 0.5 * radius, image_point_wcs[1] - 0.5 * radius, radius, radius + ) + + def draw_current_point(self, painter): + if self.data_renderer.current_object_point is None: + return + + if ( + self.data_renderer.image_to_lidar_translation is None + or self.data_renderer.image_to_lidar_rotation is None + ): + return + + # Note: ccs=camera coordinate system. ics=image coordinate system. wcs=widget coordinate system + object_point_ccs = transform_points( + self.data_renderer.image_to_lidar_translation, + self.data_renderer.image_to_lidar_rotation, + self.data_renderer.current_object_point, + ) + + # Transform to the image coordinate system + tvec = np.zeros((3, 1)) + rvec = np.zeros((3, 1)) + object_point_ics, _ = cv2.projectPoints( + object_point_ccs, rvec, tvec, self.data_renderer.k, self.data_renderer.d + ) + object_point_ics = object_point_ics.reshape(1, 2) + + # Transform (rescale) into the widget coordinate system + object_point_wcs = object_point_ics * self.image_to_widget_factor + object_point_wcs = object_point_wcs.reshape( + 2, + ) + + radius = 20 * self.width_image_to_widget_factor + + painter.setPen(Qt.magenta) + painter.drawLine( + object_point_wcs[0] - radius, + object_point_wcs[1] - radius, + object_point_wcs[0] + radius, + object_point_wcs[1] + radius, + ) + + painter.drawLine( + object_point_wcs[0] + radius, + object_point_wcs[1] - radius, + object_point_wcs[0] - radius, + object_point_wcs[1] + radius, + ) + + def mousePressEvent(self, e): + with self.lock: + if self.pix is None or self.data_renderer.widget_size is None: + return + + scaled_pix_size = self.pix.size() + scaled_pix_size.scale(self.data_renderer.widget_size, Qt.KeepAspectRatio) + + width_widget_to_image_factor = self.image_width / float(scaled_pix_size.width()) + height_widget_to_image_factor = self.image_height / float(scaled_pix_size.height()) + + x = (e.scenePos().x() + 0.5) * width_widget_to_image_factor + y = (e.scenePos().y() + 0.5) * height_widget_to_image_factor + + if x >= 0 and x < self.image_width and y >= 0 and y < self.image_height: + self.update() + + self.clicked_signal.emit(x, y) + else: + logging.error("Click out of image coordinates !") + + self.prepareGeometryChange() + return super().mousePressEvent(e) diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py similarity index 51% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py rename to sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py index 6dff89d4..51e5d972 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py +++ b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2020 Tier IV, Inc. +# 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. @@ -14,16 +14,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -import copy import json +import logging import os import signal import sys -import threading from PySide2.QtCore import Qt from PySide2.QtCore import Signal -from PySide2.QtGui import QImage from PySide2.QtGui import QPixmap from PySide2.QtWidgets import QApplication from PySide2.QtWidgets import QCheckBox @@ -31,73 +29,25 @@ from PySide2.QtWidgets import QDoubleSpinBox from PySide2.QtWidgets import QFileDialog from PySide2.QtWidgets import QFrame -from PySide2.QtWidgets import QGraphicsScene -from PySide2.QtWidgets import QGraphicsView from PySide2.QtWidgets import QGroupBox -from PySide2.QtWidgets import QHBoxLayout from PySide2.QtWidgets import QLabel -from PySide2.QtWidgets import QMainWindow from PySide2.QtWidgets import QPushButton from PySide2.QtWidgets import QSpinBox from PySide2.QtWidgets import QVBoxLayout from PySide2.QtWidgets import QWidget -from extrinsic_interactive_calibrator.calibrator import Calibrator -from extrinsic_interactive_calibrator.image_view import CustomQGraphicsView -from extrinsic_interactive_calibrator.image_view import ImageView -from extrinsic_interactive_calibrator.ros_interface import RosInterface +from interactive_camera_lidar_calibrator.calibrator import Calibrator +from interactive_camera_lidar_calibrator.ros_interface import InteractiveCalibratorRosInterface +from interactive_camera_lidar_calibrator.utils import camera_lidar_calibrate_intrinsics import numpy as np import rclpy from rosidl_runtime_py.convert import message_to_ordereddict +from tier4_calibration_views.image_view_ui import ImageViewUI -class InteractiveCalibratorUI(QMainWindow): - sensor_data_signal = Signal() - transform_signal = Signal() +class InteractiveCalibratorUI(ImageViewUI): object_point_signal = Signal(float, float, float) - external_calibration_points_signal = Signal() - optimized_intrinsics_signal = Signal() - - def __init__(self, ros_interface): - super().__init__() - self.setWindowTitle("Interactive camera-lidar calibration tool") - - # ROS Interface - self.ros_interface = ros_interface - self.ros_interface.set_sensor_data_callback(self.sensor_data_ros_callback) - self.ros_interface.set_transform_callback(self.transform_ros_callback) - self.ros_interface.set_object_point_callback(self.object_point_ros_callback) - self.ros_interface.set_external_calibration_points_callback( - self.external_calibration_points_ros_callback - ) - self.ros_interface.set_optimize_camera_intrinsics_status_callback( - self.optimize_camera_intrinsics_status_callback - ) - self.ros_interface.set_optimize_camera_intrinsics_result_callback( - self.optimize_camera_intrinsics_result_callback - ) - self.ros_interface.set_calibration_api_request_received_callback( - self.calibration_api_request_received_callback - ) - self.ros_interface.set_calibration_api_request_sent_callback( - self.calibration_api_request_sent_callback - ) - - self.sensor_data_signal.connect(self.sensor_data_callback) - self.transform_signal.connect(self.transform_callback) - self.object_point_signal.connect(self.object_point_callback) - self.external_calibration_points_signal.connect(self.external_calibration_points_callback) - self.optimized_intrinsics_signal.connect(self.optimized_camera_info_callback) - - # Threading variables - self.lock = threading.RLock() - self.transform_tmp = None - self.external_object_calibration_points_tmp = None - self.external_image_calibration_points_tmp = None - self.pixmap_tmp = None - self.camera_info_tmp = None - self.pointcloud_tmp = None - self.optimized_camera_info_tmp = None + def __init__(self, ros_interface: InteractiveCalibratorRosInterface): # Calibrator self.calibrator = Calibrator() @@ -108,37 +58,30 @@ def __init__(self, ros_interface): self.external_image_calibration_points = [] self.current_object_point = None self.current_image_point = None - self.camera_info = None self.optimized_camera_info = None - self.initial_transform = None - self.current_transform = None - self.calibrated_transform = None - self.source_transform = None - self.optimize_camera_intrinsics_status = False - self.optimize_camera_intrinsics_waiting = False self.calibration_possible = False self.calibrated_error = np.inf self.calibration_api_request_received = False - # Parent widget - self.central_widget = QWidget(self) - # self.central_widget.resize(1000,1000) + super().__init__(ros_interface) - self.setCentralWidget(self.central_widget) - self.layout = QHBoxLayout(self.central_widget) + self.setWindowTitle("Interactive camera-lidar calibration tool") - # Image View - self.make_image_view() + # ROS Interface + ros_interface.set_object_point_callback(self.object_point_ros_callback) - # Menu Widgets - self.left_menu_widget = QWidget(self.central_widget) - self.left_menu_widget.setFixedWidth(200) - self.left_menu_layout = QVBoxLayout(self.left_menu_widget) + ros_interface.set_calibration_api_request_received_callback( + self.calibration_api_request_received_callback + ) + ros_interface.set_calibration_api_request_sent_callback( + self.calibration_api_request_sent_callback + ) - self.right_menu_widget = QWidget(self.central_widget) - self.right_menu_widget.setFixedWidth(210) - self.right_menu_layout = QVBoxLayout(self.right_menu_widget) + self.object_point_signal.connect(self.object_point_callback) + + self.show() + def make_left_menu(self): # Calibration tools API group self.make_calibration_tools_api() @@ -148,8 +91,9 @@ def __init__(self, ros_interface): # Data collection group self.make_data_collection_options() - # Visualization group - self.make_vizualization_options() + self.left_menu_widget = QWidget(self.central_widget) + self.left_menu_widget.setFixedWidth(200) + self.left_menu_layout = QVBoxLayout(self.left_menu_widget) # self.menu_layout.addWidget(label) self.left_menu_layout.addWidget(self.calibration_api_group) @@ -157,35 +101,6 @@ def __init__(self, ros_interface): self.left_menu_layout.addWidget(self.calibration_status_group) self.left_menu_layout.addWidget(self.calibration_options_group) - self.right_menu_layout.addWidget(self.visualization_options_group) - - self.layout.addWidget(self.graphics_view) - # self.layout.addWidget(self.image_view) - - self.layout.addWidget(self.left_menu_widget) - self.layout.addWidget(self.right_menu_widget) - - self.show() - - def make_image_view(self): - self.image_view = ImageView() - # self.image_view.set_pixmap(pixmap) - self.image_view.clicked_signal.connect(self.image_click_callback) - - # We need the view to control the zoom - self.graphics_view = CustomQGraphicsView(self.central_widget) - self.graphics_view.setCacheMode(QGraphicsView.CacheBackground) - self.graphics_view.setViewportUpdateMode(QGraphicsView.BoundingRectViewportUpdate) - - # The scene contains the items - self.scene = QGraphicsScene() - - # Add the item into the scene - self.scene.addItem(self.image_view) - - # Add the scene into the view - self.graphics_view.setScene(self.scene) - def make_calibration_tools_api(self): self.calibration_api_group = QGroupBox("Calibration API") @@ -222,7 +137,7 @@ def calibration_api_button_callback(): self.calibration_status_points_label.setAlignment(Qt.AlignTop | Qt.AlignLeft) self.calibration_status_error_label = QLabel() - self.calibration_status_error_label.setText("Reproj error: ") + self.calibration_status_error_label.setText("r.error: ") self.calibration_status_error_label.setAlignment(Qt.AlignTop | Qt.AlignLeft) self.calibration_status_inliers_label = QLabel() @@ -230,7 +145,7 @@ def calibration_api_button_callback(): self.calibration_status_inliers_label.setAlignment(Qt.AlignTop | Qt.AlignLeft) self.state_1_message = ( - f"To add a calibration pair\nfirst click the 3d point." + "To add a calibration pair\nfirst click the 3d point." + "\nTo delete a calibration\npoint, click it in the\nimage" ) @@ -264,15 +179,11 @@ def make_calibration_options(self): self.calibration_button.setEnabled(False) def calibration_intrinsics_callback(): - self.ros_interface.optimize_camera_intrinsics( - self.object_calibration_points + self.external_object_calibration_points, - self.image_calibration_points + self.external_image_calibration_points, + self.optimized_camera_info = camera_lidar_calibrate_intrinsics( + np.array(self.object_calibration_points + self.external_object_calibration_points), + np.array(self.image_calibration_points + self.external_image_calibration_points), ) - - self.calibration2_button.setEnabled(False) - self.calibration2_button.setText("Optimizing...") - self.optimize_camera_intrinsics_waiting = True - assert self.optimize_camera_intrinsics_status == True + self.use_optimized_intrinsics_checkbox.setEnabled(True) self.calibration2_button = QPushButton("Calibrate intrinsics\n(experimental)") self.calibration2_button.clicked.connect(calibration_intrinsics_callback) @@ -324,11 +235,7 @@ def pnp_min_points_callback(): ) self.calibration_button.setEnabled(self.calibration_possible) - self.calibration2_button.setEnabled( - self.calibration_possible - and self.optimize_camera_intrinsics_status - and not self.optimize_camera_intrinsics_waiting - ) + self.calibration2_button.setEnabled(self.calibration_possible) pnp_min_points_label = QLabel("Minimum pnp\n points") self.pnp_min_points_spinbox = QSpinBox() @@ -401,198 +308,25 @@ def screenshot_callback(): self.screenshot_button.setEnabled(True) self.screenshot_button.clicked.connect(screenshot_callback) - def republish_data_callback(state): - self.ros_interface.set_republish_data(state == Qt.Checked) - - self.republish_data_checkbox = QCheckBox("Republish calibration\ndata") - self.republish_data_checkbox.stateChanged.connect(republish_data_callback) - self.republish_data_checkbox.setChecked(True) - - def republish_data_callback(state): - self.ros_interface.set_publish_tf(state == Qt.Checked) - self.publish_tf_checkbox = QCheckBox("Publish tf") - self.publish_tf_checkbox.stateChanged.connect(republish_data_callback) + self.publish_tf_checkbox.stateChanged.connect(self.ros_interface.set_publish_tf) self.publish_tf_checkbox.setChecked(True) data_collection_options_layout = QVBoxLayout() data_collection_options_layout.addWidget(self.pause_start_button) data_collection_options_layout.addWidget(self.screenshot_button) - data_collection_options_layout.addWidget(self.republish_data_checkbox) data_collection_options_layout.addWidget(self.publish_tf_checkbox) data_collection_options_layout.addStretch(1) self.data_collection_options_group.setLayout(data_collection_options_layout) - def make_vizualization_options(self): - self.visualization_options_group = QGroupBox("Visualization options") - self.visualization_options_group.setFlat(True) - - tf_source_label = QLabel("TF source:") - self.tf_source_combobox = QComboBox() - self.tf_source_combobox.currentTextChanged.connect(self.tf_source_callback) - - def marker_type_callback(value): - self.image_view.set_marker_type(value) - - marker_type_label = QLabel("Marker type:") - marker_type_combobox = QComboBox() - marker_type_combobox.currentTextChanged.connect(marker_type_callback) - marker_type_combobox.addItem("Circles") - marker_type_combobox.addItem("Rectangles") - - def marker_units_callback(value): - self.image_view.set_marker_units(value) - - marker_units_label = QLabel("Marker units:") - marker_units_combobox = QComboBox() - marker_units_combobox.currentTextChanged.connect(marker_units_callback) - marker_units_combobox.addItem("Meters") - marker_units_combobox.addItem("Pixels") - - def marker_color_callback(value): - self.image_view.set_color_channel(value) - - marker_color_label = QLabel("Marker color channel:") - marker_color_combobox = QComboBox() - marker_color_combobox.currentTextChanged.connect(marker_color_callback) - marker_color_combobox.addItem("Intensity") - marker_color_combobox.addItem("X") - marker_color_combobox.addItem("Y") - marker_color_combobox.addItem("Z") - - def marker_pixels_callback(value): - self.image_view.set_marker_size_pixels(value) - - marker_pixels_label = QLabel("Marker size (px)") - marker_pixels_spinbox = QSpinBox() - marker_pixels_spinbox.valueChanged.connect(marker_pixels_callback) - marker_pixels_spinbox.setRange(4, 100) - marker_pixels_spinbox.setSingleStep(1) - marker_pixels_spinbox.setValue(6) - - def marker_meters_callback(value): - self.image_view.set_marker_size_meters(value) - - marker_meters_label = QLabel("Marker size (m)") - marker_meters_spinbox = QDoubleSpinBox() - marker_meters_spinbox.valueChanged.connect(marker_meters_callback) - marker_meters_spinbox.setRange(0.01, 1.0) - marker_meters_spinbox.setSingleStep(0.01) - marker_meters_spinbox.setValue(0.05) - - def rainbow_distance_callback(value): - self.image_view.set_rainbow_distance(value) - - rainbow_distance_label = QLabel("Rainbow distance (m)") - rainbow_distance_spinbox = QDoubleSpinBox() - rainbow_distance_spinbox.valueChanged.connect(rainbow_distance_callback) - rainbow_distance_spinbox.setRange(0.0, 1000.0) - rainbow_distance_spinbox.setSingleStep(0.1) - rainbow_distance_spinbox.setValue(10.0) - - def rainbow_offset_callback(value): - self.image_view.set_rainbow_offset(value) - - rainbow_offset_label = QLabel("Rainbow offset") - rainbow_offset_spinbox = QDoubleSpinBox() - rainbow_offset_spinbox.valueChanged.connect(rainbow_offset_callback) - rainbow_offset_spinbox.setRange(0.0, 1.0) - rainbow_offset_spinbox.setSingleStep(0.05) - rainbow_offset_spinbox.setValue(0.0) - - def rendering_alpha_callback(value): - self.image_view.set_rendering_alpha(value) - - rendering_alpha_label = QLabel("Rendering alpha") - rendering_alpha_spinbox = QDoubleSpinBox() - rendering_alpha_spinbox.valueChanged.connect(rendering_alpha_callback) - rendering_alpha_spinbox.setRange(0.0, 1.0) - rendering_alpha_spinbox.setSingleStep(0.05) - rendering_alpha_spinbox.setValue(1.0) - - def marker_subsample_callback(value): - self.image_view.set_subsample_factor(value) - - marker_subsample_label = QLabel("PC subsample factor") - marker_subsample_spinbox = QSpinBox() - marker_subsample_spinbox.valueChanged.connect(marker_subsample_callback) - marker_subsample_spinbox.setRange(1, 10) - marker_subsample_spinbox.setSingleStep(1) - marker_subsample_spinbox.setValue(4) - - def rendering_min_distance_callback(value): - self.image_view.set_min_rendering_distance(value) - - rendering_min_distance_label = QLabel("Min rendering distance (m)") - rendering_min_distance_spinbox = QDoubleSpinBox() - rendering_min_distance_spinbox.valueChanged.connect(rendering_min_distance_callback) - rendering_min_distance_spinbox.setRange(0.01, 100.0) - rendering_min_distance_spinbox.setSingleStep(0.1) - rendering_min_distance_spinbox.setValue(0.1) - - def rendering_max_distance_callback(value): - self.image_view.set_max_rendering_distance(value) - - rendering_max_distance_label = QLabel("Max rendering distance (m)") - rendering_max_distance_spinbox = QDoubleSpinBox() - rendering_max_distance_spinbox.valueChanged.connect(rendering_max_distance_callback) - rendering_max_distance_spinbox.setRange(0.01, 100.0) - rendering_max_distance_spinbox.setSingleStep(0.1) - rendering_max_distance_spinbox.setValue(100.0) - - def render_pointcloud_callback(value): - self.image_view.set_draw_pointcloud(value == Qt.Checked) - - render_pointcloud_checkbox = QCheckBox("Render pointcloud") - render_pointcloud_checkbox.stateChanged.connect(render_pointcloud_callback) - render_pointcloud_checkbox.setChecked(True) - - def render_calibration_points_callback(value): - self.image_view.set_draw_calibration_points(value == Qt.Checked) - - render_calibration_points_checkbox = QCheckBox("Render calibration points") - render_calibration_points_checkbox.stateChanged.connect(render_calibration_points_callback) - render_calibration_points_checkbox.setChecked(True) - - def render_inliers_callback(value): - self.image_view.set_draw_inliers(value == Qt.Checked) - - self.render_inliers_checkbox = QCheckBox("Render inliers") - self.render_inliers_checkbox.stateChanged.connect(render_inliers_callback) - self.render_inliers_checkbox.setChecked(False) - self.render_inliers_checkbox.setEnabled(False) - - visualization_options_layout = QVBoxLayout() - visualization_options_layout.addWidget(tf_source_label) - visualization_options_layout.addWidget(self.tf_source_combobox) - visualization_options_layout.addWidget(marker_type_label) - visualization_options_layout.addWidget(marker_type_combobox) - visualization_options_layout.addWidget(marker_units_label) - visualization_options_layout.addWidget(marker_units_combobox) - visualization_options_layout.addWidget(marker_color_label) - visualization_options_layout.addWidget(marker_color_combobox) - visualization_options_layout.addWidget(render_pointcloud_checkbox) - visualization_options_layout.addWidget(render_calibration_points_checkbox) - visualization_options_layout.addWidget(self.render_inliers_checkbox) - - visualization_options_layout.addWidget(marker_pixels_label) - visualization_options_layout.addWidget(marker_pixels_spinbox) - visualization_options_layout.addWidget(marker_meters_label) - visualization_options_layout.addWidget(marker_meters_spinbox) - visualization_options_layout.addWidget(rainbow_distance_label) - visualization_options_layout.addWidget(rainbow_distance_spinbox) - visualization_options_layout.addWidget(rainbow_offset_label) - visualization_options_layout.addWidget(rainbow_offset_spinbox) - visualization_options_layout.addWidget(rendering_alpha_label) - visualization_options_layout.addWidget(rendering_alpha_spinbox) - visualization_options_layout.addWidget(marker_subsample_label) - visualization_options_layout.addWidget(marker_subsample_spinbox) - visualization_options_layout.addWidget(rendering_min_distance_label) - visualization_options_layout.addWidget(rendering_min_distance_spinbox) - visualization_options_layout.addWidget(rendering_max_distance_label) - visualization_options_layout.addWidget(rendering_max_distance_spinbox) - # visualization_options_layout.addStretch(1) - self.visualization_options_group.setLayout(visualization_options_layout) + def sensor_data_callback(self): + super().sensor_data_callback() + with self.lock: + self.calibrator.set_camera_info(self.camera_info_tmp.k, self.camera_info_tmp.d) + + def tf_source_callback(self, string): + super().tf_source_callback(string) + self.update_calibration_status() def save_calibration_callback(self): output_folder = QFileDialog.getExistingDirectory( @@ -605,7 +339,7 @@ def save_calibration_callback(self): if output_folder is None or output_folder == "": return - print(output_folder) + logging.info(output_folder) object_points = np.array( self.object_calibration_points + self.external_object_calibration_points, @@ -620,17 +354,18 @@ def save_calibration_callback(self): assert num_points == image_points.shape[0] assert 2 == image_points.shape[1] - np.savetxt(os.path.join(output_folder, "object_points.txt"), object_points) + np.savetxt( + os.path.join(output_folder, "object_points.txt"), object_points + ) # cSpell:ignore savetxt np.savetxt(os.path.join(output_folder, "image_points.txt"), image_points) if self.optimized_camera_info is not None: d = message_to_ordereddict(self.optimized_camera_info) - with open(os.path.join(output_folder, "optimized_camera_info.json"), "w") as fout: - fout.write(json.dumps(d, indent=4, sort_keys=False)) + with open(os.path.join(output_folder, "optimized_camera_info.json"), "w") as f: + f.write(json.dumps(d, indent=4, sort_keys=False)) self.ros_interface.save_calibration_tfs(output_folder) - pass def load_calibration_callback(self): input_dir = QFileDialog.getExistingDirectory( @@ -643,16 +378,18 @@ def load_calibration_callback(self): if input_dir is None or input_dir == "": return - print(input_dir) + logging.info(input_dir) - object_calibration_points = np.loadtxt(os.path.join(input_dir, "object_points.txt")) + object_calibration_points = np.loadtxt( + os.path.join(input_dir, "object_points.txt") + ) # cSpell:ignore loadtxt image_calibration_points = np.loadtxt(os.path.join(input_dir, "image_points.txt")) - self.object_calibration_points = [p for p in object_calibration_points] - self.image_calibration_points = [p for p in image_calibration_points] + self.object_calibration_points = list(object_calibration_points) + self.image_calibration_points = list(image_calibration_points) - print(self.object_calibration_points) - print(self.image_calibration_points) + logging.info(self.object_calibration_points) + logging.info(self.image_calibration_points) assert len(self.image_calibration_points) == len(self.object_calibration_points) @@ -662,18 +399,13 @@ def load_calibration_callback(self): ) self.calibration_button.setEnabled(self.calibration_possible) - self.calibration2_button.setEnabled( - self.calibration_possible - and self.optimize_camera_intrinsics_status - and not self.optimize_camera_intrinsics_waiting - ) + self.calibration2_button.setEnabled(self.calibration_possible) self.calibration_callback() self.image_view.set_calibration_points( self.object_calibration_points, self.image_calibration_points ) - pass def calibration_callback(self): if self.camera_info is None: @@ -724,7 +456,10 @@ def update_calibration_status(self): calibration_points_available = len(object_calibration_points) > 0 if self.calibrated_transform is not None and calibration_points_available: - calibrated_error, calibrated_inliers = self.calibrator.calculate_reproj_error( + ( + calibrated_error, + calibrated_inliers, + ) = self.calibrator.calculate_reproj_error( # cSpell:ignore reproj object_calibration_points, image_calibration_points, transform_matrix=self.calibrated_transform, @@ -748,54 +483,12 @@ def update_calibration_status(self): self.calibration_status_points_label.setText(f"#points: {len(object_calibration_points)}") self.calibration_status_error_label.setText( - f"Reproj error: {calibrated_error_string} / {source_error_string}" + f"R.error: {calibrated_error_string} / {source_error_string}" ) self.calibration_status_inliers_label.setText( f"inliers: {int(calibrated_inliers.sum())} / {int(source_inliers.sum())}" ) - def tf_source_callback(self, string): - string = string.lower() - - if "current" in string: - assert self.current_transform is not None - self.source_transform = self.current_transform - elif "initial" in string: - assert self.initial_transform is not None - self.source_transform = self.initial_transform - elif "calibrator" in string: - self.source_transform = self.calibrated_transform - else: - raise NotImplementedError - - self.update_calibration_status() - self.image_view.set_transform(self.source_transform) - - self.image_view.update() - - def sensor_data_ros_callback(self, img, camera_info, pointcloud): - # This method is executed in the ROS spin thread - with self.lock: - height, width, channel = img.shape - bytes_per_line = 3 * width - q_img = QImage( - img.data, width, height, bytes_per_line, QImage.Format_RGB888 - ).rgbSwapped() - self.pixmap_tmp = QPixmap(q_img) - - self.pointcloud_tmp = pointcloud - self.camera_info_tmp = camera_info - - self.sensor_data_signal.emit() - - def transform_ros_callback(self, transform): - # This method is executed in the ROS spin thread - with self.lock: - self.transform_tmp = transform - pass - - self.transform_signal.emit() - def object_point_ros_callback(self, point): assert np.prod(point.shape) == 3 point = point.reshape( @@ -803,37 +496,6 @@ def object_point_ros_callback(self, point): ) self.object_point_signal.emit(point[0], point[1], point[2]) - def external_calibration_points_ros_callback(self, object_points, image_points): - # This method is executed in the ROS spin thread - with self.lock: - self.external_object_calibration_points_tmp = object_points - self.external_image_calibration_points_tmp = image_points - - self.external_calibration_points_signal.emit() - - def optimize_camera_intrinsics_status_callback(self, service_status): - with self.lock: - self.optimize_camera_intrinsics_status = service_status - self.calibration2_button.setEnabled( - service_status - and self.calibration_possible - and not self.optimize_camera_intrinsics_waiting - ) - - def optimize_camera_intrinsics_result_callback(self, optimized_camera_info): - with self.lock: - self.calibration2_button.setEnabled( - self.calibration_possible and self.optimize_camera_intrinsics_status - ) - - self.calibration2_button.setText("Calibrate intrinsics\n(experimental)") - self.use_optimized_intrinsics_checkbox.setEnabled(True) - - self.optimized_camera_info_tmp = optimized_camera_info - self.optimize_camera_intrinsics_waiting = False - - self.optimized_intrinsics_signal.emit() - def calibration_api_request_received_callback(self): self.calibration_api_label.setStyleSheet( "QLabel { background-color : yellow; color : black; }" @@ -855,43 +517,6 @@ def calibration_api_request_sent_callback(self): self.calibration_api_request_received = False self.calibration_api_button.setEnabled(False) - def sensor_data_callback(self): - # This method is executed in the UI thread - with self.lock: - self.image_view.set_pixmap(self.pixmap_tmp) - self.image_view.set_pointcloud(self.pointcloud_tmp) - - if not ( - self.use_optimized_intrinsics_checkbox.isChecked() - and self.use_optimized_intrinsics_checkbox.isEnabled() - ): - self.camera_info = self.camera_info_tmp - - self.image_view.set_camera_info(self.camera_info_tmp.k, self.camera_info_tmp.d) - self.calibrator.set_camera_info(self.camera_info_tmp.k, self.camera_info_tmp.d) - - self.image_view.update() - self.graphics_view.update() - pass - - def transform_callback(self): - # This method is executed in the UI thread - with self.lock: - if self.initial_transform is None: - self.initial_transform = np.copy(self.transform_tmp) - self.current_transform = self.initial_transform - self.tf_source_combobox.addItem("Initial /tf") - self.tf_source_combobox.addItem("Current /tf") - - self.image_view.update() - - changed = (self.transform_tmp != self.current_transform).any() - self.current_transform = np.copy(self.transform_tmp) - - # the Current /tf case - if "current" in self.tf_source_combobox.currentText().lower() and changed: - self.tf_source_callback(self.tf_source_combobox.currentText()) - def image_click_callback(self, x, y): p = np.array([x, y]).reshape((2,)) @@ -921,11 +546,7 @@ def image_click_callback(self, x, y): ) self.calibration_button.setEnabled(self.calibration_possible) - self.calibration2_button.setEnabled( - self.calibration_possible - and self.optimize_camera_intrinsics_status - and not self.optimize_camera_intrinsics_waiting - ) + self.calibration2_button.setEnabled(self.calibration_possible) self.image_view.update() @@ -946,8 +567,8 @@ def delete_calibration_points(self, p): object_points = object_points[indexes, :] image_points = image_points[indexes, :] - self.object_calibration_points = [p for p in object_points] - self.image_calibration_points = [p for p in image_points] + self.object_calibration_points = list(object_points) + self.image_calibration_points = list(image_points) self.calibration_callback() self.image_view.set_calibration_points( @@ -963,17 +584,7 @@ def object_point_callback(self, x, y, z): self.current_object_point = point_array def external_calibration_points_callback(self): - with self.lock: - self.external_object_calibration_points = copy.deepcopy( - self.external_object_calibration_points_tmp - ) - self.external_image_calibration_points = copy.deepcopy( - self.external_image_calibration_points_tmp - ) - - self.image_view.set_external_calibration_points( - self.external_object_calibration_points, self.external_image_calibration_points_tmp - ) + super().external_calibration_points_callback() self.calibration_possible = ( len(self.image_calibration_points) + len(self.external_image_calibration_points) @@ -981,15 +592,7 @@ def external_calibration_points_callback(self): ) self.calibration_button.setEnabled(self.calibration_possible) - self.calibration2_button.setEnabled( - self.calibration_possible - and self.optimize_camera_intrinsics_status - and not self.optimize_camera_intrinsics_waiting - ) - - def optimized_camera_info_callback(self): - with self.lock: - self.optimized_camera_info = copy.deepcopy(self.optimized_camera_info_tmp) + self.calibration2_button.setEnabled(self.calibration_possible) def main(args=None): @@ -1001,14 +604,14 @@ def main(args=None): try: signal.signal(signal.SIGINT, sigint_handler) - ros_interface = RosInterface() - ex = InteractiveCalibratorUI(ros_interface) + ros_interface = InteractiveCalibratorRosInterface() + ex = InteractiveCalibratorUI(ros_interface) # noqa: F841 ros_interface.spin() sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quiting...") + logging.info("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py new file mode 100644 index 00000000..a34f3a7c --- /dev/null +++ b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py @@ -0,0 +1,191 @@ +#!/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. + +import json +import os +import threading +import time + +from geometry_msgs.msg import PointStamped +import numpy as np +import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.duration import Duration +from rclpy.qos import qos_profile_system_default +from rosidl_runtime_py.convert import message_to_ordereddict +from tf2_ros import TransformException +from tier4_calibration_msgs.msg import CalibrationResult +from tier4_calibration_msgs.srv import ExtrinsicCalibrator +from tier4_calibration_views.image_view_ros_interface import ImageViewRosInterface +from tier4_calibration_views.utils import decompose_transformation_matrix +from tier4_calibration_views.utils import tf_message_to_transform_matrix +from tier4_calibration_views.utils import transform_matrix_to_tf_message +from tier4_calibration_views.utils import transform_points +import transforms3d + + +class InteractiveCalibratorRosInterface(ImageViewRosInterface): + def __init__(self): + super().__init__("interactive_calibrator") + + self.lock = threading.RLock() + + self.declare_parameter("use_calibration_api", True) + self.declare_parameter("can_publish_tf", True) + + self.use_calibration_api = ( + self.get_parameter("use_calibration_api").get_parameter_value().bool_value + ) + self.can_publish_tf = self.get_parameter("can_publish_tf").get_parameter_value().bool_value + + self.new_output_tf = False + + self.calibration_error = np.inf + self.output_transform_msg = None + self.calibration_api_sent_pending = False + + # ROS Interface configuration + self.publish_tf = None + self.object_point_callback = None + self.calibration_api_request_received_callback = None + self.calibration_api_request_sent_callback = None + + self.point_sub = self.create_subscription( + PointStamped, "/clicked_point", self.point_callback, qos_profile_system_default + ) + + if self.use_calibration_api: + self.service_callback_group = MutuallyExclusiveCallbackGroup() + self.calibration_api_service_server = self.create_service( + ExtrinsicCalibrator, + "extrinsic_calibration", + self.calibration_api_service_callback, + callback_group=self.service_callback_group, + ) + + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + def send_calibration_api_result(self, calibration_error): + with self.lock: + self.calibration_api_sent_pending = True + self.calibration_error = calibration_error + + def calibration_api_service_callback( + self, request: ExtrinsicCalibrator.Request, response: ExtrinsicCalibrator.Response + ): + # Notify the UI that a request was received + self.calibration_api_request_received_callback() + + # Loop until the response arrives + while rclpy.ok(): + with self.lock: + if self.calibration_api_sent_pending: + break + + time.sleep(1.0) + + with self.lock: + assert self.output_transform_msg is not None + + result = CalibrationResult() + result.success = True + result.score = self.calibration_error + result.message = "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 + + response.results.append(result) + + self.calibration_api_request_sent_callback() + + return response + + def set_object_point_callback(self, callback): + with self.lock: + self.object_point_callback = callback + + def set_calibration_api_request_received_callback(self, callback): + self.calibration_api_request_received_callback = callback + + def set_calibration_api_request_sent_callback(self, callback): + self.calibration_api_request_sent_callback = callback + + def set_publish_tf(self, value): + with self.lock: + self.publish_tf = value + + def set_camera_lidar_transform(self, camera_optical_lidar_transform): + with self.lock: + self.output_transform_msg = transform_matrix_to_tf_message( + camera_optical_lidar_transform + ) + + self.output_transform_msg.header.frame_id = self.image_frame + self.output_transform_msg.child_frame_id = self.lidar_frame + self.new_output_tf = True + + def save_calibration_tfs(self, output_dir): + with self.lock: + d = message_to_ordereddict(self.output_transform_msg) + + q = self.output_transform_msg.transform.rotation + e = transforms3d.euler.quat2euler((q.w, q.x, q.y, q.z)) + + d["roll"] = e[0] + d["pitch"] = e[1] + d["yaw"] = e[2] + + with open(os.path.join(output_dir, "tf.json"), "w") as f: + f.write(json.dumps(d, indent=4, sort_keys=False)) + + def point_callback(self, point: PointStamped): + point_xyz = np.array([point.point.x, point.point.y, point.point.z]).reshape(1, 3) + + if point.header.frame_id != self.lidar_frame: + try: + lidar_to_point_frame_tf = self.tf_buffer.lookup_transform( + self.lidar_frame, + point.header.frame_id, + rclpy.time.Time(), + timeout=Duration(seconds=1.0), + ) + lidar_to_point_frame_transform = tf_message_to_transform_matrix( + lidar_to_point_frame_tf + ) + translation, rotation = decompose_transformation_matrix( + lidar_to_point_frame_transform + ) + + point_xyz = transform_points(translation, rotation, point_xyz) + except TransformException as ex: + self.get_logger().error( + f"Could not transform {self.lidar_frame} to {point.header.frame_id}: {ex}" + ) + return + + self.object_point_callback(point_xyz) + + def timer_callback(self): + super().timer_callback() + + with self.lock: + now = self.get_clock().now().to_msg() + + if self.publish_tf and self.new_output_tf and self.can_publish_tf: + self.output_transform_msg.header.stamp = now + self.tf_publisher.sendTransform(self.output_transform_msg) + self.new_output_tf = False diff --git a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/utils.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/utils.py new file mode 100644 index 00000000..ec31f747 --- /dev/null +++ b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/utils.py @@ -0,0 +1,100 @@ +#!/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. + +import array +from copy import deepcopy + +import cv2 +import numpy as np +from sensor_msgs.msg import CameraInfo + + +def get_calibration_flags( + fix_principal_point=False, fix_aspect_ratio=False, zero_tangent_dist=True, num_ks=2 +): + calib_flags = 0 + + if fix_principal_point: + calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT + if fix_aspect_ratio: + calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO + if zero_tangent_dist: + calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST + if num_ks > 3: + calib_flags |= cv2.CALIB_RATIONAL_MODEL + if num_ks < 6: + calib_flags |= cv2.CALIB_FIX_K6 + if num_ks < 5: + calib_flags |= cv2.CALIB_FIX_K5 + if num_ks < 4: + calib_flags |= cv2.CALIB_FIX_K4 + if num_ks < 3: + calib_flags |= cv2.CALIB_FIX_K3 + if num_ks < 2: + calib_flags |= cv2.CALIB_FIX_K2 + if num_ks < 1: + calib_flags |= cv2.CALIB_FIX_K1 + calib_flags |= cv2.CALIB_USE_INTRINSIC_GUESS + + return calib_flags + + +def camera_lidar_calibrate_intrinsics( + object_points: np.array, image_points: np.array, initial_camera_info: CameraInfo +): + object_points = object_points.astype(np.float32) + image_points = image_points.astype(np.float32) + + num_object_points, object_dim = object_points.shape + num_image_points, image_dim = image_points.shape + + assert num_object_points == num_image_points + assert object_dim == 3 + assert image_dim == 2 + + initial_k = np.array(initial_camera_info.k).reshape(3, 3) + initial_d = np.array(initial_camera_info.d).flatten() + + calib_flags = get_calibration_flags() + + _, new_k, new_d, _, _ = cv2.calibrateCamera( + [object_points.reshape(-1, 3)], + [image_points.reshape(-1, 1, 2)], + (initial_camera_info.width, initial_camera_info.height), + cameraMatrix=initial_k, + distCoeffs=initial_d, + flags=calib_flags, + ) + + optimized_camera_info = deepcopy(initial_camera_info) + optimized_camera_info.k = new_k.reshape(-1) + optimized_camera_info.d = array.array("d", new_d) + + ncm, _ = cv2.getOptimalNewCameraMatrix( + np.array(optimized_camera_info.k).reshape(3, 3), + np.array(optimized_camera_info.d).reshape(-1), + (optimized_camera_info.width, optimized_camera_info.height), + 0.0, + ) + + p = np.zeros((3, 4), dtype=np.float64) + + for j in range(3): + for i in range(3): + p[j, i] = ncm[j, i] + + optimized_camera_info.p = p.reshape(-1) + return optimized_camera_info diff --git a/sensor/extrinsic_interactive_calibrator/package.xml b/sensor/interactive_camera_lidar_calibrator/package.xml similarity index 84% rename from sensor/extrinsic_interactive_calibrator/package.xml rename to sensor/interactive_camera_lidar_calibrator/package.xml index 61b93703..0e1223a8 100644 --- a/sensor/extrinsic_interactive_calibrator/package.xml +++ b/sensor/interactive_camera_lidar_calibrator/package.xml @@ -1,19 +1,18 @@ - extrinsic_interactive_calibrator + interactive_camera_lidar_calibrator 0.0.0 TODO: Package description Kenzo Lobos Tsunekawa TODO: License declaration - intrinsic_camera_calibration python3-matplotlib python3-pyside2.qtquick python3-transforms3d - ros2_numpy ros2launch tier4_calibration_msgs + tier4_calibration_views ament_copyright ament_flake8 python3-pytest diff --git a/sensor/interactive_camera_lidar_calibrator/resource/interactive_camera_lidar_calibrator b/sensor/interactive_camera_lidar_calibrator/resource/interactive_camera_lidar_calibrator new file mode 100644 index 00000000..e69de29b diff --git a/sensor/interactive_camera_lidar_calibrator/setup.cfg b/sensor/interactive_camera_lidar_calibrator/setup.cfg new file mode 100644 index 00000000..835a7ed5 --- /dev/null +++ b/sensor/interactive_camera_lidar_calibrator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/interactive_camera_lidar_calibrator +[install] +install_scripts=$base/lib/interactive_camera_lidar_calibrator diff --git a/sensor/extrinsic_interactive_calibrator/setup.py b/sensor/interactive_camera_lidar_calibrator/setup.py similarity index 79% rename from sensor/extrinsic_interactive_calibrator/setup.py rename to sensor/interactive_camera_lidar_calibrator/setup.py index 54c32e1d..3873fd49 100644 --- a/sensor/extrinsic_interactive_calibrator/setup.py +++ b/sensor/interactive_camera_lidar_calibrator/setup.py @@ -1,6 +1,6 @@ from setuptools import setup -package_name = "extrinsic_interactive_calibrator" +package_name = "interactive_camera_lidar_calibrator" setup( name=package_name, @@ -19,7 +19,7 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "interactive_calibrator = extrinsic_interactive_calibrator.interactive_calibrator:main" + "interactive_calibrator = interactive_camera_lidar_calibrator.interactive_calibrator:main" ], }, ) diff --git a/sensor/interactive_camera_lidar_calibrator/test/test_pep257.py b/sensor/interactive_camera_lidar_calibrator/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/sensor/interactive_camera_lidar_calibrator/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/sensor/intrinsic_camera_calibration/CMakeLists.txt b/sensor/intrinsic_camera_calibration/CMakeLists.txt deleted file mode 100644 index 531576eb..00000000 --- a/sensor/intrinsic_camera_calibration/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(intrinsic_camera_calibration) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch -) -install(PROGRAMS - scripts/camera_intrinsics_optimizer.py - DESTINATION lib/${PROJECT_NAME} -) diff --git a/sensor/intrinsic_camera_calibration/launch/calibration.launch.xml b/sensor/intrinsic_camera_calibration/launch/calibration.launch.xml deleted file mode 100644 index 93bfcbcc..00000000 --- a/sensor/intrinsic_camera_calibration/launch/calibration.launch.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensor/intrinsic_camera_calibration/launch/camera_bfs.launch.xml b/sensor/intrinsic_camera_calibration/launch/camera_bfs.launch.xml deleted file mode 100644 index e1de972e..00000000 --- a/sensor/intrinsic_camera_calibration/launch/camera_bfs.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/sensor/intrinsic_camera_calibration/launch/camera_li.launch.xml b/sensor/intrinsic_camera_calibration/launch/camera_li.launch.xml deleted file mode 100644 index 5ac6b89d..00000000 --- a/sensor/intrinsic_camera_calibration/launch/camera_li.launch.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/sensor/intrinsic_camera_calibration/launch/optimizer.launch.xml b/sensor/intrinsic_camera_calibration/launch/optimizer.launch.xml deleted file mode 100644 index f22fc31f..00000000 --- a/sensor/intrinsic_camera_calibration/launch/optimizer.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/sensor/intrinsic_camera_calibration/package.xml b/sensor/intrinsic_camera_calibration/package.xml deleted file mode 100644 index 2ac19319..00000000 --- a/sensor/intrinsic_camera_calibration/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - intrinsic_camera_calibration - 0.1.0 - Intrinsic camera calibration tools - Akihito OHSATO - Apache 2 - - ament_cmake_auto - - cv_bridge - python3-nlopt - python3-opencv - sensor_msgs - std_msgs - usb_cam - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/sensor/intrinsic_camera_calibration/scripts/camera_intrinsics_optimizer.py b/sensor/intrinsic_camera_calibration/scripts/camera_intrinsics_optimizer.py deleted file mode 100755 index 7a572978..00000000 --- a/sensor/intrinsic_camera_calibration/scripts/camera_intrinsics_optimizer.py +++ /dev/null @@ -1,281 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2020 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. - -import array -from copy import deepcopy - -import cv2 -import nlopt -import numpy as np -import rclpy -from rclpy.node import Node -from tier4_calibration_msgs.srv import IntrinsicsOptimizer - - -class CameraIntrinsicsOptimizer(Node): - def __init__(self): - super().__init__("camera_intrinsics_optimizer") - - self.object_points = None - self.image_points = None - self.k0 = None - self.iteration = 0 - - self.declare_parameter("opt_method", "CV") - self.declare_parameter("opt_scale", 0.15) - self.declare_parameter("k_coefficients", 2) - self.declare_parameter("fix_principal_point", False) - self.declare_parameter("fix_aspect_ratio", False) - self.declare_parameter("zero_tangent_dist", False) - self.declare_parameter("no_distortion_model", False) - - self.method = self.get_parameter("opt_method").get_parameter_value().string_value - self.opt_allowed_percentage = self.get_parameter("opt_scale") - self.opt_allowed_percentage = self.opt_allowed_percentage.get_parameter_value().double_value - - self.num_ks = self.get_parameter("k_coefficients").get_parameter_value().integer_value - self.fix_principal_point = self.get_parameter("fix_principal_point") - self.fix_principal_point = self.fix_principal_point.get_parameter_value().bool_value - self.fix_aspect_ratio = self.get_parameter("fix_aspect_ratio") - self.fix_aspect_ratio = self.fix_aspect_ratio.get_parameter_value().bool_value - self.zero_tangent_dist = self.get_parameter("zero_tangent_dist") - self.zero_tangent_dist = self.zero_tangent_dist.get_parameter_value().bool_value - - # This options superseeds other configurations and sets a non-distorted intrinsics model - self.no_distortion_model = ( - self.get_parameter("no_distortion_model").get_parameter_value().bool_value - ) - - if self.no_distortion_model: - self.zero_tangent_dist = True - self.num_ks = 0 - - self.calib_flags = self.get_calibration_flags( - self.fix_principal_point, self.fix_aspect_ratio, self.zero_tangent_dist, self.num_ks - ) - - if self.method == "CV": - pass - elif self.method == "LN_COBYLA": - self.method = nlopt.LN_COBYLA - elif self.method == "LN_SBPLX": - self.method = nlopt.LN_SBPLX - elif self.method == "GN_CRS2_LM": - self.method = nlopt.GN_CRS2_LM - else: - raise NotImplementedError - - # Advertise service - self.opt_service = self.create_service( - IntrinsicsOptimizer, "optimize_intrinsics", self.service_callback - ) - - def get_calibration_flags( - self, fix_principal_point, fix_aspect_ratio, zero_tangent_dist, num_ks - ): - calib_flags = 0 - - if fix_principal_point: - calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT - if fix_aspect_ratio: - calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO - if zero_tangent_dist: - calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST - if num_ks > 3: - calib_flags |= cv2.CALIB_RATIONAL_MODEL - if num_ks < 6: - calib_flags |= cv2.CALIB_FIX_K6 - if num_ks < 5: - calib_flags |= cv2.CALIB_FIX_K5 - if num_ks < 4: - calib_flags |= cv2.CALIB_FIX_K4 - if num_ks < 3: - calib_flags |= cv2.CALIB_FIX_K3 - if num_ks < 2: - calib_flags |= cv2.CALIB_FIX_K2 - if num_ks < 1: - calib_flags |= cv2.CALIB_FIX_K1 - calib_flags |= cv2.CALIB_USE_INTRINSIC_GUESS - - return calib_flags - - def reproj_error(self, object_points, image_points, k): - d = np.zeros((5,)) - k = np.reshape(k, (3, 3)) - - _, rvec, tvec = cv2.solvePnP(object_points, image_points, k, d, flags=cv2.SOLVEPNP_SQPNP) - - num_points, dim = object_points.shape - projected_points, _ = cv2.projectPoints(object_points, rvec, tvec, k, d) - projected_points = projected_points.reshape((num_points, 2)) - reproj_error = np.linalg.norm(projected_points - image_points, axis=1).mean() - - return reproj_error - - def param_to_k(self, params): - k_opt = np.eye(3) - k_opt[0, 0] = self.fx0 + self.opt_allowed_percentage * self.fx0 * params[0] - k_opt[1, 1] = self.fy0 + self.opt_allowed_percentage * self.fy0 * params[1] - k_opt[0, 2] = self.cx0 + self.opt_allowed_percentage * self.cx0 * params[2] - k_opt[1, 2] = self.cy0 + self.opt_allowed_percentage * self.cy0 * params[3] - - return k_opt - - def opt_f(self, args): - k_opt = self.param_to_k(args) - - error = self.reproj_error(self.object_points, self.image_points, k_opt) - - if self.iteration % 100 == 0: - self.get_logger().info(f"iteration={self.iteration} -> error: {error}") - - self.iteration += 1 - - return error - - def optimize_nlopt(self, object_points, image_points, initial_camera_info): - self.object_points = object_points - self.image_points = image_points - - initial_k = np.array(initial_camera_info.k).reshape(3, 3) - initial_d = np.array(initial_camera_info.d).flatten() - - if np.abs(initial_d).sum() != 0.0: - self.get_logger().error("We only support distortion-less intrinsics for now") - return initial_camera_info - - self.k0 = np.array(initial_k) - self.fx0 = self.k0[0, 0] - self.fy0 = self.k0[1, 1] - self.cx0 = self.k0[0, 2] - self.cy0 = self.k0[1, 2] - - self.iteration = 0 - - x0 = [0.0, 0.0, 0.0, 0.0] - opt = nlopt.opt(nlopt.LN_SBPLX, 4) - - def f(x, grad): - return float(self.opt_f(x)) - - opt.set_min_objective(f) - opt.set_lower_bounds([-1.0, -1.0, -1.0, -1.0]) - opt.set_upper_bounds([1.0, 1.0, 1.0, 1.0]) - - tol = 1e-12 - opt.set_ftol_abs(tol) - opt.set_xtol_rel(np.sqrt(tol)) - - opt.set_maxeval(100000) - params = opt.optimize(x0) - optimized_k = self.param_to_k(params) - - self.get_logger().info("Optimization result") - self.get_logger().info(f"Parameters: {params}") - self.get_logger().info(f"Initial eval: {self.opt_f(x0)}") - self.get_logger().info(f"Final eval: {self.opt_f(params)}") - self.get_logger().info(f"Num evals: {opt.get_numevals()}") - self.get_logger().info(f"Initial K:\n {self.k0}") - self.get_logger().info(f"Final K:\n {optimized_k}") - - optimized_camera_info = deepcopy(initial_camera_info) - optimized_camera_info.k = optimized_k.reshape(-1) - - return optimized_camera_info - - def optimize_cv(self, object_points, image_points, initial_camera_info): - initial_k = np.array(initial_camera_info.k).reshape(3, 3) - initial_d = np.array(initial_camera_info.d).flatten() - - if self.no_distortion_model: - initial_d = np.zeros_like(initial_d) - - _, new_k, new_d, _, _ = cv2.calibrateCamera( - [object_points.reshape(-1, 3)], - [image_points.reshape(-1, 1, 2)], - (initial_camera_info.width, initial_camera_info.height), - cameraMatrix=initial_k, - distCoeffs=initial_d, - flags=self.calib_flags, - ) - - optimized_camera_info = deepcopy(initial_camera_info) - optimized_camera_info.k = new_k.reshape(-1) - optimized_camera_info.d = array.array("d", new_d) - - return optimized_camera_info - - def service_callback( - self, request: IntrinsicsOptimizer.Request, response: IntrinsicsOptimizer.Response - ): - points = request.calibration_points - initial_camera_info = request.initial_camera_info - - object_points = np.array([np.array([p.x, p.y, p.z]) for p in points.object_points]) - object_points = object_points.astype(np.float32) - - image_points = np.array([np.array([p.x, p.y]) for p in points.image_points]) - image_points = image_points.astype(np.float32) - - num_object_points, object_dim = object_points.shape - num_image_points, image_dim = image_points.shape - - assert num_object_points == num_image_points - assert object_dim == 3 - assert image_dim == 2 - - if self.method == "CV": - optimized_camera_info = self.optimize_cv( - object_points, image_points, initial_camera_info - ) - else: - optimized_camera_info = self.optimize_nlopt( - object_points, image_points, initial_camera_info - ) - - ncm, _ = cv2.getOptimalNewCameraMatrix( - np.array(optimized_camera_info.k).reshape(3, 3), - np.array(optimized_camera_info.d).reshape(-1), - (optimized_camera_info.width, optimized_camera_info.height), - 0.0, - ) - - p = np.zeros((3, 4), dtype=np.float64) - - for j in range(3): - for i in range(3): - p[j, i] = ncm[j, i] - - optimized_camera_info.p = p.reshape(-1) - response.optimized_camera_info = optimized_camera_info - - return response - - -def main(args=None): - try: - rclpy.init(args=args) - node = CameraIntrinsicsOptimizer() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/apriltag_grid_detection.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/apriltag_grid_detection.py index c4549643..bfc0208b 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/apriltag_grid_detection.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/apriltag_grid_detection.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index 8dd5f677..45ba7010 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index cf3c3412..4c1fa4c4 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/chess_board_detection.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/chess_board_detection.py index b3fbb3be..b5753930 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/chess_board_detection.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/chess_board_detection.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/dotboard_detection.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/dotboard_detection.py index 1a415a65..40469334 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/dotboard_detection.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/dotboard_detection.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py index e6c449d2..030ed9e9 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -30,7 +30,7 @@ class Detector(_Detector): def __del__(self): if self.tag_detector_ptr is not None: # destroy the detector - self.libc.apriltag_detector_destroy.restype = None + self.libc.apriltag_detector_destroy.restype = None # cSpell:ignore libc self.libc.apriltag_detector_destroy(self.tag_detector_ptr) # destroy the tag families @@ -84,7 +84,6 @@ def __init__(self, **kwargs): self.current_refine_edges = None self.current_decode_sharpening = None self.current_debug = None - pass def detect(self, img): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py index 3944fd11..e2a6dd6f 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector_factory.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector_factory.py index 05695387..e7132a02 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector_factory.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector_factory.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py index de3fb9ce..0e8457c9 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -30,7 +30,6 @@ def __init__(self, **kwargs): self.normalize_image = Parameter(bool, value=True, min_value=False, max_value=True) self.fast_check = Parameter(bool, value=True, min_value=False, max_value=True) self.refine = Parameter(bool, value=True, min_value=False, max_value=True) - pass def detect(self, img): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py index dc1a7e5c..3f6145b3 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -38,7 +38,6 @@ def __init__(self, **kwargs): self.min_dist_between_blobs_percentage = Parameter( float, value=1.0, min_value=0.1, max_value=10.0 ) - pass def detect(self, img: np.array): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/apriltag_grid_parameters.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/apriltag_grid_parameters.py index 7bacc932..4277935d 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/apriltag_grid_parameters.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/apriltag_grid_parameters.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters.py index c66e9ae6..520b7504 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters_factory.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters_factory.py index 7116e789..2b135d2d 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters_factory.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_parameters/board_parameters_factory.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/boards.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/boards.py index 4e729570..185db396 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/boards.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/boards.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py index 18fdbd68..cfc2f054 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -15,6 +15,7 @@ # limitations under the License. from enum import Enum +import logging import multiprocessing as mp import threading import time @@ -469,7 +470,7 @@ def _pre_rejection_filter_impl( inlier_mask = rms_errors < pre_rejection_max_rms_error num_inliers = inlier_mask.sum() - print( + logging.info( f"Iteration {it}: inliers: {num_inliers} | mean rms: {rms_errors.mean():.2f} | min rms: {rms_errors.min():.2f} | max rms: {rms_errors.max():.2f}" ) @@ -480,7 +481,7 @@ def _pre_rejection_filter_impl( min_error = rms_errors.mean() max_inliers = num_inliers - print( + logging.info( f"Pre rejection inliers = {max_inliers}/{len(detections)} | threshold = {pre_rejection_max_rms_error:.2f}" ) @@ -554,7 +555,7 @@ def _entropy_maximization_subsampling_impl( max_delta_entropy = delta_entropy max_delta_entropy_idx = idx - print(f"iteration={it}: delta entropy={max_delta_entropy:.3f}") + logging.info(f"iteration={it}: delta entropy={max_delta_entropy:.3f}") accepted_array[max_delta_entropy_idx] = True add_detection( detections[max_delta_entropy_idx], @@ -586,7 +587,7 @@ def _post_rejection_impl( ) inliers_mask = rms_error < post_rejection_max_rms_error - print( + logging.info( f"Post rejection inliers = {inliers_mask.sum()}/{len(detections)} | threshold = {post_rejection_max_rms_error}" ) diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py index 29416408..a94d8673 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 6c7bf29a..8109304f 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py index 1e738fec..dcc750e0 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py index 888b669e..e1b7a7b9 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index f5c66ce9..9bd5a7bf 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -17,6 +17,7 @@ from collections import defaultdict import copy +import logging from optparse import OptionParser import os import signal @@ -281,13 +282,13 @@ def delayed_change(): delayed_change() def on_training_sample_changed(index): - print(f"on_training_sample_changed={index}") + logging.debug(f"on_training_sample_changed={index}") self.training_sample_label.setText(f"Training sample: {index}") img = self.data_collector.get_training_image(index) self.process_db_data(img) def on_evaluation_sample_changed(index): - print(f"on_evaluation_sample_changed={index}") + logging.info(f"on_evaluation_sample_changed={index}") self.evaluation_sample_label.setText(f"Evaluation sample: {index}") img = self.data_collector.get_evaluation_image(index) self.process_db_data(img) @@ -316,7 +317,7 @@ def make_calibration_group(self): self.calibration_group.setFlat(True) self.calibrator_type_combobox = QComboBox() - self.calibrator_type_combobox.setEnabled(False) # TODO(knzo25): implement this later + self.calibrator_type_combobox.setEnabled(False) # TODO: implement this later self.calibration_parameters_button = QPushButton("Calibration parameters") self.calibration_button = QPushButton("Calibrate") @@ -338,7 +339,7 @@ def make_calibration_group(self): self.calibration_evaluation_inlier_rms_label = QLabel("\trms error (inlier):") def on_parameters_view_closed(): - # self.calibrator_type_combobox.setEnabled(True) TODO(knzo25): implement this later + # self.calibrator_type_combobox.setEnabled(True) TODO implement this later self.calibration_parameters_button.setEnabled(True) def on_parameters_button_clicked(): @@ -401,7 +402,7 @@ def on_evaluation_clicked(): CalibratorEnum.from_name(self.cfg["calibrator_type"]).get_id() ) except Exception as e: - print(f"Invalid calibration_type: {e}") + logging.error(f"Invalid calibration_type: {e}") else: self.calibrator_type_combobox.setCurrentIndex(0) @@ -429,7 +430,7 @@ def on_evaluation_clicked(): def make_detector_group(self): def detector_parameters_button_callback(): - print("detector_parameters_button_callback") + logging.info("detector_parameters_button_callback") self.detector_parameters_view = ParameterView(self.detector) self.detector_parameters_view.parameter_changed.connect(self.on_parameter_changed) @@ -509,7 +510,7 @@ def view_data_collection_statistics_callback(): else self.calibrated_camera_model ) - print("view_data_collection_statistics_callback") + logging.info("view_data_collection_statistics_callback") data_collection_statistics_view = DataCollectorView( self.data_collector.clone_without_images(), camera_model ) @@ -521,7 +522,7 @@ def data_collection_parameters_closed_callback(): def data_collection_parameters_callback(): self.data_collection_parameters_button.setEnabled(False) - print("data_collection_parameters_callback") + logging.debug("data_collection_parameters_callback") data_collection_parameters_view = ParameterView(self.data_collector) data_collection_parameters_view.closed.connect( data_collection_parameters_closed_callback @@ -647,10 +648,10 @@ def start( self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()})") - print("Init") - print(f"\tmode : {mode}") - print(f"\tdata_source : {data_source}") - print(f"\tboard_type : {board_type}") + logging.info("Init") + logging.info(f"\tmode : {mode}") + logging.info(f"\tdata_source : {data_source}") + logging.info(f"\tboard_type : {board_type}") detector_cfg = self.cfg[self.board_type.value["name"] + "_detector"] @@ -718,7 +719,7 @@ def process_calibration_results( f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}" ) - # self.calibrator_type_combobox.setEnabled(True) TODO(knzo25): implement this later + # self.calibrator_type_combobox.setEnabled(True) TODO implement this later self.calibration_parameters_button.setEnabled(True) self.calibration_button.setEnabled(True) self.evaluation_button.setEnabled(True) @@ -768,7 +769,7 @@ def process_evaluation_results( f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}" ) - # self.calibrator_type_combobox.setEnabled(True) TODO(knzo25): implement this later + # self.calibrator_type_combobox.setEnabled(True) TODO implement this later self.calibration_parameters_button.setEnabled(True) self.calibration_button.setEnabled(self.operation_mode == OperationMode.CALIBRATION) self.evaluation_button.setEnabled(True) @@ -788,7 +789,7 @@ def on_save_clicked(self): if output_folder is None or output_folder == "": return - print(f"Saving calibration results to {output_folder}") + logging.info(f"Saving calibration results to {output_folder}") save_intrinsics( self.calibrated_camera_model, @@ -1095,7 +1096,7 @@ def main(args=None): with open(options.config_file, "r") as stream: cfg = yaml.safe_load(stream) except Exception as e: - print(f"Could not load the parameters from the YAML file ({e})") + logging.error(f"Could not load the parameters from the YAML file ({e})") try: signal.signal(signal.SIGINT, sigint_handler) @@ -1103,7 +1104,7 @@ def main(args=None): ui = CameraIntrinsicsCalibratorUI(cfg) # noqa: F841 sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quitting...") + logging.info("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py index 57b26317..cf9edb8d 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 1dcab6e6..f740567d 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -133,7 +133,6 @@ def add_sample( self.detections.append(detection) self.pre_compute_stats(camera_model) - pass def pre_compute_stats(self, camera_model: CameraModel): """Compute a tensorized version of the statistics of the database. Needs to be called whenever a sample is added to the database.""" diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source.py index f1d412b0..67305953 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -36,7 +36,6 @@ def __init__(self, **kwargs): self.pending_data_not_consumed = False self.camera_name = "camera" self.paused = False - pass def set_data_callback(self, callback): """Set a callback method for the DataSource to call when an image is produced.""" diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source_factory.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source_factory.py index 78cdb44a..8421a8e7 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source_factory.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/data_source_factory.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py index 1229a98a..a72b3a14 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -14,6 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging + from PySide2.QtCore import QObject from PySide2.QtCore import QThread from PySide2.QtCore import Signal @@ -60,7 +62,7 @@ def consumed(self): def on_consumed(self): """Acts on the consumer having consumed an image. This method is executed in he source thread as it is connected to a local signal.""" if self.image_index == len(self.image_files_path) and not self.loop_images: - print("Produced all images!") + logging.info("Produced all images!") return if self.paused: diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py index 3500dbfa..42e58b75 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging from pathlib import Path from PySide2.QtCore import QObject @@ -125,7 +126,7 @@ def on_consumed(self): (topic, data, t) = self.reader.read_next() self.send_data(topic, data) else: - print("bag ended !", flush=True) + logging.info("bag ended !") def send_data(self, topic, data): """Send a image message to the consumer prior transformation to a numpy array.""" diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py index 0ffd0a31..784cbeda 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/video_file_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/video_file_data_source.py index d31bce8b..beebc61d 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/video_file_data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/video_file_data_source.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -21,4 +21,3 @@ class VideoFileDataSource(DataSource): def __init__(): super().__init__() - pass diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py index 24994b71..a35a7c1a 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/types.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/types.py index 703d27f6..e71aea1c 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/types.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/types.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py index 7b74d1f2..76ba328d 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py index c7592f1e..9ee09492 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_files_view.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_files_view.py index 0a77b8f7..2d3b7ff4 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_files_view.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_files_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index cbd422b3..1de55838 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index e8c74f23..02230174 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/parameter_view.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/parameter_view.py index b24fb2cc..2dfff052 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/parameter_view.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/parameter_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -16,6 +16,7 @@ from functools import partial +import logging from PySide2.QtCore import Signal from PySide2.QtWidgets import QCheckBox @@ -48,7 +49,7 @@ def __init__(self, parameterized_class: ParameterizedClass): self.layout.addWidget(label, i, 0) def on_value_changed(new_k, new_v): - print(f"on_value_changed {new_k}={new_v}") + logging.info(f"on_value_changed {new_k}={new_v}") self.parameterized_class.set_parameters(**{new_k: new_v}) self.parameter_changed.emit() diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_bag_view.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_bag_view.py index 6192cbfc..28ad40d8 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_bag_view.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_bag_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_topic_view.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_topic_view.py index eb4e10be..52d4c6b2 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_topic_view.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/ros_topic_view.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -14,6 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging + from PySide2.QtCore import Signal from PySide2.QtWidgets import QComboBox from PySide2.QtWidgets import QLabel @@ -112,7 +114,7 @@ def accept_callback(self): def closeEvent(self, event): """When the widget is closed it should be marked for deletion and notify the event.""" - print("Ros topic data view: closeEvent") + logging.debug("Ros topic data view: closeEvent") if not self.topic_selected: self.failed.emit() self.data_source.stop() diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml index f200f3a6..21ac7686 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml @@ -12,7 +12,6 @@ python3-pyside2.qtquick python3-ruamel.yaml python3-transforms3d - ros2_numpy ros2launch tier4_calibration_msgs ament_copyright diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/CMakeLists.txt b/sensor/lidar_to_lidar_2d_calibrator/CMakeLists.txt similarity index 65% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/CMakeLists.txt rename to sensor/lidar_to_lidar_2d_calibrator/CMakeLists.txt index df06f709..40a3db63 100755 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/CMakeLists.txt +++ b/sensor/lidar_to_lidar_2d_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_lidar_to_lidar_2d_calibrator) +project(lidar_to_lidar_2d_calibrator) find_package(autoware_cmake REQUIRED) find_package(OpenCV REQUIRED) # TODO: consider removing this one later @@ -14,12 +14,12 @@ ament_export_include_directories( # COMPILE THE SOURCE # ======================================================================== -ament_auto_add_executable(extrinsic_lidar_to_lidar_2d_calibrator - src/extrinsic_lidar_to_lidar_2d_calibrator.cpp +ament_auto_add_executable(lidar_to_lidar_2d_calibrator + src/lidar_to_lidar_2d_calibrator.cpp src/main.cpp ) -target_link_libraries(extrinsic_lidar_to_lidar_2d_calibrator +target_link_libraries(lidar_to_lidar_2d_calibrator ${OpenCV_LIBS} ) diff --git a/sensor/lidar_to_lidar_2d_calibrator/include/lidar_to_lidar_2d_calibrator/lidar_to_lidar_2d_calibrator.hpp b/sensor/lidar_to_lidar_2d_calibrator/include/lidar_to_lidar_2d_calibrator/lidar_to_lidar_2d_calibrator.hpp new file mode 100644 index 00000000..2f806136 --- /dev/null +++ b/sensor/lidar_to_lidar_2d_calibrator/include/lidar_to_lidar_2d_calibrator/lidar_to_lidar_2d_calibrator.hpp @@ -0,0 +1,214 @@ +// 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. + +#ifndef LIDAR_TO_LIDAR_2D_CALIBRATOR__LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ +#define LIDAR_TO_LIDAR_2D_CALIBRATOR__LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace lidar_to_lidar_2d_calibrator +{ + +using PointType = pcl::PointXYZ; + +/** + * A 2D lidar-lidar calibrator. + * This tools assumes the existence of two lidars that have been independently calibrated to the + * base frame (at least to the plane of the base plane), and uses that information to reduce the 3D + * lidar-lidar calibration problem into a 2D one (x, y, and rotation). Since lidars have different + * resolutions and FOV, this method selects a range of z values (measured from the base frame), + * flattens them (ignores the z value) and perform classic ICP to find the 2D transform. Optionally, + * this method also implemented basic filtering to improve the results. + * + * Since ICP works finding the closest target point for every source point, we formulate this + * problem as finding the tf from target to source (target should usually be the lidar with the + * highest resolution or FOV) + */ + +class LidarToLidar2DCalibrator : public rclcpp::Node +{ +public: + explicit LidarToLidar2DCalibrator(const rclcpp::NodeOptions & options); + +protected: + /*! + * External interface to start the calibration process and retrieve the result. + * The call gets blocked until the calibration finishes + * + * @param request An empty service request + * @param response A vector of calibration results + */ + void requestReceivedCallback( + const std::shared_ptr request, + const std::shared_ptr response); + + /*! + * Source pointcloud callback + * + * @param msg The source pointcloud in msg format + */ + void sourcePointCloudCallback(sensor_msgs::msg::PointCloud2::UniquePtr pc); + + /*! + * Target pointcloud callback + * + * @param msg The target pointcloud in msg format + */ + void targetPointCloudCallback(sensor_msgs::msg::PointCloud2::UniquePtr pc); + + /*! + * The main calibration function. + * Performs calibration, publishes debug pointclouds and optionally broadcasts the results and + * performs filtering + */ + void calibrationTimerCallback(); + + /*! + * Check if the required TFs are available and computes them + */ + bool checkInitialTransforms(); + + /*! + * Computes the alignment error between two pointclouds. + * For every point in the source pointcloud their closes point in the target pointcloud is + * computed and the distance between the points is used to determine the alignment error + * + * @param source_pointcloud The source pointcloud + * @param target_pointcloud The target pointcloud + * @return the alignment error + */ + double getAlignmentError( + pcl::PointCloud::Ptr source_pointcloud, + pcl::PointCloud::Ptr target_pointcloud); + + /*! + * The core optimization method + * Using multiple registration algorithms, looks for the transform that best solves ths + * source->target problem + * + * @param source_pointcloud_ptr The source pointcloud + * @param target_pointcloud_ptr The target pointcloud + * @param target_pointcloud_ptr A vector of pointcloud registrators to solve the calibration + * problem + * @return A tuple containing the best aligned pointcloud, transform, and score + */ + std::tuple::Ptr, Eigen::Matrix4f, float> findBestTransform( + pcl::PointCloud::Ptr & source_pointcloud_ptr, + pcl::PointCloud::Ptr & target_pointcloud_ptr, + std::vector::Ptr> & registrators); + + // Parameters + std::string base_frame_; + bool broadcast_calibration_tf_; + bool filter_estimations_; + double max_calibration_range_; + double max_corr_distance_; + int max_iterations_; + + double initial_angle_cov_; + double initial_xy_cov_; + double angle_measurement_cov_; + double angle_process_cov_; + double xy_measurement_cov_; + double xy_process_cov_; + double angle_convergence_threshold_; + double xy_convergence_threshold_; + + double min_z_; + double max_z_; + + // ROS Interface + tf2_ros::StaticTransformBroadcaster tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr transform_listener_; + + rclcpp::TimerBase::SharedPtr calibration_timer_; + + rclcpp::CallbackGroup::SharedPtr srv_callback_group_; + + rclcpp::Publisher::SharedPtr filtered_source_initial_pub_; + rclcpp::Publisher::SharedPtr filtered_source_aligned_pub_; + rclcpp::Publisher::SharedPtr filtered_target_pub_; + rclcpp::Publisher::SharedPtr flat_source_initial_pub_; + rclcpp::Publisher::SharedPtr flat_source_aligned_pub_; + rclcpp::Publisher::SharedPtr flat_target_pub_; + + rclcpp::Subscription::SharedPtr source_pointcloud_sub_; + rclcpp::Subscription::SharedPtr target_pointcloud_sub_; + + rclcpp::Service::SharedPtr service_server_; + + // Threading + std::mutex mutex_; + + // ROS Data + sensor_msgs::msg::PointCloud2::UniquePtr source_pointcloud_msg_; + sensor_msgs::msg::PointCloud2::UniquePtr target_pointcloud_msg_; + std_msgs::msg::Header source_pointcloud_header_; + std_msgs::msg::Header target_pointcloud_header_; + std::string source_pointcloud_frame_; + std::string target_pointcloud_frame_; + + // Initial tfs comparable with the one with our method + Eigen::Affine3d initial_base_to_source_eigen_; + Eigen::Affine3d initial_base_to_target_eigen_; + + std::vector::Ptr> calibration_registrators_; + pcl::GeneralizedIterativeClosestPoint::Ptr calibration_gicp_; + pcl::IterativeClosestPoint::Ptr calibration_icp_, calibration_icp_fine_, + calibration_icp_fine_2_, calibration_icp_fine_3_; + + // Calibration output + geometry_msgs::msg::TransformStamped output_calibration_msg_; + + bool got_initial_transform_{false}; + bool received_request_{false}; + bool broadcast_tf_; + bool calibration_done_{false}; + + // Filtering + KalmanFilter kalman_filter_; + bool first_observation_{true}; +}; + +} // namespace lidar_to_lidar_2d_calibrator + +#endif // LIDAR_TO_LIDAR_2D_CALIBRATOR__LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ diff --git a/sensor/lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml b/sensor/lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml new file mode 100644 index 00000000..f82cf848 --- /dev/null +++ b/sensor/lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/extrinsic_ground_plane_calibrator/package.xml b/sensor/lidar_to_lidar_2d_calibrator/package.xml old mode 100644 new mode 100755 similarity index 89% rename from sensor/extrinsic_ground_plane_calibrator/package.xml rename to sensor/lidar_to_lidar_2d_calibrator/package.xml index 5c64ad95..b94aedcd --- a/sensor/extrinsic_ground_plane_calibrator/package.xml +++ b/sensor/lidar_to_lidar_2d_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_ground_plane_calibrator + lidar_to_lidar_2d_calibrator 0.0.1 - The extrinsic_ground_plane_calibrator package + The lidar_to_lidar_2d_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz b/sensor/lidar_to_lidar_2d_calibrator/rviz/default.rviz similarity index 71% rename from sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz rename to sensor/lidar_to_lidar_2d_calibrator/rviz/default.rviz index 2a2e109a..59baffa9 100644 --- a/sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz +++ b/sensor/lidar_to_lidar_2d_calibrator/rviz/default.rviz @@ -6,11 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /left_upper1/Topic1 - - /initial_base_link1 - - /pandar_40p_left_inliers1/Topic1 - Splitter Ratio: 0.500627338886261 - Tree Height: 719 + - /target pointcloud (raw)1 + Splitter Ratio: 0.46037736535072327 + Tree Height: 1106 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -22,14 +20,13 @@ Panels: - Class: rviz_common/Views Expanded: - /Current View1 - - /Current View1/Position1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: front_lower + SyncSource: source pointcloud (raw) Visualization Manager: Class: "" Displays: @@ -51,7 +48,14 @@ Visualization Manager: Plane Cell Count: 40 Reference Frame: Value: true - - Alpha: 1 + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: base_link + Radius: 0.10000000149011612 + Reference Frame: base_link + Value: true + - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -61,7 +65,7 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: Intensity + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false @@ -69,7 +73,7 @@ Visualization Manager: Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: left_upper + Name: source pointcloud (raw) Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -81,7 +85,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/left_upper/pointcloud_raw + Value: /source_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -95,7 +99,7 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: Intensity + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false @@ -103,7 +107,7 @@ Visualization Manager: Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: front_lower + Name: target pointcloud (raw) Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -115,7 +119,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw + Value: /target_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -128,16 +132,16 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity + Color: 255; 0; 0 + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 191 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: rear_lower + Name: initial source pointcloud (filtered) Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -149,71 +153,78 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/rear_lower/pointcloud_raw + Value: /filtered_source_initial_points Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: base_link - Radius: 0.10000000149011612 - Reference Frame: base_link - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1 - Name: initial_base_link - Radius: 0.10000000149011612 - Reference Frame: initial_base_link - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: pandar_40p_left - Radius: 0.10000000149011612 - Reference Frame: pandar_40p_left - Value: true - - Class: rviz_default_plugins/Axes + - 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: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 Enabled: true - Length: 1 - Name: pandar_40p_front - Radius: 0.10000000149011612 - Reference Frame: pandar_40p_front + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: aligned source pointcloud (filtered) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /filtered_source_aligned_points + Use Fixed Frame: true + Use rainbow: true Value: true - - Class: rviz_default_plugins/Axes + - 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; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 Enabled: true - Length: 1 - Name: pandar_40p_rear - Radius: 0.10000000149011612 - Reference Frame: pandar_40p_rear - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1.5 - Name: ground_plane - Radius: 0.05000000074505806 - Reference Frame: ground_plane - Value: false - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 1.5 - Name: ground_plane_raw - Radius: 0.05000000074505806 - Reference Frame: ground_plane_raw - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray - Namespaces: - {} + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: target pointcloud (filtered) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable - Value: /base_link/sensor_kit_base_link/markers - Value: false + Reliability Policy: Best Effort + Value: /filtered_target_points + Use Fixed Frame: true + Use rainbow: true + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -223,19 +234,19 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 + Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 100 + Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 1 - Name: pandar_40p_left_inliers + Min Intensity: 0 + Name: initial source pointcloud (flattened) Position Transformer: XYZ Selectable: true - Size (Pixels): 5 + Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: @@ -244,7 +255,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /top_unit/base_link/top_unit_base_link/inliers + Value: /flat_source_initial_points Use Fixed Frame: true Use rainbow: true Value: true @@ -257,19 +268,19 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 + Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 100 + Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 1 - Name: pandar_40p_front_inliers + Min Intensity: 0 + Name: aligned source pointcloud (flattened) Position Transformer: XYZ Selectable: true - Size (Pixels): 5 + Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: @@ -278,7 +289,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /front_unit/base_link/front_unit_base_link/inliers + Value: /flat_source_aligned_points Use Fixed Frame: true Use rainbow: true Value: true @@ -291,19 +302,19 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 + Color: 255; 0; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 100 + Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 1 - Name: pandar_40p_rear_inliers + Min Intensity: 0 + Name: target pointcloud (flattened) Position Transformer: XYZ Selectable: true - Size (Pixels): 5 + Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: @@ -312,7 +323,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /rear_unit/base_link/rear_unit_base_link/inliers + Value: /flat_target_points Use Fixed Frame: true Use rainbow: true Value: true @@ -370,22 +381,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5850001573562622 + Pitch: 1.5697963237762451 Position: - X: -30.511119842529297 - Y: 2.103529930114746 - Z: 24.23221778869629 + X: 13.73879337310791 + Y: 25.009069442749023 + Z: 56.92116928100586 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 6.260024547576904 + Yaw: 6.2720112800598145 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002b40000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003670000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000031d000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000004ddfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004dd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000058d000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -394,6 +405,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1846 + Width: 2486 X: 1994 - Y: 27 + Y: 0 diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp b/sensor/lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp old mode 100755 new mode 100644 similarity index 53% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp rename to sensor/lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp index 1900f81e..5381753b --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp +++ b/sensor/lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include #include #include @@ -26,23 +27,15 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - +#include #include +#include -#define UNUSED(x) (void)x; +namespace lidar_to_lidar_2d_calibrator +{ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & options) -: Node("extrinsic_lidar_to_lidar_2d_calibrator", options), - tf_broadcaster_(this), - got_initial_transform_(false), - received_request_(false), - calibration_done_(false), - first_observation_(true) +: Node("lidar_to_lidar_2d_calibrator", options), tf_broadcaster_(this) { using std::chrono_literals::operator""ms; @@ -50,8 +43,6 @@ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & o transform_listener_ = std::make_shared(*tf_buffer_); base_frame_ = this->declare_parameter("base_frame", "base_link"); - sensor_kit_frame_ = this->declare_parameter("parent_frame"); - lidar_base_frame_ = this->declare_parameter("child_frame"); broadcast_calibration_tf_ = this->declare_parameter("broadcast_calibration_tf", false); filter_estimations_ = this->declare_parameter("filter_estimations", true); @@ -85,9 +76,18 @@ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & o min_z_ = this->declare_parameter("min_z", 0.2); max_z_ = this->declare_parameter("max_z", 0.6); - markers_pub_ = this->create_publisher("markers", 10); - source_pub_ = this->create_publisher("source_points_2d", 10); - target_pub_ = this->create_publisher("target_points_2d", 10); + filtered_source_initial_pub_ = + this->create_publisher("filtered_source_initial_points", 10); + filtered_source_aligned_pub_ = + this->create_publisher("filtered_source_aligned_points", 10); + filtered_target_pub_ = + this->create_publisher("filtered_target_points", 10); + flat_source_initial_pub_ = + this->create_publisher("flat_source_initial_points", 10); + flat_source_aligned_pub_ = + this->create_publisher("flat_source_aligned_points", 10); + flat_target_pub_ = + this->create_publisher("flat_target_points", 10); source_pointcloud_sub_ = this->create_subscription( "source_input_pointcloud", rclcpp::SensorDataQoS(), @@ -97,10 +97,10 @@ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & o "target_input_pointcloud", rclcpp::SensorDataQoS(), std::bind(&LidarToLidar2DCalibrator::targetPointCloudCallback, this, std::placeholders::_1)); - calib_timer_ = rclcpp::create_timer( + calibration_timer_ = rclcpp::create_timer( this, get_clock(), 200ms, std::bind(&LidarToLidar2DCalibrator::calibrationTimerCallback, this)); - // The service server runs in a dedicated thread + // The service server runs in a dedicated thread since it is a blocking call srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); service_server_ = this->create_service( @@ -140,17 +140,15 @@ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & o } void LidarToLidar2DCalibrator::requestReceivedCallback( - const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr + request, const std::shared_ptr response) { - // This tool uses several tfs, so for consistency we take the initial calibration using lookups - UNUSED(request); using std::chrono_literals::operator""s; { std::unique_lock lock(mutex_); received_request_ = true; - initial_calibration_ = request->initial_pose; } // Loop until the calibration finishes @@ -162,27 +160,33 @@ void LidarToLidar2DCalibrator::requestReceivedCallback( break; } - RCLCPP_WARN_SKIPFIRST(this->get_logger(), "Waiting for the calibration to end"); + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 10000, "Waiting for the calibration to end"); } - response->success = true; - response->result_pose = output_calibration_msg_; + tier4_calibration_msgs::msg::CalibrationResult result; + result.message.data = "Calibration successful"; + result.score = 0.f; + result.success = true; + result.transform_stamped = output_calibration_msg_; + + response->results.push_back(result); } void LidarToLidar2DCalibrator::sourcePointCloudCallback( - const sensor_msgs::msg::PointCloud2::SharedPtr msg) + sensor_msgs::msg::PointCloud2::UniquePtr msg) { source_pointcloud_frame_ = msg->header.frame_id; source_pointcloud_header_ = msg->header; - source_pointcloud_msg_ = msg; + source_pointcloud_msg_ = std::move(msg); } void LidarToLidar2DCalibrator::targetPointCloudCallback( - const sensor_msgs::msg::PointCloud2::SharedPtr msg) + sensor_msgs::msg::PointCloud2::UniquePtr msg) { target_pointcloud_frame_ = msg->header.frame_id; target_pointcloud_header_ = msg->header; - target_pointcloud_msg_ = msg; + target_pointcloud_msg_ = std::move(msg); } bool LidarToLidar2DCalibrator::checkInitialTransforms() @@ -199,33 +203,18 @@ bool LidarToLidar2DCalibrator::checkInitialTransforms() rclcpp::Time t = rclcpp::Time(0); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - initial_base_to_source_msg_ = + geometry_msgs::msg::Transform initial_base_to_source_msg_ = tf_buffer_->lookupTransform(base_frame_, source_pointcloud_frame_, t, timeout).transform; - initial_base_to_target_msg_ = + geometry_msgs::msg::Transform initial_base_to_target_msg_ = tf_buffer_->lookupTransform(base_frame_, target_pointcloud_frame_, t, timeout).transform; - fromMsg(initial_base_to_source_msg_, initial_base_to_source_tf2_); - fromMsg(initial_base_to_target_msg_, initial_base_to_target_tf2_); initial_base_to_source_eigen_ = tf2::transformToEigen(initial_base_to_source_msg_); initial_base_to_target_eigen_ = tf2::transformToEigen(initial_base_to_target_msg_); - base_to_sensor_kit_msg_ = - tf_buffer_->lookupTransform(base_frame_, sensor_kit_frame_, t, timeout).transform; - - lidar_base_to_source_msg_ = - tf_buffer_->lookupTransform(lidar_base_frame_, source_pointcloud_frame_, t, timeout) - .transform; - - fromMsg(base_to_sensor_kit_msg_, base_to_sensor_kit_tf2_); - base_to_sensor_kit_eigen_ = tf2::transformToEigen(base_to_sensor_kit_msg_); - - fromMsg(lidar_base_to_source_msg_, lidar_base_to_source_tf2_); - lidar_base_to_source_eigen_ = tf2::transformToEigen(lidar_base_to_source_msg_); - got_initial_transform_ = true; } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "could not get initial tf. %s", ex.what()); + RCLCPP_WARN(this->get_logger(), "Could not get initial tf. %s", ex.what()); return false; } @@ -235,102 +224,116 @@ bool LidarToLidar2DCalibrator::checkInitialTransforms() void LidarToLidar2DCalibrator::calibrationTimerCallback() { // Make sure we have all the required initial tfs - if (!checkInitialTransforms()) { + if (!checkInitialTransforms() || !source_pointcloud_msg_ || !target_pointcloud_msg_) { return; } + { + std::unique_lock lock(mutex_); + if (!received_request_) { + RCLCPP_WARN_ONCE( + this->get_logger(), "Attempted to calibrate before a request. Only printing this once"); + return; + } + } + + // nomenclature + // raw: non-processed pointcloud + // flat: null z-component + // scs = source coordinate system + // tcs = target coordinate system + // bcs = base coordinate system + // lidar coordinates pcl::PointCloud::Ptr raw_source_pointcloud_scs(new pcl::PointCloud); pcl::PointCloud::Ptr raw_target_pointcloud_tcs(new pcl::PointCloud); - pcl::PointCloud::Ptr source_pointcloud_scs(new pcl::PointCloud); - pcl::PointCloud::Ptr target_pointcloud_tcs(new pcl::PointCloud); // base coordinates pcl::PointCloud::Ptr raw_source_pointcloud_bcs(new pcl::PointCloud); pcl::PointCloud::Ptr raw_target_pointcloud_bcs(new pcl::PointCloud); - pcl::PointCloud::Ptr source_pointcloud_bcs(new pcl::PointCloud); - pcl::PointCloud::Ptr target_pointcloud_bcs(new pcl::PointCloud); + pcl::PointCloud::Ptr filtered_source_pointcloud_bcs(new pcl::PointCloud); + pcl::PointCloud::Ptr filtered_source_aligned_pointcloud_bcs( + new pcl::PointCloud); + pcl::PointCloud::Ptr filtered_target_pointcloud_bcs(new pcl::PointCloud); + pcl::PointCloud::Ptr flat_source_pointcloud_bcs(new pcl::PointCloud); + pcl::PointCloud::Ptr flat_source_aligned_pointcloud_bcs( + new pcl::PointCloud); + pcl::PointCloud::Ptr flat_target_pointcloud_bcs(new pcl::PointCloud); - // kit coordinate - pcl::PointCloud::Ptr source_pointcloud_kcs(new pcl::PointCloud); - pcl::PointCloud::Ptr target_pointcloud_kcs(new pcl::PointCloud); pcl::fromROSMsg(*source_pointcloud_msg_, *raw_source_pointcloud_scs); pcl::fromROSMsg(*target_pointcloud_msg_, *raw_target_pointcloud_tcs); + source_pointcloud_msg_.reset(); + target_pointcloud_msg_.reset(); - // Check input pointclouds and frames - // Turn them into base coordinates - - // Eigen::Matrix4f asd = initial_base_to_source_eigen_; pcl::transformPointCloud( *raw_source_pointcloud_scs, *raw_source_pointcloud_bcs, initial_base_to_source_eigen_); pcl::transformPointCloud( *raw_target_pointcloud_tcs, *raw_target_pointcloud_bcs, initial_base_to_target_eigen_); // Segment points in a range of z - for (auto & point : raw_source_pointcloud_bcs->points) { - if ( - point.z >= min_z_ && point.z <= max_z_ && - std::sqrt(point.x * point.x + point.y * point.y) <= max_calibration_range_) { - source_pointcloud_bcs->push_back(point); - } - } - - for (auto & point : raw_target_pointcloud_bcs->points) { - if ( - point.z >= min_z_ && point.z <= max_z_ && - std::sqrt(point.x * point.x + point.y * point.y) <= max_calibration_range_) { - target_pointcloud_bcs->push_back(point); - } + auto filter_point_range = [&](const auto & point) { + return point.z >= min_z_ && point.z <= max_z_ && + std::sqrt(point.x * point.x + point.y * point.y) <= max_calibration_range_; + }; + + std::copy_if( + raw_source_pointcloud_bcs->points.begin(), raw_source_pointcloud_bcs->points.end(), + std::back_inserter(filtered_source_pointcloud_bcs->points), filter_point_range); + std::copy_if( + raw_target_pointcloud_bcs->points.begin(), raw_target_pointcloud_bcs->points.end(), + std::back_inserter(filtered_target_pointcloud_bcs->points), filter_point_range); + filtered_source_pointcloud_bcs->width = filtered_source_pointcloud_bcs->size(); + filtered_source_pointcloud_bcs->height = 1; + filtered_target_pointcloud_bcs->width = filtered_target_pointcloud_bcs->size(); + filtered_target_pointcloud_bcs->height = 1; + + // Discard the z-component to perform 2D ICP + auto discard_z = [](auto & point) { point.z = 0.f; }; + + pcl::copyPointCloud(*filtered_source_pointcloud_bcs, *flat_source_pointcloud_bcs); + pcl::copyPointCloud(*filtered_target_pointcloud_bcs, *flat_target_pointcloud_bcs); + + std::for_each( + flat_source_pointcloud_bcs->points.begin(), flat_source_pointcloud_bcs->points.end(), + discard_z); + std::for_each( + flat_target_pointcloud_bcs->points.begin(), flat_target_pointcloud_bcs->points.end(), + discard_z); + + if (flat_source_pointcloud_bcs->size() == 0 || flat_target_pointcloud_bcs->size() == 0) { + RCLCPP_WARN( + this->get_logger(), + "Attempting to calibrate with empty clouds. Either the environment/sensor are not suitable " + "or the parameters need to be relaxed. source=%lu target=%lu", + filtered_source_pointcloud_bcs->size(), filtered_target_pointcloud_bcs->size()); + return; } - pcl::transformPointCloud( - *source_pointcloud_bcs, *source_pointcloud_scs, initial_base_to_source_eigen_.inverse()); - pcl::transformPointCloud( - *target_pointcloud_bcs, *target_pointcloud_tcs, initial_base_to_target_eigen_.inverse()); - - // Turn them into kit coordinates - pcl::transformPointCloud( - *source_pointcloud_bcs, *source_pointcloud_kcs, base_to_sensor_kit_eigen_.inverse()); - pcl::transformPointCloud( - *target_pointcloud_bcs, *target_pointcloud_kcs, base_to_sensor_kit_eigen_.inverse()); - - // Discard the z component - for (auto & point : source_pointcloud_kcs->points) { - point.z = 0.f; - } + // Perform 2D ICP + auto [best_source_aligned_pointcloud_bcs, best_transform, best_score] = findBestTransform( + flat_source_pointcloud_bcs, flat_target_pointcloud_bcs, calibration_registrators_); - for (auto & point : target_pointcloud_kcs->points) { - point.z = 0.f; + if (std::isnan(best_score) || std::isinf(best_score) || best_score < 0.f) { + RCLCPP_WARN(this->get_logger(), "ICP did not converge. Skipping iteration"); + return; } - pcl::PointCloud::Ptr source_pointcloud_aligned_kcs(new pcl::PointCloud); - Eigen::Matrix4f best_transform; - float best_score; - - findBestTransform( - source_pointcloud_kcs, target_pointcloud_kcs, calibration_registrators_, - source_pointcloud_aligned_kcs, best_transform, best_score); + double init_error = getAlignmentError(flat_source_pointcloud_bcs, flat_target_pointcloud_bcs); + double final_error = + getAlignmentError(best_source_aligned_pointcloud_bcs, flat_target_pointcloud_bcs); - double init_error = getAlignmentError(source_pointcloud_kcs, target_pointcloud_kcs); - double final_error = getAlignmentError(source_pointcloud_aligned_kcs, target_pointcloud_kcs); - - RCLCPP_INFO(this->get_logger(), "Initial avrage error: %.4f m", init_error); + RCLCPP_INFO(this->get_logger(), "Initial average error: %.4f m", init_error); RCLCPP_INFO(this->get_logger(), "Final average error: %.4f m", final_error); Eigen::Affine3d icp_affine = Eigen::Affine3d(best_transform.cast()); - Eigen::Affine3d inital_kit_to_source_eigen = - base_to_sensor_kit_eigen_.inverse() * initial_base_to_source_eigen_; - - Eigen::Affine3d estimated_kit_to_source_eigen = icp_affine * inital_kit_to_source_eigen; + Eigen::Affine3d filtered_affine = icp_affine; // Optional filtering if (filter_estimations_) { - auto estimated_rpy = - tier4_autoware_utils::getRPY(tf2::toMsg(estimated_kit_to_source_eigen).orientation); + // We force our RPY to avoid convention errors + auto estimated_rpy = tier4_autoware_utils::getRPY(tf2::toMsg(icp_affine).orientation); - Eigen::Vector3d x( - estimated_rpy.z, estimated_kit_to_source_eigen.translation().x(), - estimated_kit_to_source_eigen.translation().y()); + Eigen::Vector3d x(estimated_rpy.z, icp_affine.translation().x(), icp_affine.translation().y()); Eigen::DiagonalMatrix p0(initial_angle_cov_, initial_xy_cov_, initial_xy_cov_); if (first_observation_) { @@ -340,42 +343,86 @@ void LidarToLidar2DCalibrator::calibrationTimerCallback() kalman_filter_.update(x); } + // cSpell:ignore getXelement estimated_rpy.z = kalman_filter_.getXelement(0); - estimated_kit_to_source_eigen.translation().x() = kalman_filter_.getXelement(1); - estimated_kit_to_source_eigen.translation().y() = kalman_filter_.getXelement(2); + filtered_affine.translation().x() = kalman_filter_.getXelement(1); + filtered_affine.translation().y() = kalman_filter_.getXelement(2); - geometry_msgs::msg::Pose pose_msg; - pose_msg.orientation = tier4_autoware_utils::createQuaternionFromRPY( + geometry_msgs::msg::Quaternion quaternion_msg = tier4_autoware_utils::createQuaternionFromRPY( estimated_rpy.x, estimated_rpy.y, estimated_rpy.z); + Eigen::Quaterniond quaternion_eigen( + quaternion_msg.w, quaternion_msg.x, quaternion_msg.y, quaternion_msg.z); - pose_msg.position.x = kalman_filter_.getXelement(1); - pose_msg.position.y = kalman_filter_.getXelement(2); - pose_msg.position.z = estimated_kit_to_source_eigen.translation().z(); + filtered_affine.linear() = quaternion_eigen.matrix(); + } - tf2::fromMsg(pose_msg, estimated_kit_to_source_eigen); + // Compute the source_to_target_transform + Eigen::Affine3d target_to_source_transform = + initial_base_to_target_eigen_.inverse() * filtered_affine * initial_base_to_source_eigen_; + + // Optionally broadcast the current calibration TF + if (broadcast_calibration_tf_) { + geometry_msgs::msg::TransformStamped tf_msg = tf2::eigenToTransform(target_to_source_transform); + tf_msg.header.stamp = target_pointcloud_header_.stamp; + tf_msg.header.frame_id = target_pointcloud_frame_; + tf_msg.child_frame_id = source_pointcloud_frame_; + tf_broadcaster_.sendTransform(tf_msg); } - geometry_msgs::msg::TransformStamped tf_msg = - tf2::eigenToTransform(estimated_kit_to_source_eigen); - tf_msg.header.stamp = source_pointcloud_header_.stamp; - tf_msg.header.frame_id = sensor_kit_frame_; - tf_msg.child_frame_id = source_pointcloud_frame_; - tf_broadcaster_.sendTransform(tf_msg); - - // Publish the segmented pointclouds back in their frames (to evaluate visually the calibration) - sensor_msgs::msg::PointCloud2 source_pointcloud_scs_msg, target_pointcloud_tcs_msg; - pcl::toROSMsg(*source_pointcloud_scs, source_pointcloud_scs_msg); - pcl::toROSMsg(*target_pointcloud_tcs, target_pointcloud_tcs_msg); - source_pointcloud_scs_msg.header = source_pointcloud_header_; - target_pointcloud_tcs_msg.header = target_pointcloud_header_; - source_pub_->publish(source_pointcloud_scs_msg); - target_pub_->publish(target_pointcloud_tcs_msg); + pcl::transformPointCloud( + *filtered_source_pointcloud_bcs, *filtered_source_aligned_pointcloud_bcs, + filtered_affine.matrix().cast()); + + pcl::transformPointCloud( + *flat_source_pointcloud_bcs, *flat_source_aligned_pointcloud_bcs, + filtered_affine.matrix().cast()); + + // Publish visualization pointclouds + sensor_msgs::msg::PointCloud2 filtered_source_initial_pointcloud_bcs_msg, + filtered_source_aligned_pointcloud_bcs_msg, filtered_target_pointcloud_bcs_msg; + pcl::toROSMsg(*filtered_source_pointcloud_bcs, filtered_source_initial_pointcloud_bcs_msg); + pcl::toROSMsg( + *filtered_source_aligned_pointcloud_bcs, filtered_source_aligned_pointcloud_bcs_msg); + pcl::toROSMsg(*filtered_target_pointcloud_bcs, filtered_target_pointcloud_bcs_msg); + filtered_source_initial_pointcloud_bcs_msg.header.stamp = source_pointcloud_header_.stamp; + filtered_source_aligned_pointcloud_bcs_msg.header.stamp = source_pointcloud_header_.stamp; + filtered_target_pointcloud_bcs_msg.header.stamp = target_pointcloud_header_.stamp; + filtered_source_initial_pointcloud_bcs_msg.header.frame_id = base_frame_; + filtered_source_aligned_pointcloud_bcs_msg.header.frame_id = base_frame_; + filtered_target_pointcloud_bcs_msg.header.frame_id = base_frame_; + + filtered_source_initial_pub_->publish(filtered_source_initial_pointcloud_bcs_msg); + filtered_source_aligned_pub_->publish(filtered_source_aligned_pointcloud_bcs_msg); + filtered_target_pub_->publish(filtered_target_pointcloud_bcs_msg); + + sensor_msgs::msg::PointCloud2 flat_source_initial_pointcloud_bcs_msg, + flat_source_aligned_pointcloud_bcs_msg, flat_target_pointcloud_bcs_msg; + pcl::toROSMsg(*flat_source_pointcloud_bcs, flat_source_initial_pointcloud_bcs_msg); + pcl::toROSMsg(*flat_source_aligned_pointcloud_bcs, flat_source_aligned_pointcloud_bcs_msg); + pcl::toROSMsg(*flat_target_pointcloud_bcs, flat_target_pointcloud_bcs_msg); + flat_source_initial_pointcloud_bcs_msg.header.stamp = source_pointcloud_header_.stamp; + flat_source_aligned_pointcloud_bcs_msg.header.stamp = source_pointcloud_header_.stamp; + flat_target_pointcloud_bcs_msg.header.stamp = target_pointcloud_header_.stamp; + flat_source_initial_pointcloud_bcs_msg.header.frame_id = base_frame_; + flat_source_aligned_pointcloud_bcs_msg.header.frame_id = base_frame_; + flat_target_pointcloud_bcs_msg.header.frame_id = base_frame_; + + flat_source_initial_pub_->publish(flat_source_initial_pointcloud_bcs_msg); + flat_source_aligned_pub_->publish(flat_source_aligned_pointcloud_bcs_msg); + flat_target_pub_->publish(flat_target_pointcloud_bcs_msg); // We perform basic filtering on the estimated angles { std::unique_lock lock(mutex_); - if (filter_estimations_) { + output_calibration_msg_ = tf2::eigenToTransform(target_to_source_transform); + output_calibration_msg_.header.stamp = target_pointcloud_header_.stamp; + output_calibration_msg_.header.frame_id = target_pointcloud_frame_; + output_calibration_msg_.child_frame_id = source_pointcloud_frame_; + + if (!filter_estimations_) { + calibration_done_ = true; + } else { Eigen::MatrixXd p; kalman_filter_.getP(p); double yaw_cov = p(0, 0); @@ -385,8 +432,6 @@ void LidarToLidar2DCalibrator::calibrationTimerCallback() if ( yaw_cov < angle_convergence_threshold_ && x_cov < xy_convergence_threshold_ && y_cov < xy_convergence_threshold_) { - output_calibration_msg_ = - tf2::toMsg(estimated_kit_to_source_eigen * lidar_base_to_source_eigen_.inverse()); calibration_done_ = true; } @@ -396,11 +441,6 @@ void LidarToLidar2DCalibrator::calibrationTimerCallback() RCLCPP_INFO( this->get_logger(), "Convergence thresh: angle=%.2e, xy=%.2e", angle_convergence_threshold_, xy_convergence_threshold_); - - } else { - output_calibration_msg_ = - tf2::toMsg(estimated_kit_to_source_eigen * lidar_base_to_source_eigen_.inverse()); - calibration_done_ = true; } } } @@ -416,24 +456,23 @@ double LidarToLidar2DCalibrator::getAlignmentError( correspondence_estimator.determineCorrespondences(correspondences); double error = 0.0; - int n = 0; + std::size_t accepted_correspondences = 0; for (std::size_t i = 0; i < correspondences.size(); ++i) { if (correspondences[i].distance <= max_corr_distance_) { error += std::sqrt(correspondences[i].distance); - n += 1; + accepted_correspondences += 1; } } - return error / n; + return error / accepted_correspondences; } -void LidarToLidar2DCalibrator::findBestTransform( +std::tuple::Ptr, Eigen::Matrix4f, float> +LidarToLidar2DCalibrator::findBestTransform( pcl::PointCloud::Ptr & source_pointcloud_ptr, pcl::PointCloud::Ptr & target_pointcloud_ptr, - std::vector::Ptr> & registratators, - pcl::PointCloud::Ptr & best_aligned_pointcloud_ptr, Eigen::Matrix4f & best_transform, - float & best_score) + std::vector::Ptr> & registrators) { pcl::Correspondences correspondences; pcl::registration::CorrespondenceEstimation estimator; @@ -441,17 +480,17 @@ void LidarToLidar2DCalibrator::findBestTransform( estimator.setInputTarget(target_pointcloud_ptr); estimator.determineCorrespondences(correspondences); - best_transform = Eigen::Matrix4f::Identity(); - best_score = 0.f; - for (std::size_t i = 0; i < correspondences.size(); ++i) { - best_score += correspondences[i].distance; - } - - best_score /= correspondences.size(); + pcl::PointCloud::Ptr best_aligned_pointcloud_ptr(new pcl::PointCloud); + Eigen::Matrix4f best_transform = Eigen::Matrix4f::Identity(); + float best_score = + std::transform_reduce( + correspondences.cbegin(), correspondences.cend(), 0.f, std::plus{}, + [](const pcl::Correspondence & correspondence) { return correspondence.distance; }) / + correspondences.size(); std::vector transforms = {best_transform}; - for (auto & registrator : registratators) { + for (auto & registrator : registrators) { Eigen::Matrix4f best_registrator_transform = Eigen::Matrix4f::Identity(); float best_registrator_score = std::numeric_limits::max(); pcl::PointCloud::Ptr best_registrator_aligned_cloud_ptr( @@ -481,4 +520,8 @@ void LidarToLidar2DCalibrator::findBestTransform( transforms.push_back(best_registrator_transform); } -} + + return std::make_tuple<>(best_aligned_pointcloud_ptr, best_transform, best_score); +} // lidar_to_lidar_2d_calibrator + +} // namespace lidar_to_lidar_2d_calibrator diff --git a/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp b/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp new file mode 100644 index 00000000..7700f07d --- /dev/null +++ b/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp @@ -0,0 +1,34 @@ +// 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. + +#include +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + rclcpp::executors::MultiThreadedExecutor executor; + rclcpp::NodeOptions node_options; + std::shared_ptr node = + std::make_shared(node_options); + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt b/sensor/mapping_based_calibrator/CMakeLists.txt similarity index 76% rename from sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt rename to sensor/mapping_based_calibrator/CMakeLists.txt index 09f4c9fc..1b8d02f8 100644 --- a/sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt +++ b/sensor/mapping_based_calibrator/CMakeLists.txt @@ -1,7 +1,8 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_mapping_based_calibrator) +project(mapping_based_calibrator) +# cSpell:ignore DEIGEN #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") @@ -18,11 +19,6 @@ if(OPENMP_FOUND) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() -if(OPENMP_FOUND) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -endif() - ament_export_include_directories( include ${OpenCV_INCLUDE_DIRS} @@ -31,13 +27,13 @@ ament_export_include_directories( # COMPILE THE SOURCE # ======================================================================== -ament_auto_add_executable(extrinsic_mapping_based_calibrator +ament_auto_add_executable(mapping_based_calibrator src/filters/dynamics_filter.cpp src/filters/lost_state_filter.cpp src/filters/best_frames_filter.cpp src/filters/object_detection_filter.cpp src/calibration_mapper.cpp - src/extrinsic_mapping_based_calibrator.cpp + src/mapping_based_calibrator.cpp src/camera_calibrator.cpp src/lidar_calibrator.cpp src/base_lidar_calibrator.cpp @@ -46,7 +42,7 @@ ament_auto_add_executable(extrinsic_mapping_based_calibrator src/utils.cpp ) -target_link_libraries(extrinsic_mapping_based_calibrator +target_link_libraries(mapping_based_calibrator ${Boost_LIBRARIES}) add_definitions(${PCL_DEFINITIONS}) diff --git a/sensor/mapping_based_calibrator/include/mapping_based_calibrator/base_lidar_calibrator.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/base_lidar_calibrator.hpp new file mode 100644 index 00000000..7056d844 --- /dev/null +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/base_lidar_calibrator.hpp @@ -0,0 +1,70 @@ +// 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. + +#ifndef MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ +#define MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +class BaseLidarCalibrator : public SensorCalibrator +{ +public: + using Ptr = std::shared_ptr; + using PointPublisher = rclcpp::Publisher; + using PointSubscription = rclcpp::Subscription; + using FrameService = rclcpp::Service; + + BaseLidarCalibrator( + CalibrationParameters::Ptr & parameters, MappingData::Ptr & mapping_data, + std::shared_ptr & tf_buffer, tf2_ros::StaticTransformBroadcaster & broadcaster, + PointPublisher::SharedPtr & augmented_pointcloud_pub, + PointPublisher::SharedPtr & ground_pointcloud_pub); + + /*! + * Calibrate the sensor + * @returns a tuple containing the calibration success status, the transform, and a score + */ + std::tuple calibrate() override; + + void configureCalibrators() override {} + +protected: + /*! + * Publish the calibration results + */ + void publishResults( + const Eigen::Vector4d & ground_model, const Eigen::Matrix4f & pose, + const pcl::PointCloud::Ptr & ground_plane_inliers, + const pcl::PointCloud::Ptr & augmented_pointcloud_ptr); + + tf2_ros::StaticTransformBroadcaster & tf_broadcaster_; + PointPublisher::SharedPtr augmented_pointcloud_pub_; + PointPublisher::SharedPtr ground_pointcloud_pub_; +}; + +#endif // MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/calibration_mapper.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/calibration_mapper.hpp similarity index 91% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/calibration_mapper.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/calibration_mapper.hpp index b428dff8..6e5cc27d 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/calibration_mapper.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/calibration_mapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__CALIBRATION_MAPPER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__CALIBRATION_MAPPER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__CALIBRATION_MAPPER_HPP_ +#define MAPPING_BASED_CALIBRATOR__CALIBRATION_MAPPER_HPP_ #include -#include +#include #include #include #include @@ -48,6 +48,8 @@ class CalibrationMapper using PointSubscription = rclcpp::Subscription; using FrameService = rclcpp::Service; + enum State { INITIAL, MAPPING, FINISHED }; + CalibrationMapper( MappingParameters::Ptr & parameters, MappingData::Ptr & mapping_data, PointPublisher::SharedPtr & map_pub, @@ -99,9 +101,15 @@ class CalibrationMapper void dataMatchingTimerCallback(); /*! - * Whether or not map has stopped + * State of the mapper + * @return the state of the mapper + */ + State getState(); + + /*! + * Start the mapper */ - bool stopped(); + void start(); /*! * Stop the mapper @@ -117,7 +125,7 @@ class CalibrationMapper */ template - void mappingCalibrationDatamatching( + void mappingCalibrationDataMatching( const std::string & calibration_frame_name, std::list & calibration_msg_list, std::function @@ -197,8 +205,10 @@ class CalibrationMapper rclcpp::Client::SharedPtr rosbag2_pause_client_; rclcpp::Client::SharedPtr rosbag2_resume_client_; - pclomp::NormalDistributionsTransform ndt_; - pcl::GeneralizedIterativeClosestPoint gicp_; + // cSpell:ignore pclomp + pclomp::NormalDistributionsTransform::Ptr ndt_ptr_; + pcl::GeneralizedIterativeClosestPoint::Ptr gicp_ptr_; + pcl::Registration::Ptr registrator_ptr_; // ROS Data std_msgs::msg::Header::SharedPtr mapping_lidar_header_; @@ -224,7 +234,7 @@ class CalibrationMapper bool bag_resume_requested_; // External interface - bool stopped_; + State state_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__CALIBRATION_MAPPER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__CALIBRATION_MAPPER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/camera_calibrator.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/camera_calibrator.hpp similarity index 74% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/camera_calibrator.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/camera_calibrator.hpp index bb17e783..829dbc4d 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/camera_calibrator.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/camera_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__CAMERA_CALIBRATOR_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__CAMERA_CALIBRATOR_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__CAMERA_CALIBRATOR_HPP_ +#define MAPPING_BASED_CALIBRATOR__CAMERA_CALIBRATOR_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -28,6 +28,7 @@ #include #include +#include #include class CameraCalibrator : public SensorCalibrator @@ -45,17 +46,11 @@ class CameraCalibrator : public SensorCalibrator std::shared_ptr & tf_buffer, PointPublisher::SharedPtr & target_map_pub, MarkersPublisher::SharedPtr & target_markers_pub); - void singleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) override; - void multipleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) override; - /*! - * Calibrate the lidar + * Calibrate the sensor + * @returns a tuple containing the calibration success status, the transform, and a score */ - bool calibrate(Eigen::Matrix4f & best_transform, float & best_score) override; + std::tuple calibrate() override; /*! * Configure the calibrator parameters @@ -93,4 +88,4 @@ class CameraCalibrator : public SensorCalibrator Filter::Ptr filter_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__CAMERA_CALIBRATOR_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__CAMERA_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/best_frames_filter.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/best_frames_filter.hpp similarity index 76% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/best_frames_filter.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/best_frames_filter.hpp index 0847e9d4..c3188fed 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/best_frames_filter.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/best_frames_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__BEST_FRAMES_FILTER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__BEST_FRAMES_FILTER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__FILTERS__BEST_FRAMES_FILTER_HPP_ +#define MAPPING_BASED_CALIBRATOR__FILTERS__BEST_FRAMES_FILTER_HPP_ -#include -#include +#include +#include #include #include @@ -45,4 +45,4 @@ class BestFramesFilter : public Filter void setName(const std::string & name) override; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__BEST_FRAMES_FILTER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__FILTERS__BEST_FRAMES_FILTER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/dynamics_filter.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/dynamics_filter.hpp similarity index 77% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/dynamics_filter.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/dynamics_filter.hpp index f0dc67c9..4362d07a 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/dynamics_filter.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/dynamics_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__DYNAMICS_FILTER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__DYNAMICS_FILTER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__FILTERS__DYNAMICS_FILTER_HPP_ +#define MAPPING_BASED_CALIBRATOR__FILTERS__DYNAMICS_FILTER_HPP_ -#include -#include +#include +#include #include #include @@ -46,4 +46,4 @@ class DynamicsFilter : public Filter void setName(const std::string & name) override; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__DYNAMICS_FILTER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__FILTERS__DYNAMICS_FILTER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/filter.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/filter.hpp similarity index 82% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/filter.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/filter.hpp index ff5797f4..fda62d38 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/filter.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__FILTER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__FILTER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__FILTERS__FILTER_HPP_ +#define MAPPING_BASED_CALIBRATOR__FILTERS__FILTER_HPP_ -#include +#include #include #include @@ -48,4 +48,4 @@ class Filter std::string name_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__FILTER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__FILTERS__FILTER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/lost_state_filter.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/lost_state_filter.hpp similarity index 77% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/lost_state_filter.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/lost_state_filter.hpp index 7ebea281..2abff29c 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/lost_state_filter.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/lost_state_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__LOST_STATE_FILTER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__LOST_STATE_FILTER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__FILTERS__LOST_STATE_FILTER_HPP_ +#define MAPPING_BASED_CALIBRATOR__FILTERS__LOST_STATE_FILTER_HPP_ -#include -#include +#include +#include #include #include @@ -49,4 +49,4 @@ class LostStateFilter : public Filter private: }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__LOST_STATE_FILTER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__FILTERS__LOST_STATE_FILTER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/object_detection_filter.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/object_detection_filter.hpp similarity index 85% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/object_detection_filter.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/object_detection_filter.hpp index 6666c69d..0ea0f112 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/object_detection_filter.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/object_detection_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__OBJECT_DETECTION_FILTER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__OBJECT_DETECTION_FILTER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__FILTERS__OBJECT_DETECTION_FILTER_HPP_ +#define MAPPING_BASED_CALIBRATOR__FILTERS__OBJECT_DETECTION_FILTER_HPP_ -#include -#include +#include +#include #include @@ -67,4 +67,4 @@ class ObjectDetectionFilter : public Filter std::shared_ptr tf_buffer_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__OBJECT_DETECTION_FILTER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__FILTERS__OBJECT_DETECTION_FILTER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/sequential_filter.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/sequential_filter.hpp similarity index 83% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/sequential_filter.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/sequential_filter.hpp index 9645cae2..c30e2178 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/sequential_filter.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/filters/sequential_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__SEQUENTIAL_FILTER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__SEQUENTIAL_FILTER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__FILTERS__SEQUENTIAL_FILTER_HPP_ +#define MAPPING_BASED_CALIBRATOR__FILTERS__SEQUENTIAL_FILTER_HPP_ -#include -#include +#include +#include #include #include @@ -73,4 +73,4 @@ class SequentialFilter : public Filter std::vector filters_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__FILTERS__SEQUENTIAL_FILTER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__FILTERS__SEQUENTIAL_FILTER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/lidar_calibrator.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp similarity index 81% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/lidar_calibrator.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp index e6090be7..a444cf17 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/lidar_calibrator.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/lidar_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__LIDAR_CALIBRATOR_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__LIDAR_CALIBRATOR_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__LIDAR_CALIBRATOR_HPP_ +#define MAPPING_BASED_CALIBRATOR__LIDAR_CALIBRATOR_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -33,6 +33,7 @@ #include #include +#include #include class LidarCalibrator : public SensorCalibrator @@ -50,17 +51,11 @@ class LidarCalibrator : public SensorCalibrator PointPublisher::SharedPtr & calibrated_source_aligned_map_pub, PointPublisher::SharedPtr & target_map_pub); - void singleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) override; - void multipleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) override; - /*! - * Calibrate the lidar + * Calibrate the sensor + * @returns a tuple containing the calibration success status, the transform, and a score */ - bool calibrate(Eigen::Matrix4f & best_transform, float & best_score) override; + std::tuple calibrate() override; /*! * Configure the calibrator parameters @@ -114,17 +109,18 @@ class LidarCalibrator : public SensorCalibrator std::vector::Ptr> calibration_registrators_; std::vector::Ptr> calibration_batch_registrators_; + // cSpell:ignore pclomp pclomp::NormalDistributionsTransform::Ptr calibration_ndt_; pcl::GeneralizedIterativeClosestPoint::Ptr calibration_gicp_; pcl::IterativeClosestPoint::Ptr calibration_icp_coarse_; pcl::IterativeClosestPoint::Ptr calibration_icp_fine_; - pcl::IterativeClosestPoint::Ptr calibration_icp_ultrafine_; + pcl::IterativeClosestPoint::Ptr calibration_icp_ultra_fine_; pcl::registration::CorrespondenceEstimation::Ptr correspondence_estimator_; pcl::JointIterativeClosestPointExtended::Ptr calibration_batch_icp_coarse_; pcl::JointIterativeClosestPointExtended::Ptr calibration_batch_icp_fine_; pcl::JointIterativeClosestPointExtended::Ptr - calibration_batch_icp_ultrafine_; + calibration_batch_icp_ultra_fine_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__LIDAR_CALIBRATOR_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__LIDAR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_mapping_based_calibrator.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp similarity index 73% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_mapping_based_calibrator.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp index 30518943..89904eec 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_mapping_based_calibrator.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__EXTRINSIC_MAPPING_BASED_CALIBRATOR_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__EXTRINSIC_MAPPING_BASED_CALIBRATOR_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__MAPPING_BASED_CALIBRATOR_HPP_ +#define MAPPING_BASED_CALIBRATOR__MAPPING_BASED_CALIBRATOR_HPP_ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -59,15 +59,14 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node explicit ExtrinsicMappingBasedCalibrator(const rclcpp::NodeOptions & options); protected: - void cameraCalibrationRequestReceivedCallback( - const std::string & parent_frame, const std::string & calibration_camera_frame, - const std::string & calibration_camera_optical_frame, - const std::shared_ptr request, - const std::shared_ptr response); - - void lidarCalibrationRequestReceivedCallback( - const std::string & parent_frame, const std::string & calibration_lidar_base_frame, - const std::string & calibration_lidar_frame, + /*! + * External interface to start the calibration process and retrieve the result. + * The call gets blocked until the calibration finishes + * + * @param request An empty service request + * @param response A vector of calibration results + */ + void requestReceivedCallback( const std::shared_ptr request, const std::shared_ptr response); @@ -106,8 +105,7 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rclcpp::CallbackGroup::SharedPtr subs_callback_group_; - std::map srv_callback_groups_map_; + rclcpp::CallbackGroup::SharedPtr srv_callback_group_; std::map calibration_camera_info_subs_; std::map calibration_image_subs_; @@ -118,9 +116,7 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr predicted_objects_sub_; - std::map< - std::string, rclcpp::Service::SharedPtr> - calibration_api_server_map_; + rclcpp::Service::SharedPtr service_server_; rclcpp::Service::SharedPtr keyframe_map_server_; std::map single_lidar_calibration_server_map_; std::map multiple_lidar_calibration_server_map_; @@ -140,12 +136,6 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node // Calibration API std::map calibration_pending_map_; - std::map calibration_status_map_; - std::map calibration_results_map_; - std::map calibration_scores_map_; - std::map sensor_kit_frame_map_; // calibration parent frame - std::map calibration_lidar_base_frame_map_; // calibration child frame - std::map calibration_camera_frame_map_; // calibration child frame std::mutex service_mutex_; // Mapper @@ -153,9 +143,10 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node MappingData::Ptr mapping_data_; // Calibrators + bool calibration_pending_{false}; std::map camera_calibrators_; std::map lidar_calibrators_; BaseLidarCalibrator::Ptr base_lidar_calibrator_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__EXTRINSIC_MAPPING_BASED_CALIBRATOR_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__MAPPING_BASED_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/sensor_calibrator.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/sensor_calibrator.hpp similarity index 71% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/sensor_calibrator.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/sensor_calibrator.hpp index f8552a84..00ca1a57 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/sensor_calibrator.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/sensor_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__SENSOR_CALIBRATOR_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__SENSOR_CALIBRATOR_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__SENSOR_CALIBRATOR_HPP_ +#define MAPPING_BASED_CALIBRATOR__SENSOR_CALIBRATOR_HPP_ #include -#include +#include #include #include @@ -25,6 +25,7 @@ #include #include +#include class SensorCalibrator { @@ -39,17 +40,11 @@ class SensorCalibrator CalibrationParameters::Ptr & parameters, MappingData::Ptr & mapping_data, std::shared_ptr & tf_buffer); - virtual void singleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) = 0; - virtual void multipleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) = 0; - /*! * Calibrate the sensor + * @returns a tuple containing the calibration success status, the transform, and a score */ - virtual bool calibrate(Eigen::Matrix4f & best_transform, float & best_score) = 0; + virtual std::tuple calibrate() = 0; /*! * Configure the calibrator parameters @@ -63,7 +58,7 @@ class SensorCalibrator * @param[in] frame Frame to use as a center for constructing the pointcloud * @param[in] resolution Max resolution of the resulting pointcloud * @param[in] max_range Max range of the resulting pointcloud - * @retval Source to distance pointcloud distance + * @return Source to distance pointcloud distance */ PointcloudType::Ptr getDensePointcloudFromMap( const Eigen::Matrix4f & pose, const Frame::Ptr & frame, double resolution, double min_range, @@ -78,4 +73,4 @@ class SensorCalibrator std::shared_ptr tf_buffer_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__SENSOR_CALIBRATOR_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__SENSOR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/serialization.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/serialization.hpp similarity index 93% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/serialization.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/serialization.hpp index 08f4f7d4..4b31991b 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/serialization.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/serialization.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__SERIALIZATION_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__SERIALIZATION_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__SERIALIZATION_HPP_ +#define MAPPING_BASED_CALIBRATOR__SERIALIZATION_HPP_ #include -#include +#include #include #include @@ -132,8 +132,8 @@ template void serialize(Archive & ar, CalibrationFrame & frame, const unsigned int version) { (void)version; - ar & frame.source_camera_info; - ar & frame.source_image; + ar & frame.source_camera_info_; + ar & frame.source_image_; ar & frame.source_pointcloud_; ar & frame.source_header_; @@ -203,4 +203,4 @@ void serialize(Archive & ar, sensor_msgs::msg::CompressedImage & image, const un } // namespace serialization } // namespace boost -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__SERIALIZATION_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__SERIALIZATION_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/types.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/types.hpp similarity index 93% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/types.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/types.hpp index 36b79ef3..6ecede5e 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/types.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/types.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__TYPES_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__TYPES_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__TYPES_HPP_ +#define MAPPING_BASED_CALIBRATOR__TYPES_HPP_ #include @@ -86,8 +86,8 @@ struct CalibrationFrame using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - sensor_msgs::msg::CameraInfo::SharedPtr source_camera_info; - sensor_msgs::msg::CompressedImage::SharedPtr source_image; + sensor_msgs::msg::CameraInfo::SharedPtr source_camera_info_; + sensor_msgs::msg::CompressedImage::SharedPtr source_image_; PointcloudType::Ptr source_pointcloud_; std_msgs::msg::Header source_header_; @@ -112,7 +112,7 @@ struct MappingData std::vector calibration_camera_optical_link_frame_names; std::vector calibration_lidar_frame_names_; - std::mutex mutex_; + std::recursive_mutex mutex_; int n_processed_frames_{0}; std::list unprocessed_frames_; std::vector processed_frames_; @@ -136,6 +136,7 @@ struct MappingParameters using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; + std::string registrator_name_; bool mapping_verbose_; bool use_rosbag_; int mapping_max_frames_; @@ -202,7 +203,7 @@ struct CalibrationParameters int solver_iterations_; double max_corr_dist_coarse_; double max_corr_dist_fine_; - double max_corr_dist_ultrafine_; + double max_corr_dist_ultra_fine_; bool calibration_use_only_stopped_; bool calibration_use_only_last_frames_; @@ -223,6 +224,9 @@ struct CalibrationParameters int camera_calibration_max_frames_; // Base lidar calibration parameters; + bool calibrate_base_frame_; + std::string base_frame_; + double base_lidar_crop_box_min_x_; double base_lidar_crop_box_min_y_; double base_lidar_crop_box_min_z_; @@ -235,6 +239,7 @@ struct CalibrationParameters int base_lidar_min_plane_points_; double base_lidar_min_plane_points_percentage_; double base_lidar_max_cos_distance_; + bool base_lidar_overwrite_xy_yaw_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__TYPES_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__TYPES_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/utils.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/utils.hpp similarity index 88% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/utils.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/utils.hpp index 9d2630f2..bf12057b 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/utils.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__UTILS_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__UTILS_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__UTILS_HPP_ +#define MAPPING_BASED_CALIBRATOR__UTILS_HPP_ #include -#include +#include #include #include @@ -44,7 +44,7 @@ void transformPointcloud( * Crop a point cloud to a certain radius * @param[in] pointcloud Point cloud to crop * @param[in] max_range Range to crop the pointcloud to - * @retval Cropped pointcloud + * @return Cropped pointcloud */ template typename PointcloudType::Ptr cropPointCloud( @@ -54,10 +54,10 @@ typename PointcloudType::Ptr cropPointCloud( * Interpolate a transform between two points in time * @param[in] t Interpolation time t >t1 and t<=t2 * @param[in] t1 Left interpolation time - * @param[in] t2 Righti interpolation time + * @param[in] t2 Right interpolation time * @param[in] m1 Transformation at time t1 * @param[in] m2 Transformation at time t2 - * @retval Interpolated transform at time t + * @return Interpolated transform at time t */ Eigen::Matrix4f poseInterpolationBase( double t, double t1, double t2, Eigen::Matrix4f const & m1, Eigen::Matrix4f const & m2); @@ -66,10 +66,10 @@ Eigen::Matrix4f poseInterpolationBase( * Interpolate a transform between two points in time * @param[in] t Interpolation time (can be greater than t2 -> extrapolation) * @param[in] t1 Left interpolation time - * @param[in] t2 Righti interpolation time + * @param[in] t2 Right interpolation time * @param[in] m1 Transformation at time t1 * @param[in] m2 Transformation at time t2 - * @retval Interpolated transform at time t + * @return Interpolated transform at time t */ Eigen::Matrix4f poseInterpolation( double t, double t1, double t2, Eigen::Matrix4f const & m1, Eigen::Matrix4f const & m2); @@ -78,7 +78,7 @@ Eigen::Matrix4f poseInterpolation( * Compute the source to distance pointcloud distance * @param[in] estimator Correspondence estimator between source and target * @param[in] max_corr_distance Maximum distance allowed to be considered a correspondence [m] - * @retval Source to distance pointcloud distance + * @return Source to distance pointcloud distance */ template float sourceTargetDistance( @@ -91,7 +91,7 @@ float sourceTargetDistance( * @param[in] target Target pointcloud * @param[in] transform Target to input frame transform * @param[in] max_corr_distance Maximum distance allowed to be considered a correspondence [m] - * @retval Source to distance pointcloud distance + * @return Source to distance pointcloud distance */ template float sourceTargetDistance( @@ -105,7 +105,7 @@ float sourceTargetDistance( * @param[in] target Target pointcloud * @param[in] transform Target to input frame transform * @param[in] max_corr_distance Maximum distance allowed to be considered a correspondence [m] - * @retval Source to distance pointcloud distance + * @return Source to distance pointcloud distance */ template float sourceTargetDistance( @@ -129,7 +129,7 @@ void findBestTransform( bool verbose, Eigen::Matrix4f & best_transform, float & best_score); /*! - * Crop a target pointcloud to the ranges of a sorce one + * Crop a target pointcloud to the ranges of a source one * @param[in] initial_source_aligned_pc_ptr Pointcloud to use as a reference to crop a target * pointcloud * @param[out] target_dense_pc_ptr Pointcloud to be cropped @@ -139,4 +139,4 @@ void cropTargetPointcloud( const typename pcl::PointCloud::Ptr & initial_source_aligned_pc_ptr, typename pcl::PointCloud::Ptr & target_dense_pc_ptr, float max_radius); -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__UTILS_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__UTILS_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/voxel_grid_filter_wrapper.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/voxel_grid_filter_wrapper.hpp similarity index 84% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/voxel_grid_filter_wrapper.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/voxel_grid_filter_wrapper.hpp index cf4040fd..7ae62c00 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/voxel_grid_filter_wrapper.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/voxel_grid_filter_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MAPPING_BASED_CALIBRATOR__VOXEL_GRID_FILTER_WRAPPER_HPP_ -#define EXTRINSIC_MAPPING_BASED_CALIBRATOR__VOXEL_GRID_FILTER_WRAPPER_HPP_ +#ifndef MAPPING_BASED_CALIBRATOR__VOXEL_GRID_FILTER_WRAPPER_HPP_ +#define MAPPING_BASED_CALIBRATOR__VOXEL_GRID_FILTER_WRAPPER_HPP_ #include @@ -52,4 +52,4 @@ class VoxelGridWrapper pcl::VoxelGrid voxel_triplets; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__VOXEL_GRID_FILTER_WRAPPER_HPP_ +#endif // MAPPING_BASED_CALIBRATOR__VOXEL_GRID_FILTER_WRAPPER_HPP_ diff --git a/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml b/sensor/mapping_based_calibrator/launch/calibrator.launch.xml similarity index 85% rename from sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml rename to sensor/mapping_based_calibrator/launch/calibrator.launch.xml index 244d7d2e..126498ff 100644 --- a/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml +++ b/sensor/mapping_based_calibrator/launch/calibrator.launch.xml @@ -1,26 +1,27 @@ - + + + + + + + - - - - - - - + + @@ -58,21 +59,21 @@ + + - + + + + + - - - - - - @@ -82,9 +83,11 @@ + + @@ -123,10 +126,13 @@ + + + diff --git a/sensor/extrinsic_mapping_based_calibrator/package.xml b/sensor/mapping_based_calibrator/package.xml similarity index 90% rename from sensor/extrinsic_mapping_based_calibrator/package.xml rename to sensor/mapping_based_calibrator/package.xml index 14564cc8..fc6149da 100644 --- a/sensor/extrinsic_mapping_based_calibrator/package.xml +++ b/sensor/mapping_based_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_mapping_based_calibrator + mapping_based_calibrator 0.0.1 - The extrinsic_mapping_based_calibrator package + The mapping_based_calibrator package Kenzo Lobos Tsunekawa BSD @@ -20,7 +20,6 @@ kalman_filter libboost-filesystem libboost-serialization - libg2o libpcl-all-dev nav_msgs ndt_omp @@ -39,6 +38,7 @@ tier4_autoware_utils tier4_calibration_msgs tier4_calibration_pcl_extensions + tier4_ground_plane_utils visualization_msgs diff --git a/sensor/extrinsic_mapping_based_calibrator/rviz/x2.rviz b/sensor/mapping_based_calibrator/rviz/default.rviz similarity index 66% rename from sensor/extrinsic_mapping_based_calibrator/rviz/x2.rviz rename to sensor/mapping_based_calibrator/rviz/default.rviz index 4fd7d98a..15ddf925 100644 --- a/sensor/extrinsic_mapping_based_calibrator/rviz/x2.rviz +++ b/sensor/mapping_based_calibrator/rviz/default.rviz @@ -129,8 +129,8 @@ Visualization Manager: Class: rviz_default_plugins/Path Color: 255; 0; 0 Enabled: true - Head Diameter: 0.20000000298023224 - Head Length: 0.10000000149011612 + Head Diameter: 0.10000000149011612 + Head Length: 0.12999999523162842 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 @@ -142,7 +142,7 @@ Visualization Manager: Pose Color: 255; 0; 0 Pose Style: Arrows Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 + Shaft Diameter: 0.019999999552965164 Shaft Length: 0.05000000074505806 Topic: Depth: 5 @@ -152,12 +152,40 @@ Visualization Manager: Reliability Policy: Reliable Value: /frame_path Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.07999999821186066 + Head Length: 0.12999999523162842 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Frame Predicted Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 255; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.019999999552965164 + Shaft Length: 0.03999999910593033 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /frame_predicted_path + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.30000001192092896 + Head Diameter: 0.10000000149011612 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines @@ -170,7 +198,7 @@ Visualization Manager: Pose Color: 0; 255; 0 Pose Style: Arrows Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 + Shaft Diameter: 0.05999999865889549 Shaft Length: 0.10000000149011612 Topic: Depth: 5 @@ -211,7 +239,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_right_initial_source_aligned_map + Name: calibration_lidar_0_initial_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -223,7 +251,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_right/initial_source_aligned_map + Value: /calibration_lidar_0/initial_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -245,7 +273,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_right_calibrated_source_aligned_map + Name: calibration_lidar_0_calibrated_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -257,7 +285,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_right/calibrated_source_aligned_map + Value: /calibration_lidar_0/calibrated_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -279,7 +307,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_right_target_map + Name: calibration_lidar_0_target Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -291,7 +319,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_right/target_map + Value: /calibration_lidar_0/target_map Use Fixed Frame: true Use rainbow: true Value: true @@ -313,7 +341,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_left_initial_source_aligned_map + Name: calibration_lidar_1_initial_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -325,7 +353,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_left/initial_source_aligned_map + Value: /calibration_lidar_1/initial_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -347,7 +375,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_left_calibrated_source_aligned_map + Name: calibration_lidar_1_calibrated_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -359,7 +387,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_left/calibrated_source_aligned_map + Value: /calibration_lidar_1/calibrated_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -381,7 +409,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_left_target_map + Name: calibration_lidar_1_target Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -393,7 +421,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_left/target_map + Value: /calibration_lidar_1/target_map Use Fixed Frame: true Use rainbow: true Value: true @@ -415,7 +443,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_right_initial_source_aligned_map + Name: calibration_lidar_2_initial_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -427,7 +455,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_right/initial_source_aligned_map + Value: /calibration_lidar_2/initial_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -449,7 +477,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_right/calibrated_source_aligned_map + Name: calibration_lidar_2_calibrated_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -461,7 +489,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: pandar_qt_right/calibrated_source_aligned_map + Value: /calibration_lidar_2/calibrated_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -483,7 +511,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_right_target_map + Name: calibration_lidar_2_target Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -495,7 +523,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: pandar_qt_right/target_map + Value: /calibration_lidar_2/target_map Use Fixed Frame: true Use rainbow: true Value: true @@ -517,7 +545,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_front_initial_source_aligned_map + Name: calibration_lidar_3_initial_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -529,7 +557,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_front/initial_source_aligned_map + Value: /calibration_lidar_3/initial_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -551,7 +579,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_front_calibrated_source_aligned_map + Name: calibration_lidar_3_calibrated_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -563,7 +591,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_front/calibrated_source_aligned_map + Value: /calibration_lidar_3/calibrated_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -585,7 +613,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_front_target_map + Name: calibration_lidar_3_target Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -597,7 +625,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_front/target_map + Value: /calibration_lidar_3/target_map Use Fixed Frame: true Use rainbow: true Value: true @@ -619,7 +647,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_front_initial_source_aligned_map + Name: calibration_lidar_4_initial_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -631,7 +659,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_front/initial_source_aligned_map + Value: /calibration_lidar_4/initial_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -653,7 +681,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_front_calibrated_source_aligned_map + Name: calibration_lidar_4_calibrated_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -665,7 +693,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_front/calibrated_source_aligned_map + Value: /calibration_lidar_4/calibrated_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -687,7 +715,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_front_target_map + Name: calibration_lidar_4_target Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -699,7 +727,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_front/target_map + Value: /calibration_lidar_4/target_map Use Fixed Frame: true Use rainbow: true Value: true @@ -721,7 +749,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_rear_initial_source_aligned_map + Name: calibration_lidar_5_initial_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -733,7 +761,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_rear/initial_source_aligned_map + Value: /calibration_lidar_5/initial_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -755,7 +783,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_rear_calibrated_source_aligned_map + Name: calibration_lidar_5_calibrated_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -767,7 +795,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_rear/calibrated_source_aligned_map + Value: /calibration_lidar_5/calibrated_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -789,7 +817,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_40p_rear_target_map + Name: calibration_lidar_5_target Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -801,7 +829,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_40p_rear/target_map + Value: /calibration_lidar_5/target_map Use Fixed Frame: true Use rainbow: true Value: true @@ -823,7 +851,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_rear_initial_source_aligned_map + Name: calibration_lidar_6_initial_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -835,7 +863,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_rear/initial_source_aligned_map + Value: /calibration_lidar_6/initial_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -857,7 +885,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_rear_calibrated_source_aligned_map + Name: calibration_lidar_6_calibrated_source_aligned Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -869,7 +897,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_rear/calibrated_source_aligned_map + Value: /calibration_lidar_6/calibrated_source_aligned_map Use Fixed Frame: true Use rainbow: true Value: true @@ -891,7 +919,351 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: pandar_qt_rear_target_map + Name: calibration_lidar_6_target + 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: Reliable + Value: /calibration_lidar_6/target_map + Use Fixed Frame: true + Use rainbow: true + 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: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: camera0_target + 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: Reliable + Value: /camera0/camera_optical_link/target_map + Use Fixed Frame: true + Use rainbow: true + 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: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: camera1_target + 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: Reliable + Value: /camera1/camera_optical_link/target_map + Use Fixed Frame: true + Use rainbow: true + 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; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: camera2_target + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera2/camera_optical_link/target_map + Use Fixed Frame: true + Use rainbow: true + 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: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: camera3_target + 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: Reliable + Value: /camera3/camera_optical_link/target_map + Use Fixed Frame: true + Use rainbow: true + 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: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: camera4_target + 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: Reliable + Value: /camera4/camera_optical_link/target_map + Use Fixed Frame: true + Use rainbow: true + 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: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: camera5_target + 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: Reliable + Value: /camera5/camera_optical_link/target_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: camera_0_markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera0/camera_optical_link/target_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: camera_1_markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera1/camera_optical_link/target_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: camera_2_markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera2/camera_optical_link/target_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: camera_3_markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera3/camera_optical_link/target_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: camera_4_markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera4/camera_optical_link/target_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: camera_5_markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera5/camera_optical_link/target_markers + 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: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: base_lidar_augmented + 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: Reliable + Value: /base_lidar_augmented_pointcloud + Use Fixed Frame: true + Use rainbow: true + 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: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: base_lidar_ground Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -903,7 +1275,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /pandar_qt_rear/target_map + Value: /ground_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -961,14 +1333,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.22979731857776642 + Pitch: 0.1600005179643631 Position: - X: 5.110201835632324 - Y: -0.12188102304935455 - Z: 1.4448553323745728 + X: 0.22517220675945282 + Y: 8.361368179321289 + Z: 1.8489781618118286 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 3.1094319820404053 + Yaw: 4.6881561279296875 Saved: ~ Window Geometry: Displays: @@ -976,7 +1348,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002df0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004510000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002330000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004fd0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp b/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp new file mode 100644 index 00000000..aa328a27 --- /dev/null +++ b/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp @@ -0,0 +1,168 @@ +// 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. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +BaseLidarCalibrator::BaseLidarCalibrator( + CalibrationParameters::Ptr & parameters, MappingData::Ptr & mapping_data, + std::shared_ptr & tf_buffer, tf2_ros::StaticTransformBroadcaster & broadcaster, + PointPublisher::SharedPtr & augmented_pointcloud_pub, + PointPublisher::SharedPtr & ground_pointcloud_pub) +: SensorCalibrator( + mapping_data->mapping_lidar_frame_, + "base_lidar_calibrator(" + mapping_data->mapping_lidar_frame_ + ")", parameters, mapping_data, + tf_buffer), + tf_broadcaster_(broadcaster), + augmented_pointcloud_pub_(augmented_pointcloud_pub), + ground_pointcloud_pub_(ground_pointcloud_pub) +{ +} + +std::tuple BaseLidarCalibrator::calibrate() +{ + auto & last_keyframe = data_->keyframes_and_stopped_.back(); + PointcloudType::Ptr augmented_pointcloud_ptr = getDensePointcloudFromMap( + last_keyframe->pose_, last_keyframe, parameters_->leaf_size_dense_map_, + parameters_->min_calibration_range_, parameters_->max_calibration_range_); + + Eigen::Isometry3d initial_base_to_lidar_transform; + + try { + rclcpp::Time t = rclcpp::Time(0); + rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); + + initial_base_to_lidar_transform = tf2::transformToEigen( + tf_buffer_->lookupTransform(parameters_->base_frame_, data_->mapping_lidar_frame_, t, timeout) + .transform); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "could not get initial tf. %s", ex.what()); + + return std::make_tuple<>(false, Eigen::Matrix4d::Identity(), 0.f); + } + + PointcloudType::Ptr augmented_pointcloud_base_ptr(new PointcloudType()); + pcl::transformPointCloud( + *augmented_pointcloud_ptr, *augmented_pointcloud_base_ptr, + initial_base_to_lidar_transform.cast()); + + pcl::CropBox box_filter; + box_filter.setMin(Eigen::Vector4f( + parameters_->base_lidar_crop_box_min_x_, parameters_->base_lidar_crop_box_min_y_, + parameters_->base_lidar_crop_box_min_z_, 1.0)); + box_filter.setMax(Eigen::Vector4f( + parameters_->base_lidar_crop_box_max_x_, parameters_->base_lidar_crop_box_max_y_, + parameters_->base_lidar_crop_box_max_z_, 1.0)); + box_filter.setInputCloud(augmented_pointcloud_base_ptr); + box_filter.filter(*augmented_pointcloud_base_ptr); + + pcl::transformPointCloud( + *augmented_pointcloud_base_ptr, *augmented_pointcloud_ptr, + initial_base_to_lidar_transform.inverse().cast()); + + tier4_ground_plane_utils::GroundPlaneExtractorParameters parameters; + parameters.verbose_ = true; + parameters.use_crop_box_filter_ = false; + parameters.use_pca_rough_normal_ = false; + parameters.max_inlier_distance_ = parameters_->base_lidar_max_inlier_distance_; + parameters.min_plane_points_ = parameters_->base_lidar_min_plane_points_; + parameters.min_plane_points_percentage_ = parameters_->base_lidar_min_plane_points_percentage_; + parameters.max_cos_distance_ = parameters_->base_lidar_max_cos_distance_; + parameters.max_iterations_ = parameters_->base_lidar_max_iterations_; + parameters.remove_outliers_ = false; + parameters.initial_base_to_lidar_transform_ = initial_base_to_lidar_transform; + std::vector outlier_models; + + auto [status, ground_plane_model, ground_plane_inliers_ptr] = + tier4_ground_plane_utils::extractGroundPlane( + augmented_pointcloud_ptr, parameters, outlier_models); + + if (!status) { + RCLCPP_WARN( + rclcpp::get_logger(calibrator_name_), "Base calibration failed because not plane was found"); + return std::make_tuple<>(false, Eigen::Matrix4d::Identity(), 0.f); + } + + publishResults( + ground_plane_model, last_keyframe->pose_, ground_plane_inliers_ptr, augmented_pointcloud_ptr); + + Eigen::Isometry3d calibrated_base_to_lidar_transform = + tier4_ground_plane_utils::estimateBaseLidarTransform( + initial_base_to_lidar_transform, ground_plane_model); + + if (parameters_->base_lidar_overwrite_xy_yaw_) { + geometry_msgs::msg::TransformStamped initial_base_to_lidar_transform_msg_ = + tf2::eigenToTransform(initial_base_to_lidar_transform); + geometry_msgs::msg::TransformStamped calibrated_base_to_lidar_transform_msg = + tf2::eigenToTransform(calibrated_base_to_lidar_transform); + + calibrated_base_to_lidar_transform_msg = tier4_ground_plane_utils::overwriteXYYawValues( + initial_base_to_lidar_transform_msg_, calibrated_base_to_lidar_transform_msg); + + calibrated_base_to_lidar_transform = + tf2::transformToEigen(calibrated_base_to_lidar_transform_msg); + } + + // Other calibrators look for lidar -> calibration_frame, so we follow suit + return std::make_tuple<>(true, calibrated_base_to_lidar_transform.inverse().matrix(), 0.f); +} + +void BaseLidarCalibrator::publishResults( + const Eigen::Vector4d & ground_model, const Eigen::Matrix4f & pose, + const pcl::PointCloud::Ptr & ground_plane_inliers_lcs_ptr, + const pcl::PointCloud::Ptr & augmented_pointcloud_lcs_ptr) +{ + // Note: lcs=lidar coordinate system. mcs=map coordinate system + PointcloudType::Ptr ground_plane_inliers_mcs_ptr(new PointcloudType()); + PointcloudType::Ptr augmented_pointcloud_mcs_ptr(new PointcloudType()); + + pcl::transformPointCloud(*ground_plane_inliers_lcs_ptr, *ground_plane_inliers_mcs_ptr, pose); + pcl::transformPointCloud(*augmented_pointcloud_lcs_ptr, *augmented_pointcloud_mcs_ptr, pose); + + RCLCPP_INFO( + rclcpp::get_logger(calibrator_name_), "Estimated model. a=%f, b=%f, c=%f, d=%f", + ground_model.x(), ground_model.y(), ground_model.z(), ground_model.w()); + + sensor_msgs::msg::PointCloud2 ground_plane_inliers_msg, augmented_pointcloud_msg; + ground_plane_inliers_mcs_ptr->width = ground_plane_inliers_mcs_ptr->points.size(); + ground_plane_inliers_mcs_ptr->height = 1; + augmented_pointcloud_mcs_ptr->width = augmented_pointcloud_mcs_ptr->points.size(); + augmented_pointcloud_mcs_ptr->height = 1; + + pcl::toROSMsg(*ground_plane_inliers_mcs_ptr, ground_plane_inliers_msg); + pcl::toROSMsg(*augmented_pointcloud_mcs_ptr, augmented_pointcloud_msg); + + ground_plane_inliers_msg.header.frame_id = data_->map_frame_; + augmented_pointcloud_msg.header.frame_id = data_->map_frame_; + + augmented_pointcloud_pub_->publish(augmented_pointcloud_msg); + ground_pointcloud_pub_->publish(ground_plane_inliers_msg); + + Eigen::Isometry3d estimated_ground_pose = + tier4_ground_plane_utils::modelPlaneToPose(ground_model); + auto estimated_ground_msg = tf2::eigenToTransform(estimated_ground_pose); + estimated_ground_msg.header.frame_id = data_->mapping_lidar_frame_; + estimated_ground_msg.child_frame_id = "estimated_ground_pose"; + tf_broadcaster_.sendTransform(estimated_ground_msg); +} diff --git a/sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp b/sensor/mapping_based_calibrator/src/calibration_mapper.cpp similarity index 83% rename from sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp rename to sensor/mapping_based_calibrator/src/calibration_mapper.cpp index 6618c128..1ee056dc 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp +++ b/sensor/mapping_based_calibrator/src/calibration_mapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,20 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include +#include +#include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - CalibrationMapper::CalibrationMapper( MappingParameters::Ptr & parameters, MappingData::Ptr & mapping_data, PointPublisher::SharedPtr & map_pub, @@ -49,25 +44,37 @@ CalibrationMapper::CalibrationMapper( bag_paused_(false), bag_pause_requested_(false), bag_resume_requested_(false), - stopped_(false) + state_(INITIAL) { published_map_pointcloud_ptr_.reset(new PointcloudType()); assert(parameters_); - ndt_.setResolution(parameters_->mapper_resolution_); - ndt_.setStepSize(parameters_->mapper_step_size_); - ndt_.setMaximumIterations(parameters_->mapper_max_iterations_); - ndt_.setTransformationEpsilon(parameters_->mapper_epsilon_); - ndt_.setNeighborhoodSearchMethod(pclomp::DIRECT7); - - gicp_.setMaximumIterations( - parameters_->mapper_max_iterations_); // The maximum number of iterations - gicp_.setMaxCorrespondenceDistance(parameters_->mapper_max_correspondence_distance_); - gicp_.setTransformationEpsilon(parameters_->mapper_epsilon_); - gicp_.setEuclideanFitnessEpsilon(parameters_->mapper_epsilon_); + // cSpell:ignore pclomp + ndt_ptr_ = std::make_shared>(); + ndt_ptr_->setResolution(parameters_->mapper_resolution_); + ndt_ptr_->setStepSize(parameters_->mapper_step_size_); + ndt_ptr_->setMaximumIterations(parameters_->mapper_max_iterations_); + ndt_ptr_->setTransformationEpsilon(parameters_->mapper_epsilon_); + ndt_ptr_->setNeighborhoodSearchMethod(pclomp::DIRECT7); if (parameters_->mapper_num_threads_ > 0) { - ndt_.setNumThreads(parameters_->mapper_num_threads_); + ndt_ptr_->setNumThreads(parameters_->mapper_num_threads_); + } + + gicp_ptr_ = std::make_shared>(); + gicp_ptr_->setMaximumIterations( + parameters_->mapper_max_iterations_); // The maximum number of iterations + gicp_ptr_->setMaxCorrespondenceDistance(parameters_->mapper_max_correspondence_distance_); + gicp_ptr_->setTransformationEpsilon(parameters_->mapper_epsilon_); + gicp_ptr_->setEuclideanFitnessEpsilon(parameters_->mapper_epsilon_); + + if (parameters_->registrator_name_ == "ndt") { + registrator_ptr_ = ndt_ptr_; + } else if (parameters_->registrator_name_ == "gicp") { + registrator_ptr_ = gicp_ptr_; + } else { + RCLCPP_ERROR(rclcpp::get_logger("calibration_mapper"), "Invalid registrator"); + return; } for (auto & calibration_frame_name : data_->calibration_lidar_frame_names_) { @@ -79,27 +86,43 @@ CalibrationMapper::CalibrationMapper( data_->last_unmatched_keyframe_map_[calibration_frame_name] = parameters_->calibration_skip_keyframes_; } +} - std::thread thread = std::thread(&CalibrationMapper::mappingThreadWorker, this); - thread.detach(); +CalibrationMapper::State CalibrationMapper::getState() +{ + std::unique_lock lock(data_->mutex_); + return state_; } -bool CalibrationMapper::stopped() +void CalibrationMapper::start() { - std::unique_lock lock(data_->mutex_); - return stopped_; + if (this->getState() == INITIAL) { + RCLCPP_INFO(rclcpp::get_logger("calibration_mapper"), "Starting mapping thread"); + + std::unique_lock lock(data_->mutex_); + state_ = MAPPING; + std::thread thread = std::thread(&CalibrationMapper::mappingThreadWorker, this); + thread.detach(); + } else { + RCLCPP_INFO( + rclcpp::get_logger("calibration_mapper"), + "Attempting to start mapping when it was running or had already finished. Ignoring request"); + } } -void CalibrationMapper::stop() { stopped_ = true; } +void CalibrationMapper::stop() +{ + std::unique_lock lock(data_->mutex_); + state_ = FINISHED; +} void CalibrationMapper::calibrationCameraInfoCallback( const sensor_msgs::msg::CameraInfo::SharedPtr msg, const std::string & frame_name) { - if (stopped_) { - std::unique_lock lock(data_->mutex_); - RCLCPP_WARN( + if (this->getState() != MAPPING) { + RCLCPP_WARN_ONCE( rclcpp::get_logger("calibration_mapper"), - "Reveived a calibration camera info while not mapping. Ignoring it"); + "Received a calibration camera info while not mapping. Ignoring it"); return; } @@ -109,11 +132,10 @@ void CalibrationMapper::calibrationCameraInfoCallback( void CalibrationMapper::calibrationImageCallback( const sensor_msgs::msg::CompressedImage::SharedPtr msg, const std::string & frame_name) { - if (stopped_) { - std::unique_lock lock(data_->mutex_); - RCLCPP_WARN( + if (this->getState() != MAPPING) { + RCLCPP_WARN_ONCE( rclcpp::get_logger("calibration_mapper"), - "Reveived a calibration image while not mapping. Ignoring it"); + "Received a calibration image while not mapping. Ignoring it"); return; } @@ -123,11 +145,13 @@ void CalibrationMapper::calibrationImageCallback( void CalibrationMapper::calibrationPointCloudCallback( const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string & frame_name) { - if (stopped_) { - std::unique_lock lock(data_->mutex_); - RCLCPP_WARN( + // This method does not need a lock since calibration_pointclouds_list_map_ is only accessed in + // the main ROS thread + + if (this->getState() != MAPPING) { + RCLCPP_WARN_ONCE( rclcpp::get_logger("calibration_mapper"), - "Reveived a calibration pc while not mapping. Ignoring it"); + "Received a calibration pc while not mapping. Ignoring it"); return; } @@ -137,11 +161,11 @@ void CalibrationMapper::calibrationPointCloudCallback( void CalibrationMapper::mappingPointCloudCallback( const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - if (stopped_) { - std::unique_lock lock(data_->mutex_); - RCLCPP_WARN( + if (this->getState() != MAPPING) { + RCLCPP_WARN_ONCE( rclcpp::get_logger("calibration_mapper"), - "Reveived a mapping pc while not mapping. Ignoring it"); + "Received a mapping pc while not mapping. Ignoring it"); + return; } @@ -162,18 +186,24 @@ void CalibrationMapper::mappingPointCloudCallback( transformPointcloud( msg->header.frame_id, data_->mapping_lidar_frame_, pc_ptr, *tf_buffer_); - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); auto frame = std::make_shared(); frame->header_ = msg->header; frame->pointcloud_raw_ = pc_ptr; - if ( - rclcpp::Time(msg->header.stamp) < rclcpp::Time(mapping_lidar_header_->stamp) || - static_cast(data_->processed_frames_.size()) >= parameters_->mapping_max_frames_) { + if (rclcpp::Time(msg->header.stamp) < rclcpp::Time(mapping_lidar_header_->stamp)) { + stop(); + RCLCPP_WARN( + rclcpp::get_logger("calibration_mapper"), "Stopping mapper due to negative delta timestamps"); + return; + } + + if (static_cast(data_->processed_frames_.size()) >= parameters_->mapping_max_frames_) { stop(); RCLCPP_WARN( rclcpp::get_logger("calibration_mapper"), - "Stopping mapper due to enough frames being collected"); + "Stopping mapper due to enough frames being collected (%lu)", + data_->processed_frames_.size()); return; } @@ -181,9 +211,8 @@ void CalibrationMapper::mappingPointCloudCallback( parameters_->use_rosbag_ && !bag_paused_ && !bag_pause_requested_ && data_->unprocessed_frames_.size() > 0) { auto cb = [&](rclcpp::Client::SharedFuture response_client) { - auto res = response_client.get(); - (void)res; - std::unique_lock lock(data_->mutex_); + [[maybe_unused]] auto res = response_client.get(); + std::unique_lock lock(data_->mutex_); bag_paused_ = true; bag_pause_requested_ = false; }; @@ -204,17 +233,17 @@ void CalibrationMapper::mappingThreadWorker() using std::chrono_literals::operator""ms; Eigen::Matrix4f last_pose = - Eigen::Matrix4f::Identity(); // not necessarily assciated with a frame + Eigen::Matrix4f::Identity(); // not necessarily associated with a frame builtin_interfaces::msg::Time last_stamp; - while (rclcpp::ok() && !stopped_) { + while (rclcpp::ok() && getState() == MAPPING) { Frame::Ptr frame, prev_frame, prev_frame_not_last; float prev_distance = 0.f; // Locked section { - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); if (data_->unprocessed_frames_.size() == 0) { if (parameters_->use_rosbag_ && bag_paused_ && !bag_resume_requested_) { @@ -222,7 +251,7 @@ void CalibrationMapper::mappingThreadWorker() [&](rclcpp::Client::SharedFuture response_client) { auto res = response_client.get(); (void)res; - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); bag_paused_ = false; bag_resume_requested_ = false; RCLCPP_WARN(rclcpp::get_logger("calibration_mapper"), "Received resume response"); @@ -258,7 +287,7 @@ void CalibrationMapper::mappingThreadWorker() } if (!prev_frame) { - initLocalMap(frame); + this->initLocalMap(frame); } VoxelGridWrapper voxel_grid; @@ -273,8 +302,8 @@ void CalibrationMapper::mappingThreadWorker() voxel_grid.filter(*frame->pointcloud_subsampled_); // Register the frame to the map - gicp_.setInputTarget(data_->local_map_ptr_); - gicp_.setInputSource(frame->pointcloud_subsampled_); + registrator_ptr_->setInputTarget(data_->local_map_ptr_); + registrator_ptr_->setInputSource(frame->pointcloud_subsampled_); Eigen::Matrix4f guess = last_pose; double dt_since_last = @@ -306,19 +335,19 @@ void CalibrationMapper::mappingThreadWorker() guess = prev_frame_not_last->pose_ * interpolated_pose; } - gicp_.align(*aligned_cloud_ptr, guess); - last_pose = gicp_.getFinalTransformation(); + registrator_ptr_->align(*aligned_cloud_ptr, guess); + last_pose = registrator_ptr_->getFinalTransformation(); float innovation = Eigen::Affine3f(guess.inverse() * last_pose).translation().norm(); - float score = gicp_.getFitnessScore(); + float score = registrator_ptr_->getFitnessScore(); RCLCPP_INFO( - rclcpp::get_logger("calibration_mapper"), "NDT Innovation=%.2f. Score=%.2f", innovation, - score); + rclcpp::get_logger("calibration_mapper"), "Registrator innovation=%.2f. Score=%.2f", + innovation, score); last_stamp = frame->header_.stamp; } - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); // Fill the frame information frame->pose_ = last_pose; @@ -343,12 +372,14 @@ void CalibrationMapper::mappingThreadWorker() pose_msg.pose = tf2::toMsg(Eigen::Affine3d(frame->pose_.cast())); data_->trajectory_.push_back(pose_msg); - if (!checkFrameLost(prev_frame, frame, dt_since_last) && shouldDropFrame(prev_frame, frame)) { + if ( + !this->checkFrameLost(prev_frame, frame, dt_since_last) && + this->shouldDropFrame(prev_frame, frame)) { continue; } data_->processed_frames_.push_back(frame); - checkKeyframe(frame); + this->checkKeyframe(frame); RCLCPP_INFO( rclcpp::get_logger("calibration_mapper"), @@ -386,8 +417,8 @@ void CalibrationMapper::checkKeyframe(Frame::Ptr frame) data_->keyframes_and_stopped_.push_back(frame); frame->is_key_frame_ = true; frame->keyframe_id_ = data_->keyframes_.size(); - checkKeyframeLost(frame); - recalculateLocalMap(); + this->checkKeyframeLost(frame); + this->recalculateLocalMap(); } } @@ -403,33 +434,36 @@ void CalibrationMapper::checkKeyframeLost(Frame::Ptr keyframe) const Frame::Ptr & left_frame = data_->keyframes_[keyframe->keyframe_id_ - 1]; Frame::Ptr & right_frame = keyframe; - Eigen::Affine3f dpose1(two_left_frame->pose_.inverse() * left_frame->pose_); - Eigen::Affine3f dpose2(left_frame->pose_.inverse() * right_frame->pose_); + Eigen::Affine3f delta_pose1(two_left_frame->pose_.inverse() * left_frame->pose_); + Eigen::Affine3f delta_pose2(left_frame->pose_.inverse() * right_frame->pose_); Eigen::Affine3f left_pose(left_frame->pose_); Eigen::Affine3f right_pose(right_frame->pose_); - auto d1 = dpose1.translation().normalized(); - auto d2 = dpose2.translation().normalized(); + auto d1 = delta_pose1.translation().normalized(); + auto d2 = delta_pose2.translation().normalized(); - float trans_angle_diff = (180.0 / M_PI) * std::acos(d1.dot(d2)); - trans_angle_diff = - dpose2.translation().norm() > parameters_->new_keyframe_min_distance_ ? trans_angle_diff : 0.0; + float translation_angle_diff = (180.0 / M_PI) * std::acos(d1.dot(d2)); + translation_angle_diff = + delta_pose2.translation().norm() > parameters_->new_keyframe_min_distance_ + ? translation_angle_diff + : 0.0; - float rot_angle_diff = - (180.0 / M_PI) * std::acos(std::min( - 1.0, 0.5 * ((dpose1.rotation().inverse() * dpose2.rotation()).trace() - - 1.0))); // Tr(R) = 1 + 2*cos(theta) + float rotation_angle_diff = + (180.0 / M_PI) * + std::acos(std::min( + 1.0, 0.5 * ((delta_pose1.rotation().inverse() * delta_pose2.rotation()).trace() - + 1.0))); // Tr(R) = 1 + 2*cos(theta) if ( - std::abs(trans_angle_diff) > parameters_->lost_frame_max_angle_diff_ || - std::abs(trans_angle_diff) > parameters_->lost_frame_max_angle_diff_) { + std::abs(translation_angle_diff) > parameters_->lost_frame_max_angle_diff_ || + std::abs(translation_angle_diff) > parameters_->lost_frame_max_angle_diff_) { keyframe->lost_ = true; RCLCPP_WARN( rclcpp::get_logger("calibration_mapper"), "Mapping failed. Angle between keyframes is too high. a1=%.2f (deg) a2=%.2f (deg) " - "theshold=%.2f (deg)", - trans_angle_diff, rot_angle_diff, parameters_->lost_frame_max_angle_diff_); + "threshold=%.2f (deg)", + translation_angle_diff, rotation_angle_diff, parameters_->lost_frame_max_angle_diff_); return; } @@ -449,7 +483,7 @@ void CalibrationMapper::checkKeyframeLost(Frame::Ptr keyframe) Eigen::Affine3f frame_pose(mid_frame->pose_); float trans_diff = (interpolated_pose.inverse() * frame_pose).translation().norm(); - float rot_angle_diff = + float rotation_angle_diff = (180.0 / M_PI) * std::acos(std::min( 1.0, 0.5 * ((interpolated_pose.rotation().inverse() * frame_pose.rotation()).trace() - @@ -458,13 +492,13 @@ void CalibrationMapper::checkKeyframeLost(Frame::Ptr keyframe) if ( (!left_frame->stopped_ && !right_frame->stopped_) && (trans_diff > parameters_->lost_frame_interpolation_error_ || - std::abs(rot_angle_diff) > parameters_->lost_frame_max_angle_diff_)) { + std::abs(rotation_angle_diff) > parameters_->lost_frame_max_angle_diff_)) { keyframe->lost_ = true; RCLCPP_WARN( rclcpp::get_logger("calibration_mapper"), "Mapping failed. Interpolation error is too high. d=%.2f (m) a=%.2f (deg)", trans_diff, - rot_angle_diff); + rotation_angle_diff); return; } @@ -579,15 +613,12 @@ void CalibrationMapper::recalculateLocalMap() voxel_grid.filter(*data_->local_map_ptr_); } -#pragma GCC push_options -#pragma GCC optimize("O0") - void CalibrationMapper::publisherTimerCallback() { static int published_frames = 0; static int published_keyframes = 0; - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); if (static_cast(data_->keyframes_.size()) == published_keyframes) { return; @@ -726,12 +757,12 @@ void CalibrationMapper::publisherTimerCallback() return; } -#pragma GCC pop_options - void CalibrationMapper::dataMatchingTimerCallback() { + // calibration_pointclouds_list_map_ are only used in the main ROS thread and thus do not need a + // lock for (const auto & frame_name : data_->calibration_camera_optical_link_frame_names) { - mappingCalibrationDatamatching( + mappingCalibrationDataMatching( frame_name, calibration_images_list_map_[frame_name], std::bind( &CalibrationMapper::addNewCameraCalibrationFrame, this, std::placeholders::_1, @@ -739,7 +770,7 @@ void CalibrationMapper::dataMatchingTimerCallback() } for (const auto & frame_name : data_->calibration_lidar_frame_names_) { - mappingCalibrationDatamatching( + mappingCalibrationDataMatching( frame_name, calibration_pointclouds_list_map_[frame_name], std::bind( &CalibrationMapper::addNewLidarCalibrationFrame, this, std::placeholders::_1, @@ -748,13 +779,13 @@ void CalibrationMapper::dataMatchingTimerCallback() } template -void CalibrationMapper::mappingCalibrationDatamatching( +void CalibrationMapper::mappingCalibrationDataMatching( const std::string & calibration_frame_name, std::list & calibration_msg_list, std::function add_frame_function) { - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); auto & last_unmatched_keyframe = data_->last_unmatched_keyframe_map_[calibration_frame_name]; while (last_unmatched_keyframe < static_cast(data_->keyframes_and_stopped_.size()) - 1) { @@ -767,7 +798,7 @@ void CalibrationMapper::mappingCalibrationDatamatching( return; } - // We need there to be a frame after the keyframe since we may wanto to interpolate in that + // We need there to be a frame after the keyframe since we may want to interpolate in that // direction if (keyframe->frame_id_ + 1 >= static_cast(data_->processed_frames_.size())) { return; @@ -795,7 +826,7 @@ void CalibrationMapper::mappingCalibrationDatamatching( double interpolated_speed; double interpolated_accel; - // Compute first and second order derivates with finite differences + // Compute first and second order derivatives with finite differences if (dt_left < dt_right) { msg = msg_left; @@ -884,9 +915,9 @@ bool CalibrationMapper::addNewCameraCalibrationFrame( const std::string & calibration_frame_name, sensor_msgs::msg::CompressedImage::SharedPtr & msg, CalibrationFrame & calibration_frame) { - calibration_frame.source_camera_info = + calibration_frame.source_camera_info_ = latest_calibration_camera_infos_map_[calibration_frame_name]; - calibration_frame.source_image = msg; + calibration_frame.source_image_ = msg; data_->camera_calibration_frames_map_[calibration_frame_name].emplace_back(calibration_frame); return true; @@ -930,13 +961,13 @@ bool CalibrationMapper::addNewLidarCalibrationFrame( return true; } -template void CalibrationMapper::mappingCalibrationDatamatching( +template void CalibrationMapper::mappingCalibrationDataMatching( const std::string & calibration_frame, std::list & calibration_msg_list, std::function< bool(const std::string &, sensor_msgs::msg::CompressedImage::SharedPtr &, CalibrationFrame &)> add_frame_function); -template void CalibrationMapper::mappingCalibrationDatamatching( +template void CalibrationMapper::mappingCalibrationDataMatching( const std::string & calibration_frame, std::list & calibration_msg_list, std::function< diff --git a/sensor/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp b/sensor/mapping_based_calibrator/src/camera_calibrator.cpp similarity index 85% rename from sensor/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp rename to sensor/mapping_based_calibrator/src/camera_calibrator.cpp index 675fc96d..1d392abb 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/camera_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include +#include #include #include @@ -26,17 +27,6 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#define UNUSED(x) (void)x; - -#pragma GCC push_options -#pragma GCC optimize("O0") - CameraCalibrator::CameraCalibrator( const std::string & calibration_camera_optical_link_frame, CalibrationParameters::Ptr & parameters, MappingData::Ptr & mapping_data, @@ -72,9 +62,9 @@ CameraCalibrator::CameraCalibrator( void CameraCalibrator::configureCalibrators() {} -bool CameraCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_score) +std::tuple CameraCalibrator::calibrate() { - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); Eigen::Matrix4f initial_calibration_transform; float initial_distance; @@ -99,7 +89,7 @@ bool CameraCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_ } catch (tf2::TransformException & ex) { RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "could not get initial tf. %s", ex.what()); - return false; + return std::make_tuple<>(false, Eigen::Matrix4d::Identity(), 0.f); } // Filter calibration frames using several criteria and select the best ones suited for @@ -109,24 +99,22 @@ bool CameraCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_ if (static_cast(calibration_frames.size()) < parameters_->camera_calibration_min_frames_) { RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "Insufficient calibration frames. aborting."); - return false; + return std::make_tuple<>(false, Eigen::Matrix4d::Identity(), 0.f); } - // Prepate augmented calibration pointclouds + // Prepare augmented calibration pointclouds std::vector::Ptr> targets; prepareCalibrationData( calibration_frames, initial_calibration_transform, initial_distance, targets); - // We no lnoger used the shared data + // We no longer used the shared data lock.unlock(); - // Publish the calbiraton resullts + // Publish the calibration results publishResults(calibration_frames, targets, map_frame, initial_calibration_transform); - best_transform = Eigen::Matrix4f::Identity(); - best_score = 0.f; - - return true; + return std::make_tuple<>(true, Eigen::Matrix4d::Identity(), 0.f); + ; } void CameraCalibrator::prepareCalibrationData( @@ -138,8 +126,8 @@ void CameraCalibrator::prepareCalibrationData( rclcpp::get_logger(calibrator_name_), "Preparing dense calibration pointclouds from the map..."); - // Time fustrum-ing the last pointcloud or all the pointclouds in between - auto & camera_info = calibration_frames.front().source_camera_info; + // Time frustum-ing the last pointcloud or all the pointclouds in between + auto & camera_info = calibration_frames.front().source_camera_info_; float fx = camera_info->p[0]; float fy = camera_info->p[5]; float fov_x = (180.f / CV_PI) * 2 * std::atan(0.5f * camera_info->width / fx); @@ -178,7 +166,7 @@ void CameraCalibrator::publishResults( const Eigen::Matrix4f & initial_calibration_transform) { image_geometry::PinholeCameraModel pinhole_camera_model_; - pinhole_camera_model_.fromCameraInfo(calibration_frames.front().source_camera_info); + pinhole_camera_model_.fromCameraInfo(calibration_frames.front().source_camera_info_); auto size = pinhole_camera_model_.fullResolution(); cv::Point3d corner1 = parameters_->pc_features_max_distance_ * @@ -191,6 +179,7 @@ void CameraCalibrator::publishResults( cv::Point3d corner4 = parameters_->pc_features_max_distance_ * pinhole_camera_model_.projectPixelTo3dRay(cv::Point2d(0.0, size.height)); + // Note: ccs=camera coordinate system std::array corners_ccs{ Eigen::Vector4f(corner1.x, corner1.y, corner1.z, 1.f), Eigen::Vector4f(corner2.x, corner2.y, corner2.z, 1.f), @@ -280,25 +269,3 @@ void CameraCalibrator::publishResults( target_map_pub_->publish(target_map_msg); target_markers_pub_->publish(markers); } - -void CameraCalibrator::singleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) -{ - UNUSED(request); - UNUSED(response); -} - -void CameraCalibrator::multipleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) -{ - UNUSED(request); - UNUSED(response); - - Eigen::Matrix4f result; - float score; - response->success = calibrate(result, score); -} - -#pragma GCC pop_options diff --git a/sensor/extrinsic_mapping_based_calibrator/src/filters/best_frames_filter.cpp b/sensor/mapping_based_calibrator/src/filters/best_frames_filter.cpp similarity index 97% rename from sensor/extrinsic_mapping_based_calibrator/src/filters/best_frames_filter.cpp rename to sensor/mapping_based_calibrator/src/filters/best_frames_filter.cpp index 31759e5c..8a563f33 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/best_frames_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/best_frames_filter.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include diff --git a/sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp b/sensor/mapping_based_calibrator/src/filters/dynamics_filter.cpp similarity index 96% rename from sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp rename to sensor/mapping_based_calibrator/src/filters/dynamics_filter.cpp index d32a9d1f..20185f8a 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/dynamics_filter.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #define UNUSED(x) (void)x; @@ -26,7 +26,7 @@ std::vector DynamicsFilter::filter( std::vector filtered_frames; std::stringstream ss; - ss << "Accepted kyframes due to dynamics & interpolation: "; + ss << "Accepted keyframes due to dynamics & interpolation: "; for (auto & frame : calibration_frames) { RCLCPP_INFO( diff --git a/sensor/extrinsic_mapping_based_calibrator/src/filters/lost_state_filter.cpp b/sensor/mapping_based_calibrator/src/filters/lost_state_filter.cpp similarity index 97% rename from sensor/extrinsic_mapping_based_calibrator/src/filters/lost_state_filter.cpp rename to sensor/mapping_based_calibrator/src/filters/lost_state_filter.cpp index 94ea0e84..deefbd07 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/lost_state_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/lost_state_filter.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include void LostStateFilter::setName(const std::string & name) { name_ = name + " (LostStateFilter)"; } diff --git a/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp b/sensor/mapping_based_calibrator/src/filters/object_detection_filter.cpp similarity index 96% rename from sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp rename to sensor/mapping_based_calibrator/src/filters/object_detection_filter.cpp index 25cee1d4..9fc7a6af 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/object_detection_filter.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include +#include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include @@ -99,7 +94,8 @@ void ObjectDetectionFilter::filter( const Eigen::Affine3f & source_lidar_to_mapping_lidar_transform, const Eigen::Affine3f & mapping_lidar_to_detection_frame_transform) { - // Compute the min/max from the source pointcloud to use as AABB on the source pointcloud frame + // Compute the min/max from the source pointcloud to use as axis-aligned-bounding-boxes on the + // source pointcloud frame Eigen::Array4f min_p, max_p; min_p.setConstant(std::numeric_limits::max()); max_p.setConstant(-std::numeric_limits::max()); diff --git a/sensor/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp b/sensor/mapping_based_calibrator/src/lidar_calibrator.cpp similarity index 66% rename from sensor/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp rename to sensor/mapping_based_calibrator/src/lidar_calibrator.cpp index 05449bd0..08e64e4f 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/lidar_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,24 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else +#include +#include +#include +#include +#include +#include +#include +#include #include -#endif -#define UNUSED(x) (void)x; +#include LidarCalibrator::LidarCalibrator( const std::string & calibration_lidar_frame, CalibrationParameters::Ptr & parameters, @@ -67,25 +60,27 @@ LidarCalibrator::LidarCalibrator( correspondence_estimator_ = pcl::make_shared>(); + // cSpell:ignore pclomp calibration_ndt_ = pcl::make_shared>(); calibration_gicp_ = pcl::make_shared>(); calibration_icp_coarse_ = pcl::make_shared>(); calibration_icp_fine_ = pcl::make_shared>(); - calibration_icp_ultrafine_ = pcl::make_shared>(); + calibration_icp_ultra_fine_ = + pcl::make_shared>(); calibration_registrators_ = { calibration_ndt_, calibration_gicp_, calibration_icp_coarse_, calibration_icp_fine_, - calibration_icp_ultrafine_}; + calibration_icp_ultra_fine_}; calibration_batch_icp_coarse_ = pcl::make_shared>(); calibration_batch_icp_fine_ = pcl::make_shared>(); - calibration_batch_icp_ultrafine_ = + calibration_batch_icp_ultra_fine_ = pcl::make_shared>(); calibration_batch_registrators_ = { - calibration_batch_icp_coarse_, calibration_batch_icp_fine_, calibration_batch_icp_ultrafine_}; + calibration_batch_icp_coarse_, calibration_batch_icp_fine_, calibration_batch_icp_ultra_fine_}; configureCalibrators(); } @@ -95,7 +90,7 @@ void LidarCalibrator::configureCalibrators() calibration_gicp_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_coarse_); calibration_icp_coarse_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_coarse_); calibration_icp_fine_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_fine_); - calibration_icp_ultrafine_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_ultrafine_); + calibration_icp_ultra_fine_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_ultra_fine_); for (auto & calibrator : calibration_registrators_) { calibrator->setMaximumIterations(parameters_->solver_iterations_); @@ -103,8 +98,8 @@ void LidarCalibrator::configureCalibrators() calibration_batch_icp_coarse_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_coarse_); calibration_batch_icp_fine_->setMaxCorrespondenceDistance(parameters_->max_corr_dist_fine_); - calibration_batch_icp_ultrafine_->setMaxCorrespondenceDistance( - parameters_->max_corr_dist_ultrafine_); + calibration_batch_icp_ultra_fine_->setMaxCorrespondenceDistance( + parameters_->max_corr_dist_ultra_fine_); for (auto & calibrator : calibration_batch_registrators_) { calibrator->setMaximumIterations(parameters_->solver_iterations_); @@ -120,172 +115,9 @@ void LidarCalibrator::setUpCalibrators( } } -void LidarCalibrator::singleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) -{ - std::unique_lock lock(data_->mutex_); - - Eigen::Matrix4f initial_calibration_transform; - float initial_distance; - - // Get the tf between frames - try { - rclcpp::Time t = rclcpp::Time(0); - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - - geometry_msgs::msg::Transform initial_target_to_source_msg; - Eigen::Affine3d initial_target_to_source_affine; - - initial_target_to_source_msg = - tf_buffer_->lookupTransform(data_->mapping_lidar_frame_, calibrator_sensor_frame_, t, timeout) - .transform; - - initial_target_to_source_affine = tf2::transformToEigen(initial_target_to_source_msg); - initial_distance = initial_target_to_source_affine.translation().norm(); - initial_calibration_transform = initial_target_to_source_affine.matrix().cast(); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "could not get initial tf. %s", ex.what()); - return; - } - - auto & calibration_frames = data_->lidar_calibration_frames_map_[calibrator_sensor_frame_]; - - std::vector filtered_calibration_frames = - filter_->filter(calibration_frames, data_); - - if (request->id >= static_cast(filtered_calibration_frames.size())) { - RCLCPP_WARN( - rclcpp::get_logger(calibrator_name_), "Invalid requested calibration frame. size=%lu", - filtered_calibration_frames.size()); - return; - } - - CalibrationFrame & calibration_frame = filtered_calibration_frames[request->id]; - PointcloudType::Ptr source_pc_ptr = cropPointCloud( - calibration_frame.source_pointcloud_, parameters_->min_calibration_range_, - parameters_->max_calibration_range_); - - PointcloudType::Ptr target_dense_pc_ptr = getDensePointcloudFromMap( - calibration_frame.local_map_pose_, calibration_frame.target_frame_, - parameters_->leaf_size_dense_map_, parameters_->min_calibration_range_, - parameters_->max_calibration_range_ + initial_distance); - PointcloudType::Ptr target_thin_pc_ptr = getDensePointcloudFromMap( - calibration_frame.local_map_pose_, calibration_frame.target_frame_, - parameters_->calibration_viz_leaf_size_, parameters_->min_calibration_range_, - parameters_->max_calibration_range_ + initial_distance); - - PointcloudType::Ptr initial_source_aligned_pc_ptr(new PointcloudType()); - pcl::transformPointCloud( - *source_pc_ptr, *initial_source_aligned_pc_ptr, initial_calibration_transform); - - // Evaluate the initial calibration - setUpCalibrators(source_pc_ptr, target_dense_pc_ptr); - - // Crop unused areas of the target pointcloud to save processing time - cropTargetPointcloud( - initial_source_aligned_pc_ptr, target_dense_pc_ptr, initial_distance); - cropTargetPointcloud( - initial_source_aligned_pc_ptr, target_thin_pc_ptr, initial_distance); - - correspondence_estimator_->setInputSource(initial_source_aligned_pc_ptr); - correspondence_estimator_->setInputTarget(target_dense_pc_ptr); - double initial_score = sourceTargetDistance( - *correspondence_estimator_, parameters_->calibration_eval_max_corr_distance_); - - RCLCPP_WARN( - rclcpp::get_logger(calibrator_name_), - "Initial calibration score = %.4f (avg.squared.dist) | sqrt.score = %.4f m | discretization = " - "%.4f m", - initial_score, std::sqrt(initial_score), parameters_->leaf_size_dense_map_); - - // Find best calibration using an "ensemble" of calibrators - std::vector candidate_transforms = {initial_calibration_transform}; - Eigen::Matrix4f best_transform; - float best_score; - - findBestTransform, PointType>( - candidate_transforms, calibration_registrators_, - parameters_->calibration_eval_max_corr_distance_, parameters_->calibration_verbose_, - best_transform, best_score); - - PointcloudType::Ptr calibrated_source_aligned_pc_ptr(new PointcloudType()); - pcl::transformPointCloud(*source_pc_ptr, *calibrated_source_aligned_pc_ptr, best_transform); - - RCLCPP_WARN( - rclcpp::get_logger(calibrator_name_), - "Best calibration score = %.4f (avg.squared.dist) | sqrt.score = %.4f m | discretization = " - "%.4f m", - best_score, std::sqrt(best_score), parameters_->leaf_size_dense_map_); - - PointcloudType::Ptr test_aligned_pc_ptr(new PointcloudType()); - pcl::transformPointCloud(*source_pc_ptr, *test_aligned_pc_ptr, best_transform); - - correspondence_estimator_->setInputSource(test_aligned_pc_ptr); - correspondence_estimator_->setInputTarget(target_dense_pc_ptr); - double test_score = sourceTargetDistance( - *correspondence_estimator_, parameters_->calibration_eval_max_corr_distance_); - - RCLCPP_WARN( - rclcpp::get_logger(calibrator_name_), - "Test calibration score = %.4f (avg.squared.dist) | sqrt.score = %.4f m | discretization = " - "%.4f m", - test_score, std::sqrt(test_score), parameters_->leaf_size_dense_map_); - - // Output ROS data - PointcloudType::Ptr initial_source_aligned_map_ptr(new PointcloudType()); - PointcloudType::Ptr calibrated_source_aligned_map_ptr(new PointcloudType()); - PointcloudType::Ptr target_thin_map_ptr(new PointcloudType()); - pcl::transformPointCloud( - *initial_source_aligned_pc_ptr, *initial_source_aligned_map_ptr, - calibration_frame.local_map_pose_); - pcl::transformPointCloud( - *calibrated_source_aligned_pc_ptr, *calibrated_source_aligned_map_ptr, - calibration_frame.local_map_pose_); - pcl::transformPointCloud( - *target_thin_pc_ptr, *target_thin_map_ptr, calibration_frame.local_map_pose_); - - sensor_msgs::msg::PointCloud2 initial_source_aligned_map_msg, calibrated_source_aligned_map_msg, - target_map_msg; - initial_source_aligned_pc_ptr->width = initial_source_aligned_pc_ptr->points.size(); - initial_source_aligned_pc_ptr->height = 1; - calibrated_source_aligned_pc_ptr->width = calibrated_source_aligned_pc_ptr->points.size(); - calibrated_source_aligned_pc_ptr->height = 1; - target_thin_pc_ptr->width = target_thin_pc_ptr->points.size(); - target_thin_pc_ptr->height = 1; - - pcl::toROSMsg(*initial_source_aligned_map_ptr, initial_source_aligned_map_msg); - pcl::toROSMsg(*calibrated_source_aligned_map_ptr, calibrated_source_aligned_map_msg); - pcl::toROSMsg(*target_thin_map_ptr, target_map_msg); - - initial_source_aligned_map_msg.header = calibration_frame.source_header_; - initial_source_aligned_map_msg.header.frame_id = data_->map_frame_; - calibrated_source_aligned_map_msg.header = calibration_frame.source_header_; - calibrated_source_aligned_map_msg.header.frame_id = data_->map_frame_; - target_map_msg.header = calibration_frame.target_frame_->header_; - target_map_msg.header.frame_id = data_->map_frame_; - - initial_source_aligned_map_pub_->publish(initial_source_aligned_map_msg); - calibrated_source_aligned_map_pub_->publish(calibrated_source_aligned_map_msg); - target_map_pub_->publish(target_map_msg); - - response->success = true; -} - -void LidarCalibrator::multipleSensorCalibrationCallback( - const std::shared_ptr request, - const std::shared_ptr response) -{ - UNUSED(request); - - Eigen::Matrix4f result; - float score; - response->success = calibrate(result, score); -} - -bool LidarCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_score) +std::tuple LidarCalibrator::calibrate() { - std::unique_lock lock(data_->mutex_); + std::unique_lock lock(data_->mutex_); Eigen::Matrix4f initial_calibration_transform; float initial_distance; @@ -310,7 +142,7 @@ bool LidarCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_s } catch (tf2::TransformException & ex) { RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "could not get initial tf. %s", ex.what()); - return false; + return std::make_tuple<>(false, Eigen::Matrix4d::Identity(), 0.f); } // Filter calibration frames using several criteria and select the best ones suited for @@ -319,17 +151,19 @@ bool LidarCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_s filter_->filter(data_->lidar_calibration_frames_map_[calibrator_sensor_frame_], data_); if (static_cast(calibration_frames.size()) < parameters_->lidar_calibration_min_frames_) { - RCLCPP_WARN(rclcpp::get_logger(calibrator_name_), "Insufficient calibration frames. aborting."); - return false; + RCLCPP_WARN( + rclcpp::get_logger(calibrator_name_), "Insufficient calibration frames (%lu / %d). aborting.", + calibration_frames.size(), parameters_->lidar_calibration_min_frames_); + return std::make_tuple<>(false, Eigen::Matrix4d::Identity(), 0.f); } - // Prepate augmented calibration pointclouds + // Prepare augmented calibration pointclouds std::vector::Ptr> sources, targets, targets_thin; prepareCalibrationData( calibration_frames, initial_distance, initial_calibration_transform, sources, targets, targets_thin); - // We no lnoger used the shared data + // We no longer used the shared data lock.unlock(); // Set all the registrators with the pointclouds @@ -392,7 +226,7 @@ bool LidarCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_s best_single_frame_transform_score, best_single_frame_transform_multi_frame_score); } - // Choose the best sigle-frame calibration + // Choose the best single-frame calibration std::vector::iterator best_single_frame_calibration_multi_frame_score_it = std::min_element( std::begin(single_frame_calibration_multi_frame_score), @@ -485,15 +319,14 @@ bool LidarCalibrator::calibrate(Eigen::Matrix4f & best_transform, float & best_s rclcpp::get_logger(calibrator_name_), "\t\tw: %f", best_multi_frame_calibration_tf.transform.rotation.w); - // Publish the calbiraton resullts + // Publish the calibration results publishResults( calibration_frames, sources, targets_thin, initial_calibration_transform, best_multi_frame_calibration_transform, map_frame); - best_transform = best_multi_frame_calibration_transform; - best_score = std::sqrt(best_multi_frame_calibration_multi_frame_score); - - return true; + return std::make_tuple<>( + true, best_multi_frame_calibration_transform.cast(), + std::sqrt(best_multi_frame_calibration_multi_frame_score)); } void LidarCalibrator::prepareCalibrationData( @@ -523,7 +356,7 @@ void LidarCalibrator::prepareCalibrationData( parameters_->calibration_viz_leaf_size_, parameters_->min_calibration_range_, parameters_->max_calibration_range_ + initial_distance); - // Transfor the source to target frame to crop it later + // Transform the source to target frame to crop it later PointcloudType::Ptr initial_source_aligned_pc_ptr(new PointcloudType()); pcl::transformPointCloud( *source_pc_ptr, *initial_source_aligned_pc_ptr, initial_calibration_transform); diff --git a/sensor/extrinsic_mapping_based_calibrator/src/main.cpp b/sensor/mapping_based_calibrator/src/main.cpp similarity index 89% rename from sensor/extrinsic_mapping_based_calibrator/src/main.cpp rename to sensor/mapping_based_calibrator/src/main.cpp index c3f3dd88..b0ab0a01 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/main.cpp +++ b/sensor/mapping_based_calibrator/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include diff --git a/sensor/extrinsic_mapping_based_calibrator/src/extrinsic_mapping_based_calibrator.cpp b/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp similarity index 66% rename from sensor/extrinsic_mapping_based_calibrator/src/extrinsic_mapping_based_calibrator.cpp rename to sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp index e53798d7..c34d22f5 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/extrinsic_mapping_based_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include +#include #include #include +#include #include #include @@ -24,18 +25,10 @@ #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include -#define UNUSED(x) (void)x; - #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(p, #NAME, PARAM_STRUCT.NAME##_) namespace @@ -50,7 +43,7 @@ void update_param( if (it != parameters.cend()) { value = it->template get_value(); RCLCPP_INFO_STREAM( - rclcpp::get_logger("extrinsic_mapping_based_calibrator"), + rclcpp::get_logger("mapping_based_calibrator"), "Setting parameter [" << name << "] to " << value); } } @@ -58,7 +51,7 @@ void update_param( ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( const rclcpp::NodeOptions & options) -: Node("extrinsic_mapping_based_calibrator_node", options), +: Node("mapping_based_calibrator_node", options), tf_broadcaster_(this), tf_buffer_(std::make_shared(this->get_clock())), transform_listener_(std::make_shared(*tf_buffer_)), @@ -68,22 +61,9 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( { using std::chrono_literals::operator""s; - std::vector camera_calibration_service_names = - this->declare_parameter>("camera_calibration_service_names"); - std::vector lidar_calibration_service_names = - this->declare_parameter>("lidar_calibration_service_names"); - - std::vector camera_calibration_sensor_kit_frames = - this->declare_parameter>("camera_calibration_sensor_kit_frames"); - std::vector lidar_calibration_sensor_kit_frames = - this->declare_parameter>("lidar_calibration_sensor_kit_frames"); - - std::vector calibration_camera_frames = - this->declare_parameter>("calibration_camera_frames"); - - std::vector calibration_lidar_base_frames = - this->declare_parameter>("calibration_lidar_base_frames"); - + calibration_parameters_->calibrate_base_frame_ = + this->declare_parameter("calibrate_base_frame"); + calibration_parameters_->base_frame_ = this->declare_parameter("base_frame"); mapping_data_->map_frame_ = this->declare_parameter("map_frame"); mapping_data_->calibration_camera_optical_link_frame_names = @@ -110,11 +90,6 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( v1 = v2; }; - remove_empty_strings(camera_calibration_service_names); - remove_empty_strings(lidar_calibration_service_names); - remove_empty_strings(lidar_calibration_sensor_kit_frames); - remove_empty_strings(calibration_camera_frames); - remove_empty_strings(calibration_lidar_base_frames); remove_empty_strings(mapping_data_->calibration_camera_optical_link_frame_names); remove_empty_strings(mapping_data_->calibration_lidar_frame_names_); remove_empty_strings(calibration_camera_info_topics); @@ -123,6 +98,8 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( mapping_data_->mapping_lidar_frame_ = this->declare_parameter("mapping_lidar_frame"); + mapping_parameters_->registrator_name_ = + this->declare_parameter("mapping_registrator"); mapping_parameters_->mapping_verbose_ = this->declare_parameter("mapping_verbose", false); calibration_parameters_->calibration_verbose_ = this->declare_parameter("calibration_verbose", false); @@ -159,7 +136,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( this->declare_parameter("mapping_viz_leaf_size", 0.15); calibration_parameters_->calibration_viz_leaf_size_ = this->declare_parameter("calibration_viz_leaf_size", 0.15); - mapping_parameters_->leaf_size_input_ = this->declare_parameter("leaf_size_iput", 0.1); + mapping_parameters_->leaf_size_input_ = this->declare_parameter("leaf_size_input", 0.1); mapping_parameters_->leaf_size_local_map_ = this->declare_parameter("leaf_size_local_map", 0.1); calibration_parameters_->leaf_size_dense_map_ = @@ -237,8 +214,8 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( this->declare_parameter("max_corr_dist_coarse", 0.5); calibration_parameters_->max_corr_dist_fine_ = this->declare_parameter("max_corr_dist_fine", 0.1); - calibration_parameters_->max_corr_dist_ultrafine_ = - this->declare_parameter("max_corr_dist_ultrafine", 0.05); + calibration_parameters_->max_corr_dist_ultra_fine_ = + this->declare_parameter("max_corr_dist_ultra_fine", 0.05); // Lidar calibration-only parameters calibration_parameters_->lidar_calibration_min_frames_ = @@ -246,7 +223,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( calibration_parameters_->lidar_calibration_max_frames_ = this->declare_parameter("lidar_calibration_max_frames", 10); - // Camera calibration-only parameters TODO(knzo25): sort the parameters + // Camera calibration-only parameters calibration_parameters_->camera_calibration_min_frames_ = this->declare_parameter("camera_calibration_min_frames", 1); calibration_parameters_->camera_calibration_max_frames_ = @@ -279,6 +256,8 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( this->declare_parameter("base_lidar_min_plane_points_percentage", 10.0); calibration_parameters_->base_lidar_max_cos_distance_ = this->declare_parameter("base_lidar_max_cos_distance", 0.5); + calibration_parameters_->base_lidar_overwrite_xy_yaw_ = + this->declare_parameter("base_lidar_overwrite_xy_yaw", false); auto map_pub = this->create_publisher("output_map", 10); @@ -300,10 +279,15 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( keyframe_path_pub, keyframe_markers_pub, rosbag2_pause_client_, rosbag2_resume_client_, tf_buffer_); - // Set up lidar calibrators - for (const auto & frame_name : mapping_data_->calibration_camera_optical_link_frame_names) { - auto target_map_pub = - this->create_publisher(frame_name + "/target_map", 10); + // Set up camera calibrators + for (std::size_t camera_id = 0; + camera_id < mapping_data_->calibration_camera_optical_link_frame_names.size(); camera_id++) { + const auto & frame_name = mapping_data_->calibration_camera_optical_link_frame_names[camera_id]; + const std::string calibration_lidar_namespace = + "calibration_lidar_" + std::to_string(camera_id); + + auto target_map_pub = this->create_publisher( + calibration_lidar_namespace + "/target_map", 10); auto target_markers_pub = this->create_publisher( frame_name + "/target_markers", 10); @@ -312,19 +296,24 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( target_markers_pub); } - for (const auto & frame_name : mapping_data_->calibration_lidar_frame_names_) { + // Set up lidar calibrators + for (std::size_t lidar_id = 0; lidar_id < mapping_data_->calibration_lidar_frame_names_.size(); + lidar_id++) { + const auto & frame_name = mapping_data_->calibration_lidar_frame_names_[lidar_id]; + const std::string calibration_lidar_namespace = "calibration_lidar_" + std::to_string(lidar_id); auto initial_source_aligned_map_pub = this->create_publisher( - frame_name + "/initial_source_aligned_map", 10); + calibration_lidar_namespace + "/initial_source_aligned_map", 10); auto calibrated_source_aligned_map_pub = this->create_publisher( - frame_name + "/calibrated_source_aligned_map", 10); - auto target_map_pub = - this->create_publisher(frame_name + "/target_map", 10); + calibration_lidar_namespace + "/calibrated_source_aligned_map", 10); + auto target_map_pub = this->create_publisher( + calibration_lidar_namespace + "/target_map", 10); lidar_calibrators_[frame_name] = std::make_shared( frame_name, calibration_parameters_, mapping_data_, tf_buffer_, initial_source_aligned_map_pub, calibrated_source_aligned_map_pub, target_map_pub); } + // Set up base calibrators auto base_lidar_augmented_pointcloud_pub = this->create_publisher("base_lidar_augmented_pointcloud", 10); auto base_lidar_augmented_pub = @@ -334,6 +323,15 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( calibration_parameters_, mapping_data_, tf_buffer_, tf_broadcaster_, base_lidar_augmented_pointcloud_pub, base_lidar_augmented_pub); + srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + service_server_ = this->create_service( + "extrinsic_calibration", + std::bind( + &ExtrinsicMappingBasedCalibrator::requestReceivedCallback, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, srv_callback_group_); + // Set up sensor callbacks assert( mapping_data_->calibration_camera_optical_link_frame_names.size() == @@ -346,26 +344,6 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( const std::string & calibration_frame_name = mapping_data_->calibration_camera_optical_link_frame_names[i]; - sensor_kit_frame_map_[calibration_frame_name] = camera_calibration_sensor_kit_frames[i]; - calibration_camera_frame_map_[calibration_frame_name] = calibration_camera_frames[i]; - - srv_callback_groups_map_[calibration_frame_name] = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - calibration_api_server_map_[calibration_frame_name] = - this->create_service( - camera_calibration_service_names[i] + "/extrinsic_calibration", - [&]( - const std::shared_ptr request, - const std::shared_ptr - response) { - cameraCalibrationRequestReceivedCallback( - sensor_kit_frame_map_[calibration_frame_name], - calibration_camera_frame_map_[calibration_frame_name], calibration_frame_name, request, - response); - }, - rmw_qos_profile_services_default, srv_callback_groups_map_[calibration_frame_name]); - calibration_camera_info_subs_[calibration_frame_name] = this->create_subscription( calibration_camera_info_topic, rclcpp::SensorDataQoS().keep_all(), @@ -387,26 +365,6 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( const std::string & calibration_pointcloud_topic = calibration_pointcloud_topics[i]; const std::string & calibration_frame_name = mapping_data_->calibration_lidar_frame_names_[i]; - sensor_kit_frame_map_[calibration_frame_name] = lidar_calibration_sensor_kit_frames[i]; - calibration_lidar_base_frame_map_[calibration_frame_name] = calibration_lidar_base_frames[i]; - - srv_callback_groups_map_[calibration_frame_name] = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - calibration_api_server_map_[calibration_frame_name] = - this->create_service( - lidar_calibration_service_names[i] + "/extrinsic_calibration", - [&]( - const std::shared_ptr request, - const std::shared_ptr - response) { - lidarCalibrationRequestReceivedCallback( - sensor_kit_frame_map_[calibration_frame_name], - calibration_lidar_base_frame_map_[calibration_frame_name], calibration_frame_name, - request, response); - }, - rmw_qos_profile_services_default, srv_callback_groups_map_[calibration_frame_name]); - calibration_pointcloud_subs_[calibration_frame_name] = this->create_subscription( calibration_pointcloud_topic, rclcpp::SensorDataQoS().keep_all(), @@ -431,47 +389,11 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( std::bind( &ExtrinsicMappingBasedCalibrator::predictedObjectsCallback, this, std::placeholders::_1)); - for (auto & calibration_frame_name : mapping_data_->calibration_lidar_frame_names_) { - single_lidar_calibration_server_map_[calibration_frame_name] = - this->create_service( - calibration_frame_name + "/single_lidar_calibration", - std::bind( - &LidarCalibrator::singleSensorCalibrationCallback, - lidar_calibrators_[calibration_frame_name], std::placeholders::_1, std::placeholders::_2), - rmw_qos_profile_services_default); - - multiple_lidar_calibration_server_map_[calibration_frame_name] = - this->create_service( - calibration_frame_name + "/multiple_lidar_calibration", - std::bind( - &LidarCalibrator::multipleSensorCalibrationCallback, - lidar_calibrators_[calibration_frame_name], std::placeholders::_1, std::placeholders::_2), - rmw_qos_profile_services_default); - } - - base_link_calibration_server_ = this->create_service( - "base_link_calibration", - [&]( - const std::shared_ptr request, - const std::shared_ptr response) { - UNUSED(request); - UNUSED(response); - Eigen::Matrix4f transform; - float score; - std::unique_lock data_lock(mapping_data_->mutex_); - RCLCPP_INFO_STREAM(this->get_logger(), "Starting base lidar calibration"); - base_lidar_calibrator_->calibrate(transform, score); - }, - rmw_qos_profile_services_default); - stop_mapping_server_ = this->create_service( "stop_mapping", [&]( - const std::shared_ptr request, - const std::shared_ptr response) { - UNUSED(request); - UNUSED(response); - std::unique_lock data_lock(mapping_data_->mutex_); + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) { mapper_->stop(); RCLCPP_INFO_STREAM(this->get_logger(), "Mapper stopped through service"); }, @@ -483,6 +405,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( &ExtrinsicMappingBasedCalibrator::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); + save_database_server_ = this->create_service( "save_database", std::bind( @@ -491,10 +414,10 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( rmw_qos_profile_services_default); publisher_timer_ = rclcpp::create_timer( - this, get_clock(), 5s, std::bind(&CalibrationMapper::publisherTimerCallback, mapper_)); + this, this->get_clock(), 5s, std::bind(&CalibrationMapper::publisherTimerCallback, mapper_)); data_matching_timer_ = rclcpp::create_timer( - this, get_clock(), 1s, std::bind(&CalibrationMapper::dataMatchingTimerCallback, mapper_)); + this, this->get_clock(), 1s, std::bind(&CalibrationMapper::dataMatchingTimerCallback, mapper_)); } rcl_interfaces::msg::SetParametersResult ExtrinsicMappingBasedCalibrator::paramCallback( @@ -504,9 +427,18 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicMappingBasedCalibrator::paramC result.successful = true; result.reason = "success"; + std::unique_lock service_lock(service_mutex_); + std::unique_lock mapping_lock(mapping_data_->mutex_); + + if (mapper_->getState() == CalibrationMapper::MAPPING || calibration_pending_) { + RCLCPP_WARN(this->get_logger(), "Can not modify the parameters while mapping or calibrating"); + result.successful = false; + result.reason = "Attempted to modify the parameters while mapping / calibrating"; + return result; + } + MappingParameters mapping_parameters = *mapping_parameters_; CalibrationParameters calibration_parameters = *calibration_parameters_; - std::unique_lock lock(mapping_data_->mutex_); try { UPDATE_PARAM(mapping_parameters, use_rosbag); @@ -576,203 +508,124 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicMappingBasedCalibrator::paramC return result; } -void ExtrinsicMappingBasedCalibrator::cameraCalibrationRequestReceivedCallback( - const std::string & parent_frame, const std::string & calibration_camera_frame, - const std::string & calibration_camera_optical_link_frame, - const std::shared_ptr request, +void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( + [[maybe_unused]] const std::shared_ptr + request, const std::shared_ptr response) { - (void)request; - using std::chrono_literals::operator""ms; - - Eigen::Isometry3d parent_to_mapping_lidar_eigen; - Eigen::Isometry3d camera_to_camera_optical_link_eigen; - - { - std::unique_lock service_lock(service_mutex_); - std::unique_lock data_lock(mapping_data_->mutex_); - - calibration_pending_map_[calibration_camera_optical_link_frame] = true; - calibration_status_map_[calibration_camera_optical_link_frame] = false; - calibration_results_map_[calibration_camera_optical_link_frame] = Eigen::Matrix4f::Identity(); + using std::chrono_literals::operator""s; - RCLCPP_INFO_STREAM(this->get_logger(), "Calibration request received"); - RCLCPP_INFO_STREAM(this->get_logger(), "\t\tparent_frame = " << parent_frame); - RCLCPP_INFO_STREAM( - this->get_logger(), "\t\tcalibration_camera_frame = " << calibration_camera_frame); - RCLCPP_INFO_STREAM( - this->get_logger(), - "\t\tcalibration_camera_optical_link_frame = " << calibration_camera_optical_link_frame); - - try { - rclcpp::Time t = rclcpp::Time(0); - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - - geometry_msgs::msg::Transform parent_to_mapping_lidar_msg = - tf_buffer_->lookupTransform(parent_frame, mapping_data_->mapping_lidar_frame_, t, timeout) - .transform; - - parent_to_mapping_lidar_eigen = tf2::transformToEigen(parent_to_mapping_lidar_msg); - - geometry_msgs::msg::Transform camera_to_camera_optical_link_msg = - tf_buffer_ - ->lookupTransform( - calibration_camera_frame, calibration_camera_optical_link_frame, t, timeout) - .transform; - - camera_to_camera_optical_link_eigen = - tf2::transformToEigen(camera_to_camera_optical_link_msg); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - this->get_logger(), "could not get initial tfs. Aborting calibration. %s", ex.what()); - response->success = false; - return; - } + if (mapper_->getState() == CalibrationMapper::INITIAL) { + mapper_->start(); + } else { + RCLCPP_WARN(this->get_logger(), "The mapper had already started / finished !"); } - // Start monitoring and calibration frame - std::thread t([&]() { - while (!mapper_->stopped() && rclcpp::ok()) { - rclcpp::sleep_for(1000ms); - } - - Eigen::Matrix4f calibration_result; - float calibration_score; - bool calibration_status = camera_calibrators_[calibration_camera_optical_link_frame]->calibrate( - calibration_result, calibration_score); + // Wait until map finishes + while (mapper_->getState() != CalibrationMapper::FINISHED && rclcpp::ok()) { + rclcpp::sleep_for(1s); + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 30000, "Waiting for the mapper to finish"); + } + { std::unique_lock lock(service_mutex_); - calibration_pending_map_[calibration_camera_optical_link_frame] = false; - calibration_status_map_[calibration_camera_optical_link_frame] = calibration_status; - calibration_results_map_[calibration_camera_optical_link_frame] = calibration_result; - calibration_scores_map_[calibration_camera_optical_link_frame] = calibration_score; - }); - - while (rclcpp::ok()) { - { - std::unique_lock lock(service_mutex_); - - if (!calibration_pending_map_[calibration_camera_optical_link_frame]) { - break; - } + if (calibration_pending_) { + RCLCPP_WARN(this->get_logger(), "There is a calibration pending !. Aborting this request"); + return; } - rclcpp::sleep_for(1000ms); + calibration_pending_ = true; } - std::unique_lock lock(service_mutex_); - t.join(); + std::vector calibrator_thread_pool; - Eigen::Isometry3d mapping_to_calibration_lidar_lidar_eigen = Eigen::Isometry3d( - calibration_results_map_[calibration_camera_optical_link_frame].cast()); - Eigen::Isometry3d parent_to_lidar_base_eigen = parent_to_mapping_lidar_eigen * - mapping_to_calibration_lidar_lidar_eigen * - camera_to_camera_optical_link_eigen.inverse(); + for (const std::string & calibration_frame_name : mapping_data_->calibration_lidar_frame_names_) { + calibrator_thread_pool.emplace_back([&]() { + RCLCPP_INFO( + this->get_logger(), "Beginning lidar calibration for %s", calibration_frame_name.c_str()); - response->result_pose = tf2::toMsg(parent_to_lidar_base_eigen); - response->score = calibration_scores_map_[calibration_camera_optical_link_frame]; - response->success = calibration_status_map_[calibration_camera_optical_link_frame]; + auto [status, transform, score] = lidar_calibrators_[calibration_frame_name]->calibrate(); - // Convert raw calibration to the output tf - RCLCPP_INFO_STREAM(this->get_logger(), "Sending the tesults to the calibrator manager"); -} + RCLCPP_INFO( + this->get_logger(), "Lidar calibration for %s finished", calibration_frame_name.c_str()); -void ExtrinsicMappingBasedCalibrator::lidarCalibrationRequestReceivedCallback( - const std::string & parent_frame, const std::string & calibration_lidar_base_frame, - const std::string & calibration_lidar_frame, - const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request; - using std::chrono_literals::operator""ms; + std::unique_lock lock(service_mutex_); + calibration_pending_map_[calibration_frame_name] = false; + + tier4_calibration_msgs::msg::CalibrationResult result; + result.transform_stamped = tf2::eigenToTransform(Eigen::Isometry3d(transform)); + result.transform_stamped.header.frame_id = mapping_data_->mapping_lidar_frame_; + result.transform_stamped.child_frame_id = calibration_frame_name; + result.score = score; + result.success = status; + result.message.data = "Score corresponds to the source->target distance error"; + response->results.emplace_back(result); + }); + } - Eigen::Isometry3d parent_to_mapping_lidar_eigen; - Eigen::Isometry3d lidar_base_to_lidar_eigen; + for (const std::string & calibration_frame_name : + mapping_data_->calibration_camera_optical_link_frame_names) { + calibrator_thread_pool.emplace_back([&]() { + RCLCPP_INFO( + this->get_logger(), "Beginning camera calibration for %s", calibration_frame_name.c_str()); - { - std::unique_lock service_lock(service_mutex_); - std::unique_lock data_lock(mapping_data_->mutex_); + auto [status, transform, score] = camera_calibrators_[calibration_frame_name]->calibrate(); - calibration_pending_map_[calibration_lidar_frame] = true; - calibration_status_map_[calibration_lidar_frame] = false; - calibration_results_map_[calibration_lidar_frame] = Eigen::Matrix4f::Identity(); + RCLCPP_INFO( + this->get_logger(), "Camera calibration for %s finished", calibration_frame_name.c_str()); - RCLCPP_INFO_STREAM(this->get_logger(), "Calibration request received"); - RCLCPP_INFO_STREAM(this->get_logger(), "\t\tparent_frame = " << parent_frame); - RCLCPP_INFO_STREAM( - this->get_logger(), "\t\tcalibration_lidar_base_frame = " << calibration_lidar_base_frame); - RCLCPP_INFO_STREAM( - this->get_logger(), "\t\tcalibration_lidar_frame = " << calibration_lidar_frame); + std::unique_lock lock(service_mutex_); + calibration_pending_map_[calibration_frame_name] = false; + tier4_calibration_msgs::msg::CalibrationResult result; + result.transform_stamped = tf2::eigenToTransform(Eigen::Isometry3d(transform)); + result.transform_stamped.header.frame_id = mapping_data_->mapping_lidar_frame_; + result.transform_stamped.child_frame_id = calibration_frame_name; + result.score = score; + result.success = status; + result.message.data = "Not implemented"; + response->results.emplace_back(result); + }); + } - try { - rclcpp::Time t = rclcpp::Time(0); - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); + if (calibration_parameters_->calibrate_base_frame_) { + calibrator_thread_pool.emplace_back([&]() { + const std::string & base_frame = calibration_parameters_->base_frame_; + if (base_frame == "") { + RCLCPP_INFO(this->get_logger(), "Base frame can not be empty"); + return; + } - geometry_msgs::msg::Transform parent_to_mapping_lidar_msg = - tf_buffer_->lookupTransform(parent_frame, mapping_data_->mapping_lidar_frame_, t, timeout) - .transform; + RCLCPP_INFO( + this->get_logger(), "Beginning ground plane calibration for %s", base_frame.c_str()); - parent_to_mapping_lidar_eigen = tf2::transformToEigen(parent_to_mapping_lidar_msg); + auto [status, transform, score] = base_lidar_calibrator_->calibrate(); - geometry_msgs::msg::Transform lidar_base_to_lidar_msg = - tf_buffer_ - ->lookupTransform(calibration_lidar_base_frame, calibration_lidar_frame, t, timeout) - .transform; + RCLCPP_INFO( + this->get_logger(), "Ground plane calibration for %s finished", base_frame.c_str()); - lidar_base_to_lidar_eigen = tf2::transformToEigen(lidar_base_to_lidar_msg); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - this->get_logger(), "could not get initial tfs. Aborting calibration. %s", ex.what()); - response->success = false; - return; - } + std::unique_lock lock(service_mutex_); + calibration_pending_map_[base_frame] = false; + tier4_calibration_msgs::msg::CalibrationResult result; + result.transform_stamped = tf2::eigenToTransform(Eigen::Isometry3d(transform)); + result.transform_stamped.header.frame_id = mapping_data_->mapping_lidar_frame_; + result.transform_stamped.child_frame_id = base_frame; + result.score = score; + result.success = status; + result.message.data = "Base calibration provides no score"; + response->results.emplace_back(result); + }); } - // Start monitoring and calibration frame - std::thread t([&]() { - while (!mapper_->stopped() && rclcpp::ok()) { - rclcpp::sleep_for(1000ms); - } - - Eigen::Matrix4f calibration_result; - float calibration_score; - bool calibration_status = - lidar_calibrators_[calibration_lidar_frame]->calibrate(calibration_result, calibration_score); - - std::unique_lock lock(service_mutex_); - calibration_pending_map_[calibration_lidar_frame] = false; - calibration_status_map_[calibration_lidar_frame] = calibration_status; - calibration_results_map_[calibration_lidar_frame] = calibration_result; - calibration_scores_map_[calibration_lidar_frame] = calibration_score; + // Wait until all calibrators finish + std::for_each(calibrator_thread_pool.begin(), calibrator_thread_pool.end(), [](std::thread & t) { + t.join(); }); - while (rclcpp::ok()) { - { - std::unique_lock lock(service_mutex_); - - if (!calibration_pending_map_[calibration_lidar_frame]) { - break; - } - } - - rclcpp::sleep_for(1000ms); - } + RCLCPP_INFO_STREAM(this->get_logger(), "Sending the results to the calibrator manager"); std::unique_lock lock(service_mutex_); - t.join(); - - Eigen::Isometry3d mapping_to_calibration_lidar_lidar_eigen = - Eigen::Isometry3d(calibration_results_map_[calibration_lidar_frame].cast()); - Eigen::Isometry3d parent_to_lidar_base_eigen = parent_to_mapping_lidar_eigen * - mapping_to_calibration_lidar_lidar_eigen * - lidar_base_to_lidar_eigen.inverse(); - - response->result_pose = tf2::toMsg(parent_to_lidar_base_eigen); - response->score = calibration_scores_map_[calibration_lidar_frame]; - response->success = calibration_status_map_[calibration_lidar_frame]; - - // Convert raw calibration to the output tf - RCLCPP_INFO_STREAM(this->get_logger(), "Sending the tesults to the calibrator manager"); + calibration_pending_ = false; } void ExtrinsicMappingBasedCalibrator::detectedObjectsCallback( @@ -796,7 +649,7 @@ void ExtrinsicMappingBasedCalibrator::detectedObjectsCallback( RCLCPP_INFO(this->get_logger(), "Adding %ld detections", new_objects.objects_.size()); // Add them to the data - std::unique_lock lock(mapping_data_->mutex_); + std::unique_lock lock(mapping_data_->mutex_); mapping_data_->detected_objects_.push_back(new_objects); } @@ -821,7 +674,7 @@ void ExtrinsicMappingBasedCalibrator::predictedObjectsCallback( RCLCPP_INFO(this->get_logger(), "Adding %ld detections", new_objects.objects_.size()); // Add them to the data - std::unique_lock lock(mapping_data_->mutex_); + std::unique_lock lock(mapping_data_->mutex_); mapping_data_->detected_objects_.push_back(new_objects); } @@ -829,10 +682,11 @@ void ExtrinsicMappingBasedCalibrator::loadDatabaseCallback( const std::shared_ptr request, const std::shared_ptr response) { + // cSpell:ignore iarchive std::ifstream ifs(request->path); boost::archive::binary_iarchive ia(ifs); - std::unique_lock lock(mapping_data_->mutex_); + std::unique_lock lock(mapping_data_->mutex_); mapping_data_->camera_calibration_frames_map_.clear(); mapping_data_->lidar_calibration_frames_map_.clear(); @@ -877,10 +731,11 @@ void ExtrinsicMappingBasedCalibrator::saveDatabaseCallback( const std::shared_ptr request, const std::shared_ptr response) { + // cSpell:ignore oarchive std::ofstream ofs(request->path); boost::archive::binary_oarchive oa(ofs); - std::unique_lock lock(mapping_data_->mutex_); + std::unique_lock lock(mapping_data_->mutex_); for (auto it = mapping_data_->camera_calibration_frames_map_.begin(); it != mapping_data_->camera_calibration_frames_map_.end(); it++) { diff --git a/sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp b/sensor/mapping_based_calibrator/src/sensor_calibrator.cpp similarity index 95% rename from sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp rename to sensor/mapping_based_calibrator/src/sensor_calibrator.cpp index 1465949a..13f6ae63 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/sensor_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include SensorCalibrator::SensorCalibrator( diff --git a/sensor/extrinsic_mapping_based_calibrator/src/utils.cpp b/sensor/mapping_based_calibrator/src/utils.cpp similarity index 95% rename from sensor/extrinsic_mapping_based_calibrator/src/utils.cpp rename to sensor/mapping_based_calibrator/src/utils.cpp index 07856e8b..68c63125 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/utils.cpp +++ b/sensor/mapping_based_calibrator/src/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -13,20 +13,15 @@ // limitations under the License. #include -#include +#include #include +#include #include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include template @@ -92,6 +87,7 @@ Eigen::Matrix4f poseInterpolationBase( Eigen::Affine3f aff1(m1); Eigen::Affine3f aff2(m2); + // cSpell:ignore Quaternionf Eigen::Quaternionf rot1(aff1.linear()); Eigen::Quaternionf rot2(aff2.linear()); @@ -114,19 +110,18 @@ Eigen::Matrix4f poseInterpolation( return poseInterpolationBase(t, t1, t2, m1, m2); } + // Extrapolation case double dt = t2 - t1; - double te = t - t2; + double t_rem = t - t2; Eigen::Matrix4f m = m2; Eigen::Matrix4f dm = m1.inverse() * m2; - while (te >= dt) { + while (t_rem >= dt) { m = m * dm; - te -= dt; + t_rem -= dt; } - auto asd = poseInterpolationBase(te, 0, dt, Eigen::Matrix4f::Identity(), dm); - - return m * asd; + return m * poseInterpolationBase(t_rem, 0, dt, Eigen::Matrix4f::Identity(), dm); } template @@ -137,15 +132,10 @@ float sourceTargetDistance( pcl::Correspondences correspondences; estimator.determineCorrespondences(correspondences, max_corr_distance); - int n_points = static_cast(correspondences.size()); - float sum = 0; - - for (int i = 0; i < n_points; ++i) { - float distance = correspondences[i].distance; - sum += distance; - } - - return sum / n_points; + return std::transform_reduce( + correspondences.begin(), correspondences.end(), 0.0, std::plus{}, + [](auto & correspondence) { return correspondence.distance; }) / + correspondences.size(); } template diff --git a/sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt b/sensor/marker_radar_lidar_calibrator/CMakeLists.txt similarity index 71% rename from sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt rename to sensor/marker_radar_lidar_calibrator/CMakeLists.txt index 89695f72..abb003dd 100644 --- a/sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt +++ b/sensor/marker_radar_lidar_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_reflector_based_calibrator) +project(marker_radar_lidar_calibrator) find_package(autoware_cmake REQUIRED) @@ -13,13 +13,13 @@ ament_export_include_directories( ${OpenCV_INCLUDE_DIRS} ) -ament_auto_add_executable(extrinsic_reflector_based_calibrator - src/extrinsic_reflector_based_calibrator.cpp +ament_auto_add_executable(marker_radar_lidar_calibrator + src/marker_radar_lidar_calibrator.cpp src/track.cpp src/main.cpp ) -target_link_libraries(extrinsic_reflector_based_calibrator +target_link_libraries(marker_radar_lidar_calibrator ${OpenCV_LIBS} ) diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp similarity index 89% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp rename to sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp index 3eee6d3c..fd44ee99 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__EXTRINSIC_REFLECTOR_BASED_CALIBRATOR_HPP_ -#define EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__EXTRINSIC_REFLECTOR_BASED_CALIBRATOR_HPP_ +#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ +#define MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ #include -#include -#include +#include +#include #include #include #include @@ -25,7 +25,9 @@ #include #include +#include #include +#include #include #include @@ -36,15 +38,10 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include +#include #include #include #include @@ -53,6 +50,9 @@ #include #include +namespace marker_radar_lidar_calibrator +{ + class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node { public: @@ -149,7 +149,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node struct Parameters { - std::string parent_frame; + std::string radar_parallel_frame; // frame that is assumed to be parallel to the radar (needed + // for radars that do not provide elevation) bool use_lidar_initial_crop_box_filter; double lidar_initial_crop_box_min_x; double lidar_initial_crop_box_min_y; @@ -230,22 +231,22 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node // Initial tfs comparable with the one with our method geometry_msgs::msg::Transform initial_radar_to_lidar_msg_; - tf2::Transform initial_radar_to_lidar_tf2_; Eigen::Isometry3d initial_radar_to_lidar_eigen_; Eigen::Isometry3d calibrated_radar_to_lidar_eigen_; - geometry_msgs::msg::Transform parent_to_lidar_msg_; - tf2::Transform parent_to_lidar_tf2_; - Eigen::Isometry3d parent_to_lidar_eigen_; + geometry_msgs::msg::Transform radar_parallel_to_lidar_msg_; + Eigen::Isometry3d radar_parallel_to_lidar_eigen_; - bool got_initial_transform_; - bool broadcast_tf_; - bool calibration_valid_; - bool send_calibration_; + bool got_initial_transform_{false}; + bool broadcast_tf_{false}; + bool calibration_valid_{false}; + double calibration_distance_score_{std::numeric_limits::max()}; + double calibration_yaw_score_{std::numeric_limits::max()}; + bool send_calibration_{false}; // Background model - bool extract_lidar_background_model_; - bool extract_radar_background_model_; + bool extract_lidar_background_model_{false}; + bool extract_radar_background_model_{false}; std_msgs::msg::Header latest_updated_lidar_header_; std_msgs::msg::Header latest_updated_radar_header_; std_msgs::msg::Header first_lidar_header_; @@ -257,8 +258,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node radar_msgs::msg::RadarTracks::SharedPtr latest_radar_msgs_; // Tracking - bool tracking_active_; - int current_new_tracks_; + bool tracking_active_{false}; + int current_new_tracks_{false}; TrackFactory::Ptr factory_ptr_; std::vector active_tracks_; std::vector converged_tracks_; @@ -269,4 +270,6 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node static constexpr int MARKER_SIZE_PER_TRACK = 8; }; -#endif // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__EXTRINSIC_REFLECTOR_BASED_CALIBRATOR_HPP_ +} // namespace marker_radar_lidar_calibrator + +#endif // MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp similarity index 85% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp rename to sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp index ee0f6349..d6a7a968 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,17 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TRACK_HPP_ -#define EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TRACK_HPP_ +#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ +#define MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ #include #include -#include #include +#include #include #include +namespace marker_radar_lidar_calibrator +{ + class Track { public: @@ -64,7 +67,7 @@ class TrackFactory TrackFactory( double initial_lidar_cov, double initial_radar_cov, double lidar_measurement_cov, double radar_measurement_cov, double lidar_process_cov, double radar_process_cov, - double lidar_convergence_tresh, double radar_convergence_thresh, double timeout_thresh, + double lidar_convergence_thresh, double radar_convergence_thresh, double timeout_thresh, double max_matching_distance); Track makeTrack( @@ -81,4 +84,6 @@ class TrackFactory double max_matching_distance_; }; -#endif // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TRACK_HPP_ +} // namespace marker_radar_lidar_calibrator + +#endif // MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp similarity index 80% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp rename to sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp index e05c2fbf..a5700ad1 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TYPES_HPP_ -#define EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TYPES_HPP_ +#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +#define MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ #include @@ -25,11 +25,14 @@ #include #include +namespace marker_radar_lidar_calibrator +{ + struct BackgroundModel { public: using PointType = pcl::PointXYZ; - using TreeType = pcl::KdTreeFLANN; + using TreeType = pcl::KdTreeFLANN; // cSpell:ignore FLANN using index_t = std::uint32_t; BackgroundModel() @@ -54,4 +57,6 @@ struct BackgroundModel TreeType tree_; }; -#endif // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TYPES_HPP_ +} // namespace marker_radar_lidar_calibrator + +#endif // MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml b/sensor/marker_radar_lidar_calibrator/launch/calibrator.launch.xml similarity index 72% rename from sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml rename to sensor/marker_radar_lidar_calibrator/launch/calibrator.launch.xml index 75004f6a..f6b11b6a 100644 --- a/sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml +++ b/sensor/marker_radar_lidar_calibrator/launch/calibrator.launch.xml @@ -1,7 +1,10 @@ - + + + + @@ -25,8 +28,10 @@ - - + + + + @@ -49,7 +54,11 @@ - - + + + + + + diff --git a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/__init__.py b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/__init__.py similarity index 100% rename from sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/__init__.py rename to sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/__init__.py diff --git a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py similarity index 98% rename from sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py rename to sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py index bd01043d..3c8698b7 100644 --- a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py +++ b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Tier IV, Inc. +# 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. @@ -23,7 +23,7 @@ class CalibratorUI(QMainWindow): def __init__(self, ros_interface): super().__init__() - self.setWindowTitle("Reflector-based lidar-radar calibrator") + self.setWindowTitle("Marker radar-lidar calibrator") # ROS Interface self.ros_interface = ros_interface diff --git a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py similarity index 97% rename from sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py rename to sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py index d7ceafea..e736c6a2 100644 --- a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py +++ b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Tier IV, Inc. +# 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. @@ -59,7 +59,7 @@ def __call__(self): class RosInterface(Node): def __init__(self): - super().__init__("extrinsic_reflector_based_calibrator_ui") + super().__init__("marker_radar_lidar_calibrator_ui") self.ros_context = None self.ros_executor = None diff --git a/sensor/extrinsic_reflector_based_calibrator/package.xml b/sensor/marker_radar_lidar_calibrator/package.xml similarity index 89% rename from sensor/extrinsic_reflector_based_calibrator/package.xml rename to sensor/marker_radar_lidar_calibrator/package.xml index 8932c4fa..3c4059b9 100644 --- a/sensor/extrinsic_reflector_based_calibrator/package.xml +++ b/sensor/marker_radar_lidar_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_reflector_based_calibrator + marker_radar_lidar_calibrator 0.0.1 - The extrinsic_reflector_based_calibrator package + The marker_radar_lidar_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz b/sensor/marker_radar_lidar_calibrator/rviz/default.rviz similarity index 81% rename from sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz rename to sensor/marker_radar_lidar_calibrator/rviz/default.rviz index cde57172..90edc3bf 100644 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz +++ b/sensor/marker_radar_lidar_calibrator/rviz/default.rviz @@ -8,7 +8,6 @@ Panels: - /lidar1/Topic1 - /lidar_background_pointcloud1/Topic1 - /lidar_colored_clusters1/Topic1 - - /DetectedObjects1/Topic1 Splitter Ratio: 0.5 Tree Height: 1106 - Class: rviz_common/Selection @@ -28,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: lidar Visualization Manager: Class: "" Displays: @@ -69,7 +68,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/rear_lower/pointcloud_raw + Value: /pointcloud_topic Use Fixed Frame: true Use rainbow: true Value: true @@ -103,7 +102,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/lidar_background_pointcloud + Value: /lidar_background_pointcloud Use Fixed Frame: true Use rainbow: true Value: false @@ -137,7 +136,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/lidar_foreground_pointcloud + Value: /lidar_foreground_pointcloud Use Fixed Frame: true Use rainbow: true Value: false @@ -171,7 +170,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/lidar_colored_clusters + Value: /lidar_colored_clusters Use Fixed Frame: true Use rainbow: true Value: true @@ -179,72 +178,26 @@ Visualization Manager: Enabled: true Name: lidar_detections Namespaces: - {} + "": true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/lidar_detection_markers + Value: /lidar_detection_markers Value: true - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: false - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/rear_left/detected_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: false - Visualization Type: Normal - Class: rviz_default_plugins/MarkerArray Enabled: true Name: radar_detections Namespaces: - {} + center: true + line: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/radar_detection_markers + Value: /radar_detection_markers Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -258,7 +211,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -276,21 +229,22 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/radar_background_pointcloud + Value: /radar_background_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: tracking_markers Namespaces: - {} + calibrated: true + initial: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/tracking_markers + Value: /tracking_markers Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -302,7 +256,19 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /rear_unit/rear_unit_base_link/rear_left/radar_link/matches_markers + Value: /matches_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: text_markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /text_markers Value: true - Alpha: 0.5 Cell Size: 1 @@ -325,7 +291,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_rear + Fixed Frame: radar_frame Frame Rate: 30 Name: root Tools: @@ -376,14 +342,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.2947973608970642 + Pitch: 0.23479722440242767 Position: - X: 4.067726135253906 - Y: 0.976508617401123 - Z: 2.6349170207977295 + X: -6.539778709411621 + Y: 0.01612010970711708 + Z: 3.799168348312378 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 3.4413163661956787 + Yaw: 0.0031585693359375 Saved: ~ Window Geometry: Displays: diff --git a/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py b/sensor/marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py similarity index 84% rename from sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py rename to sensor/marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py index 9fbb257c..7fb8c626 100755 --- a/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -14,12 +14,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging import signal import sys from PySide2.QtWidgets import QApplication -from extrinsic_tag_based_base_calibrator import CalibratorUI -from extrinsic_tag_based_base_calibrator import RosInterface +from marker_radar_lidar_calibrator import CalibratorUI +from marker_radar_lidar_calibrator import RosInterface import rclpy @@ -38,7 +39,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quiting...", flush=True) + logging.info("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/extrinsic_reflector_based_calibrator/scripts/metrics_plotter_node.py b/sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py similarity index 99% rename from sensor/extrinsic_reflector_based_calibrator/scripts/metrics_plotter_node.py rename to sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py index c3d3ab7a..b3f4f0dc 100755 --- a/sensor/extrinsic_reflector_based_calibrator/scripts/metrics_plotter_node.py +++ b/sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Tier IV, Inc. +# 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. diff --git a/sensor/extrinsic_reflector_based_calibrator/src/main.cpp b/sensor/marker_radar_lidar_calibrator/src/main.cpp similarity index 72% rename from sensor/extrinsic_reflector_based_calibrator/src/main.cpp rename to sensor/marker_radar_lidar_calibrator/src/main.cpp index 374ecae8..65ef82a9 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/main.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include int main(int argc, char ** argv) @@ -21,8 +21,9 @@ int main(int argc, char ** argv) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::NodeOptions node_options; - std::shared_ptr node = - std::make_shared(node_options); + std::shared_ptr node = + std::make_shared( + node_options); executor.add_node(node); executor.spin(); diff --git a/sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp similarity index 92% rename from sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp rename to sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index 3cbc2307..0c81978c 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include +#include #include #include @@ -28,15 +30,11 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include +#include +#include #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(parameters, #NAME, PARAM_STRUCT.NAME) @@ -52,12 +50,15 @@ void update_param( if (it != parameters.cend()) { value = it->template get_value(); RCLCPP_INFO_STREAM( - rclcpp::get_logger("extrinsic_reflector_based_calibrator"), + rclcpp::get_logger("marker_radar_lidar_calibrator"), "Setting parameter [" << name << "] to " << value); } } } // namespace +namespace marker_radar_lidar_calibrator +{ + rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::paramCallback( const std::vector & parameters) { @@ -68,7 +69,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para Parameters p = parameters_; try { - UPDATE_PARAM(p, parent_frame); + UPDATE_PARAM(p, radar_parallel_frame); UPDATE_PARAM(p, use_lidar_initial_crop_box_filter); UPDATE_PARAM(p, lidar_initial_crop_box_min_x); UPDATE_PARAM(p, lidar_initial_crop_box_min_y); @@ -117,19 +118,11 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( const rclcpp::NodeOptions & options) -: Node("extrinsic_reflector_based_calibrator_node", options), - tf_broadcaster_(this), - got_initial_transform_(false), - calibration_valid_(false), - send_calibration_(false), - extract_lidar_background_model_(false), - extract_radar_background_model_(false), - tracking_active_(false), - current_new_tracks_(0) +: Node("marker_radar_lidar_calibrator_node", options), tf_broadcaster_(this) { tf_buffer_ = std::make_shared(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); - parameters_.parent_frame = this->declare_parameter("parent_frame"); + parameters_.radar_parallel_frame = this->declare_parameter("radar_parallel_frame"); parameters_.use_lidar_initial_crop_box_filter = this->declare_parameter("use_lidar_initial_crop_box_filter", true); @@ -288,8 +281,7 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( } void ExtrinsicReflectorBasedCalibrator::requestReceivedCallback( - __attribute__((unused)) - const std::shared_ptr + [[maybe_unused]] const std::shared_ptr request, const std::shared_ptr response) { @@ -309,12 +301,19 @@ void ExtrinsicReflectorBasedCalibrator::requestReceivedCallback( std::unique_lock lock(mutex_); - Eigen::Isometry3d calibrated_parent_to_radar_transform = - parent_to_lidar_eigen_ * calibrated_radar_to_lidar_eigen_.inverse(); - geometry_msgs::msg::Pose calibrated_parent_to_radar_pose = - toMsg(calibrated_parent_to_radar_transform); - response->result_pose = calibrated_parent_to_radar_pose; - response->success = true; + std::stringstream ss; + ss << "Calibration successful. distance_score=" << calibration_distance_score_ + << " yaw_score=" << calibration_yaw_score_; + + tier4_calibration_msgs::msg::CalibrationResult result; + result.message.data = ss.str(); + result.score = calibration_distance_score_; + result.success = true; + result.transform_stamped = tf2::eigenToTransform(calibrated_radar_to_lidar_eigen_); + result.transform_stamped.header.frame_id = radar_frame_; + result.transform_stamped.child_frame_id = lidar_frame_; + + response->results.emplace_back(result); } void ExtrinsicReflectorBasedCalibrator::timerCallback() @@ -349,8 +348,8 @@ void ExtrinsicReflectorBasedCalibrator::timerCallback() } void ExtrinsicReflectorBasedCalibrator::backgroundModelRequestCallback( - __attribute__((unused)) const std::shared_ptr request, - __attribute__((unused)) const std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) { using std::chrono_literals::operator""s; @@ -376,8 +375,8 @@ void ExtrinsicReflectorBasedCalibrator::backgroundModelRequestCallback( } void ExtrinsicReflectorBasedCalibrator::trackingRequestCallback( - __attribute__((unused)) const std::shared_ptr request, - __attribute__((unused)) const std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) { using std::chrono_literals::operator""s; @@ -405,8 +404,8 @@ void ExtrinsicReflectorBasedCalibrator::trackingRequestCallback( } void ExtrinsicReflectorBasedCalibrator::deleteTrackRequestCallback( - __attribute__((unused)) const std::shared_ptr request, - __attribute__((unused)) const std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) { using std::chrono_literals::operator""s; @@ -427,8 +426,8 @@ void ExtrinsicReflectorBasedCalibrator::deleteTrackRequestCallback( } void ExtrinsicReflectorBasedCalibrator::sendCalibrationCallback( - __attribute__((unused)) const std::shared_ptr request, - __attribute__((unused)) const std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) { std::unique_lock lock(mutex_); send_calibration_ = true; @@ -498,11 +497,11 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector pcl::PointCloud::Ptr tmp_lidar_pointcloud_ptr(new pcl::PointCloud); RCLCPP_INFO(this->get_logger(), "pre lidar_pointcloud_ptr=%lu", lidar_pointcloud_ptr->size()); RCLCPP_WARN( - this->get_logger(), "cropbox parameters=%f | %f | %f", + this->get_logger(), "crop box parameters=%f | %f | %f", parameters_.lidar_initial_crop_box_min_x, parameters_.lidar_initial_crop_box_min_y, parameters_.lidar_initial_crop_box_min_z); RCLCPP_WARN( - this->get_logger(), "cropbox parameters=%f | %f | %f", + this->get_logger(), "crop box parameters=%f | %f | %f", parameters_.lidar_initial_crop_box_max_x, parameters_.lidar_initial_crop_box_max_y, parameters_.lidar_initial_crop_box_max_z); box_filter.setMin(Eigen::Vector4f( @@ -555,7 +554,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector for (std::size_t i = 0; i < clusters.size(); i++) { const auto & cluster = clusters[i]; - pcl::PointXYZRGB colored_p; + pcl::PointXYZRGB colored_p; // cSpell:ignore XYZRGB auto & color = colors[i % 7]; for (const auto & p : cluster->points) { @@ -839,7 +838,7 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( pcl::SACSegmentation seg; pcl::ExtractIndices extract; seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); + seg.setModelType(pcl::SACMODEL_PLANE); // cSpell:ignore SACMODEL seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(parameters_.ransac_threshold); seg.setMaxIterations(parameters_.ransac_max_iterations); @@ -976,15 +975,14 @@ bool ExtrinsicReflectorBasedCalibrator::checkInitialTransforms() initial_radar_to_lidar_msg_ = tf_buffer_->lookupTransform(radar_frame_, lidar_frame_, t, timeout).transform; - fromMsg(initial_radar_to_lidar_msg_, initial_radar_to_lidar_tf2_); initial_radar_to_lidar_eigen_ = tf2::transformToEigen(initial_radar_to_lidar_msg_); calibrated_radar_to_lidar_eigen_ = initial_radar_to_lidar_eigen_; - parent_to_lidar_msg_ = - tf_buffer_->lookupTransform(parameters_.parent_frame, lidar_frame_, t, timeout).transform; + radar_parallel_to_lidar_msg_ = + tf_buffer_->lookupTransform(parameters_.radar_parallel_frame, lidar_frame_, t, timeout) + .transform; - fromMsg(parent_to_lidar_msg_, parent_to_lidar_tf2_); - parent_to_lidar_eigen_ = tf2::transformToEigen(parent_to_lidar_msg_); + radar_parallel_to_lidar_eigen_ = tf2::transformToEigen(radar_parallel_to_lidar_msg_); got_initial_transform_ = true; } catch (tf2::TransformException & ex) { @@ -1019,6 +1017,17 @@ ExtrinsicReflectorBasedCalibrator::matchDetections( return transformed_point; }); + RCLCPP_INFO( + this->get_logger(), + "Lidar reflectors in radar coordinate system (using the initial transformation)"); + for (std::size_t lidar_index = 0; lidar_index < lidar_detections.size(); lidar_index++) { + const auto & lidar_detection = lidar_detections_transformed[lidar_index]; + RCLCPP_INFO( + this->get_logger(), "\t Lidar reflector (rcs) id=%lu size=%lu center: x=%.2f y=%.2f z=%.2f", + lidar_index, lidar_detections.size(), lidar_detection.x(), lidar_detection.y(), + lidar_detection.z()); + } + std::vector lidar_to_radar_closest_idx, radar_to_lidar_closest_idx; lidar_to_radar_closest_idx.resize(lidar_detections.size()); radar_to_lidar_closest_idx.resize(radar_detections.size()); @@ -1164,6 +1173,7 @@ std::tuple< ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta() { // Define two sets of 2D points (just 3D points with z=0) + // Note: pcs=parallel coordinate system rcs=radar coordinate system pcl::PointCloud::Ptr lidar_points_pcs(new pcl::PointCloud); pcl::PointCloud::Ptr radar_points_rcs(new pcl::PointCloud); lidar_points_pcs->reserve(converged_tracks_.size()); @@ -1178,8 +1188,8 @@ ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta() auto track = converged_tracks_[track_index]; // lidar coordinates const auto & lidar_estimation = track.getLidarEstimation(); - // to parent coordinates - const auto & lidar_estimation_pcs = parent_to_lidar_eigen_ * lidar_estimation; + // to radar parallel coordinates + const auto & lidar_estimation_pcs = radar_parallel_to_lidar_eigen_ * lidar_estimation; // to radar coordinates const auto & lidar_transformed_estimation = initial_radar_to_lidar_eigen_ * lidar_estimation; const auto & radar_estimation_rcs = track.getRadarEstimation(); @@ -1241,18 +1251,39 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation( pcl::PointCloud::Ptr lidar_points_pcs, pcl::PointCloud::Ptr radar_points_rcs, double delta_cos_sum, double delta_sin_sum) { + // Note: pcs=parallel coordinate system rcs=radar coordinate system // Estimate full transformation using SVD pcl::registration::TransformationEstimationSVD estimator; - Eigen::Matrix4f full_radar_to_parent_transformation; + Eigen::Matrix4f full_radar_to_radar_parallel_transformation; estimator.estimateRigidTransformation( - *lidar_points_pcs, *radar_points_rcs, full_radar_to_parent_transformation); - Eigen::Isometry3d calibrated_2d_radar_to_parent_transformation( - full_radar_to_parent_transformation.cast()); + *lidar_points_pcs, *radar_points_rcs, full_radar_to_radar_parallel_transformation); + Eigen::Isometry3d calibrated_2d_radar_to_radar_parallel_transformation( + full_radar_to_radar_parallel_transformation.cast()); + + // Check that it is actually a 2D transformation + auto calibrated_2d_radar_to_radar_parallel_rpy = tier4_autoware_utils::getRPY( + tf2::toMsg(calibrated_2d_radar_to_radar_parallel_transformation).orientation); + double calibrated_2d_radar_to_radar_parallel_z = + calibrated_2d_radar_to_radar_parallel_transformation.translation().z(); + double calibrated_2d_radar_to_radar_parallel_roll = calibrated_2d_radar_to_radar_parallel_rpy.x; + double calibrated_2d_radar_to_radar_parallel_pitch = calibrated_2d_radar_to_radar_parallel_rpy.y; - calibrated_2d_radar_to_parent_transformation.translation().z() = - (initial_radar_to_lidar_eigen_ * parent_to_lidar_eigen_.inverse()).translation().z(); + if ( + calibrated_2d_radar_to_radar_parallel_z != 0.0 || + calibrated_2d_radar_to_radar_parallel_roll != 0.0 || + calibrated_2d_radar_to_radar_parallel_pitch != 0.0) { + RCLCPP_ERROR( + this->get_logger(), + "The estimated 2D translation was not really 2D. Continue at your own risk. z=%.3f roll=%.3f " + "pitch=%.3f", + calibrated_2d_radar_to_radar_parallel_z, calibrated_2d_radar_to_radar_parallel_roll, + calibrated_2d_radar_to_radar_parallel_pitch); + } + + calibrated_2d_radar_to_radar_parallel_transformation.translation().z() = + (initial_radar_to_lidar_eigen_ * radar_parallel_to_lidar_eigen_.inverse()).translation().z(); Eigen::Isometry3d calibrated_2d_radar_to_lidar_transformation = - calibrated_2d_radar_to_parent_transformation * parent_to_lidar_eigen_; + calibrated_2d_radar_to_radar_parallel_transformation * radar_parallel_to_lidar_eigen_; // Estimate the 2D transformation estimating only yaw double delta_cos = delta_cos_sum / converged_tracks_.size(); @@ -1320,6 +1351,8 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation( this->get_logger(), "The 2D calibration pose was chosen as the output calibration pose"); calibrated_radar_to_lidar_eigen_ = calibrated_2d_radar_to_lidar_transformation; calibration_valid_ = true; + calibration_distance_score_ = calibrated_2d_distance_error; + calibration_yaw_score_ = calibrated_2d_yaw_error; } else if ( calibrated_rotation_translation_difference < parameters_.max_initial_calibration_translation_error && @@ -1330,6 +1363,8 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation( "you need to collect more points"); calibrated_radar_to_lidar_eigen_ = calibrated_rotation_radar_to_lidar_transformation; calibration_valid_ = true; + calibration_distance_score_ = calibrated_rotation_distance_error; + calibration_yaw_score_ = calibrated_rotation_yaw_error; } else { RCLCPP_WARN( this->get_logger(), @@ -1369,14 +1404,15 @@ void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( pcl::PointCloud::Ptr lidar_points_pcs, pcl::PointCloud::Ptr radar_points_rcs) { + // Note: pcs=parallel coordinate system rcs=radar coordinate system int tracks_size = static_cast(converged_tracks_.size()); if (tracks_size <= 3) return; pcl::PointCloud::Ptr crossval_lidar_points_pcs(new pcl::PointCloud); pcl::PointCloud::Ptr crossval_radar_points_rcs(new pcl::PointCloud); pcl::registration::TransformationEstimationSVD crossval_estimator; - Eigen::Matrix4f crossval_radar_to_parent_transformation; - Eigen::Isometry3d crossval_calibrated_2d_radar_to_parent_transformation; + Eigen::Matrix4f crossval_radar_to_radar_parallel_transformation; + Eigen::Isometry3d crossval_calibrated_2d_radar_to_radar_parallel_transformation; Eigen::Isometry3d crossval_calibrated_2d_radar_to_lidar_transformation; for (int num_of_samples = 3; num_of_samples < tracks_size; num_of_samples++) { @@ -1422,13 +1458,16 @@ void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( } crossval_estimator.estimateRigidTransformation( *crossval_lidar_points_pcs, *crossval_radar_points_rcs, - crossval_radar_to_parent_transformation); - crossval_calibrated_2d_radar_to_parent_transformation = - crossval_radar_to_parent_transformation.cast(); - crossval_calibrated_2d_radar_to_parent_transformation.translation().z() = - (initial_radar_to_lidar_eigen_ * parent_to_lidar_eigen_.inverse()).translation().z(); + crossval_radar_to_radar_parallel_transformation); + crossval_calibrated_2d_radar_to_radar_parallel_transformation = + crossval_radar_to_radar_parallel_transformation.cast(); + crossval_calibrated_2d_radar_to_radar_parallel_transformation.translation().z() = + (initial_radar_to_lidar_eigen_ * radar_parallel_to_lidar_eigen_.inverse()) + .translation() + .z(); crossval_calibrated_2d_radar_to_lidar_transformation = - crossval_calibrated_2d_radar_to_parent_transformation * parent_to_lidar_eigen_; + crossval_calibrated_2d_radar_to_radar_parallel_transformation * + radar_parallel_to_lidar_eigen_; // calculate the error. auto [crossval_calibrated_2d_distance_error, crossval_calibrated_2d_yaw_error] = @@ -1490,6 +1529,7 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() output_metrics_.clear(); + // Note: pcs=parallel coordinate system rcs=radar coordinate system auto [lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum] = getPointsSetAndDelta(); estimateTransformation(lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum); crossValEvaluation(lidar_points_pcs, radar_points_rcs); @@ -1733,7 +1773,7 @@ void ExtrinsicReflectorBasedCalibrator::drawCalibrationStatusText() // show the latest cross validation results which is located in the last two elements of the // metrics vector show the latest calibration result, which is located in the 2nd and 3rd index of // the metrics vector - double m_to_cm = 100.0; + constexpr double m_to_cm = 100.0; if (converged_tracks_.size() == 0) { text_marker.text = " pairs=" + std::to_string(converged_tracks_.size()); @@ -1778,3 +1818,5 @@ double ExtrinsicReflectorBasedCalibrator::getYawError( { return std::abs(std::acos(v1.dot(v2) / (v1.norm() * v2.norm()))); } + +} // namespace marker_radar_lidar_calibrator diff --git a/sensor/extrinsic_reflector_based_calibrator/src/track.cpp b/sensor/marker_radar_lidar_calibrator/src/track.cpp similarity index 96% rename from sensor/extrinsic_reflector_based_calibrator/src/track.cpp rename to sensor/marker_radar_lidar_calibrator/src/track.cpp index c331f524..f2ce0a62 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/track.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/track.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -13,9 +13,12 @@ // limitations under the License. #include -#include -#include #include +#include +#include + +namespace marker_radar_lidar_calibrator +{ Track::Track( builtin_interfaces::msg::Time & t0, const KalmanFilter & initial_lidar_filter, @@ -151,3 +154,5 @@ Track TrackFactory::makeTrack( t0, lidar_filter, radar_filter, lidar_convergence_thresh_, radar_convergence_thresh_, timeout_thresh_, max_matching_distance_); } + +} // namespace marker_radar_lidar_calibrator diff --git a/sensor/sensor_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..4990e9da --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml new file mode 100644 index 00000000..4ffc7fe5 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/mapping_based_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/mapping_based_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..715249e0 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/mapping_based_base_lidar_calibrator.launch.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..91e08665 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..2393b0e7 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..97a99e41 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..06ceaae5 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml new file mode 100644 index 00000000..ad4e2cd4 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..f7426e64 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..18472445 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..26b57743 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..e4034e2e --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..0543dba6 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..bbc67b99 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml new file mode 100644 index 00000000..21dea311 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..5fdff88f --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..fbe556ba --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..161e7cc4 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..e7d7983c --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..4f0dff1a --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..10b47c0a --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..6a2707ad --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..a6651b9d --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml new file mode 100644 index 00000000..67637af6 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..2db1d8d9 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..fbe556ba --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..b0f539fe --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..5e63d9dd --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..715ea834 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..32e1848f --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..edf191ac --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..aaad0766 --- /dev/null +++ b/sensor/sensor_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/sensor_calibration_manager/package.xml b/sensor/sensor_calibration_manager/package.xml new file mode 100644 index 00000000..b9bceb7b --- /dev/null +++ b/sensor/sensor_calibration_manager/package.xml @@ -0,0 +1,18 @@ + + + + sensor_calibration_manager + 0.0.0 + the sensor_calibration_manager package + Kenzo Lobos Tsunekawa + Apache License 2.0 + + python3-pyside2.qtquick + python3-transforms3d + ros2launch + tier4_calibration_msgs + + + ament_python + + diff --git a/sensor/sensor_calibration_manager/resource/sensor_calibration_manager b/sensor/sensor_calibration_manager/resource/sensor_calibration_manager new file mode 100644 index 00000000..e69de29b diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibration_manager_model.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibration_manager_model.py new file mode 100644 index 00000000..2361d608 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibration_manager_model.py @@ -0,0 +1,72 @@ +#!/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. + +import logging +from typing import List + +from PySide2.QtCore import QAbstractTableModel +from PySide2.QtCore import Qt +from sensor_calibration_manager.calibrator_wrapper import CalibratorServiceWrapper + + +class CalibratorManagerModel(QAbstractTableModel): + column_names = ["Service name", "Parent", "Child", "Elapsed time", "Score", "Status"] + + def __init__(self, calibrator_service_wrapper_list: List[CalibratorServiceWrapper]): + super().__init__() + self.calibrator_service_wrapper_list = calibrator_service_wrapper_list + + self.index_to_calibrator_index = [] + self.index_to_frame_index = [] + + for calibrator_index, calibrator_wrapper in enumerate(self.calibrator_service_wrapper_list): + for frame_index in range(calibrator_wrapper.get_number_of_frames()): + self.index_to_calibrator_index.append(calibrator_index) + self.index_to_frame_index.append(frame_index) + + calibrator_wrapper.data_changed.connect(self.on_changed) + + def on_changed(self): + logging.debug("CalibratorManagerModel: on_changed") + self.dataChanged.emit( + self.createIndex(0, 0), self.createIndex(self.rowCount(0), self.columnCount(0)) + ) + + def data(self, index, role): + if role in [Qt.DisplayRole, Qt.ToolTipRole]: + value = self.calibrator_service_wrapper_list[ + self.index_to_calibrator_index[index.row()] + ].get_data(self.index_to_frame_index[index.row()])[index.column()] + return str(value) + + def rowCount(self, index): + return sum( + [ + calibrator.get_number_of_frames() + for calibrator in self.calibrator_service_wrapper_list + ] + ) + + def columnCount(self, index): + return len(self.column_names) + + def headerData(self, index, orientation, role): + # section is the index of the column/row. + if role == Qt.DisplayRole: + if orientation == Qt.Horizontal: + return str(self.column_names[index]) + + super().headerData(index, orientation, role) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py new file mode 100644 index 00000000..3a06a771 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py @@ -0,0 +1,227 @@ +#!/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 abc import ABCMeta +from abc import abstractproperty +from collections import defaultdict +import logging +from typing import Dict +from typing import List + +from PySide2.QtCore import QObject +from PySide2.QtCore import QTimer +from PySide2.QtCore import Signal +from geometry_msgs.msg import Transform +import numpy as np +from sensor_calibration_manager.calibrator_wrapper import CalibratorServiceWrapper +from sensor_calibration_manager.ros_interface import RosInterface +from sensor_calibration_manager.types import CalibratorState +from sensor_calibration_manager.types import FramePair +from sensor_calibration_manager.utils import tf_message_to_transform_matrix +from sensor_calibration_manager.utils import transform_matrix_to_tf_message +import transforms3d +import yaml + + +class CalibratorBase(QObject): + __metaclass__ = ABCMeta + + state_changed_signal = Signal(CalibratorState) + calibration_finished_signal = Signal() + + def __init__(self, ros_interface: RosInterface): + logging.debug("CalibratorBase: constructor start") + super().__init__() + self.ros_interface = ros_interface + self.calibrators: List[CalibratorServiceWrapper] = [] + self.expected_calibration_frames: List[FramePair] = [] + self.state = CalibratorState.WAITING_TFS + + self.calibration_result_tfs = defaultdict(lambda: defaultdict(Transform)) + self.calibration_result_transforms = defaultdict(lambda: defaultdict(Transform)) + + self.check_tf_timer = QTimer() + self.check_tf_timer.timeout.connect(self.on_check_tf_timer) + self.check_tf_timer.start(500) + logging.debug("CalibratorBase: constructor end") + + def init(): + logging.debug("CalibratorBase: Calibrator init?") + + def on_check_tf_timer(self): + logging.debug("CalibratorBase: on_check_tf_timer") + assert self.state == CalibratorState.WAITING_TFS + tfs_ready = all( + self.ros_interface.can_transform(self.required_frames[0], frame) + for frame in self.required_frames[1:] + ) + + if tfs_ready: + self.state = CalibratorState.WAITING_SERVICES + self.state_changed_signal.emit(self.state) + self.check_tf_timer.stop() + logging.debug("CalibratorBase: on_check_tf_timer stop") + else: + for frame in self.required_frames[1:]: + if not self.ros_interface.can_transform(self.required_frames[0], frame): + logging.debug(f"could not transform {self.required_frames[0]} to {frame}") + + def get_transform_matrix(self, parent: str, child: str) -> np.array: + if parent not in self.required_frames or child not in self.required_frames: + raise ValueError + tf_msg = self.ros_interface.get_transform(parent, child) + return tf_message_to_transform_matrix(tf_msg) + + def add_calibrator(self, service_name: str, expected_calibration_frames: List[FramePair]): + logging.debug("CalibratorBase: add_calibrator") + + for pair in expected_calibration_frames: + assert ( + pair not in self.expected_calibration_frames + ), f"The pair {pair} was already registered by a previous calibrator" + self.expected_calibration_frames.append(pair) + + calibration_wrapper = CalibratorServiceWrapper( + self.ros_interface, service_name, expected_calibration_frames + ) + calibration_wrapper.status_changed_signal.connect(self.on_service_status_changed) + calibration_wrapper.result_signal.connect(self.on_calibration_result) + self.calibrators.append(calibration_wrapper) + + def on_service_status_changed(self): + if self.state in [CalibratorState.WAITING_SERVICES, CalibratorState.READY]: + services_available = all([calibrator.is_available() for calibrator in self.calibrators]) + + if services_available and self.state == CalibratorState.WAITING_SERVICES: + self.state = CalibratorState.READY + self.state_changed_signal.emit(self.state) + elif not services_available and self.state == CalibratorState.READY: + self.state = CalibratorState.WAITING_SERVICES + self.state_changed_signal.emit(self.state) + + def on_calibration_result(self): + logging.debug("CalibratorBase: on_calibration_result") + + for calibrator in self.calibrators: + d = calibrator.get_calibration_results() + + for parent_frame, d2 in d.items(): + for child_frame, transform in d2.items(): + self.calibration_result_tfs[parent_frame][child_frame] = transform + + if not calibrator.finished(): + return + + self.post_process_internal() + self.state = CalibratorState.FINISHED + self.state_changed_signal.emit(self.state) + self.calibration_finished_signal.emit() + + def get_service_wrappers(self) -> List[CalibratorServiceWrapper]: + return self.calibrators + + def get_calibration_results(self) -> Dict[str, Dict[str, Transform]]: + return self.calibration_result_tfs + + def get_processed_calibration_results(self) -> Dict[str, Dict[str, Transform]]: + return self.processed_calibration_result_tfs + + def start_calibration(self): + assert self.state == CalibratorState.READY + + self.pre_process() + + for calibrator in self.calibrators: + calibrator.start() + + self.state = CalibratorState.CALIBRATING + self.state_changed_signal.emit(self.state) + + @abstractproperty + def required_frames(self) -> List[str]: + raise NotImplementedError + + def pre_process(self): + pass + + def post_process_internal(self): + logging.debug("Before post_process") + for parent, children_and_transforms in self.calibration_result_tfs.items(): + for child, transform in children_and_transforms.items(): + logging.debug(f"{parent}->{child}:\n{transform}") + + calibration_transforms = { + parent: { + child: tf_message_to_transform_matrix(transform) + for child, transform in children_and_transforms.items() + } + for parent, children_and_transforms in self.calibration_result_tfs.items() + } + calibration_transforms = self.post_process(calibration_transforms) + self.processed_calibration_result_tfs = { + parent: { + child: transform_matrix_to_tf_message(transform) + for child, transform in children_and_transforms.items() + } + for parent, children_and_transforms in calibration_transforms.items() + } + + logging.debug("After post_process") + for parent, children_and_transforms in self.processed_calibration_result_tfs.items(): + for child, transform in children_and_transforms.items(): + logging.debug(f"{parent}->{child}:\n{transform}") + + def post_process(self, calibration_transforms) -> Dict[str, Dict[str, np.array]]: + return calibration_transforms + + def save_results(self, path, use_rpy=True): + output_dict = {} + + for parent, children_and_tfs_dict in self.get_processed_calibration_results().items(): + output_dict[parent] = {} + + for child, tf_msg in children_and_tfs_dict.items(): + translation = tf_msg.translation + quat = tf_msg.rotation + + d = {} + d["x"] = translation.x.item() + d["y"] = translation.y.item() + d["z"] = translation.z.item() + + if use_rpy: + roll, pitch, yaw = transforms3d.euler.quat2euler( + [quat.w, quat.x, quat.y, quat.z] + ) + d["roll"] = roll + d["pitch"] = pitch + d["yaw"] = yaw + else: + d["qx"] = quat.x.item() + d["qy"] = quat.y.item() + d["qz"] = quat.z.item() + d["qw"] = quat.w.item() + + output_dict[parent][child] = d + + def float_representer(dumper, value): + text = "{0:.6f}".format(value) + return dumper.represent_scalar("tag:yaml.org,2002:float", text) + + yaml.add_representer(float, float_representer) + + with open(path, "w") as f: + yaml.dump(output_dict, f, sort_keys=False) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py new file mode 100644 index 00000000..d076f241 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py @@ -0,0 +1,91 @@ +#!/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 collections import defaultdict +import logging +from typing import Callable +from typing import List + +from sensor_calibration_manager.calibrator_base import CalibratorBase + + +class CalibratorRegistry: + """The factory class to register and execute calibrators.""" + + logger = logging.getLogger(__name__) + registry = defaultdict(lambda: defaultdict(CalibratorBase)) + """ Internal registry for available calibrators """ + + @classmethod + def getProjects(cls) -> List: + return list(cls.registry.keys()) + + @classmethod + def getProjectCalibrators(cls, project_name) -> List: + return list(cls.registry[project_name].keys()) + + @classmethod + def register_calibrator(cls, project_name: str, calibrator_name: str) -> Callable: + """ + Class method to register implementations of the CalibratorBase class into the internal registry. + + Args: + project_name (str): The name of the calibration project. + calibrator_name (str): The name of the calibrator. + + Returns + ------- + wrapper + + """ + + def inner_wrapper(wrapped_class: CalibratorBase) -> CalibratorBase: + cls.logger.info(f"Adding {wrapped_class.__name__}") + if project_name in cls.registry and calibrator_name in cls.registry[project_name]: + cls.logger.warning( + f"Calibrator project={project_name} name={calibrator_name} already exist. Overwriting by {type(wrapped_class).__name__}" + ) + cls.registry[project_name][calibrator_name] = wrapped_class + return wrapped_class + + return inner_wrapper + + @classmethod + def create_calibrator(cls, project_name: str, calibrator_name: str, **kwargs) -> CalibratorBase: + """ + Create the executor using a factory pattern. + + This method gets the appropriate Executor class from the registry + and creates an instance of it, while passing in the parameters + given in ``kwargs``. + + Args: + name (str): The name of the executor to create. + + Returns + ------- + An instance of the executor that is created. + + """ + if project_name not in cls.registry or calibrator_name not in cls.registry[project_name]: + cls.logger.error( + f"Calibrator project={project_name} name={calibrator_name} does not exist" + ) + return None + + exec_class = cls.registry[project_name][calibrator_name] + executor = exec_class(**kwargs) + return executor diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py new file mode 100644 index 00000000..8696e030 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py @@ -0,0 +1,189 @@ +#!/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 collections import defaultdict +import logging +import threading +import time +from typing import Dict +from typing import List + +from PySide2.QtCore import QObject +from PySide2.QtCore import QTimer +from PySide2.QtCore import Signal +from geometry_msgs.msg import Transform +from sensor_calibration_manager.ros_interface import RosInterface +from sensor_calibration_manager.types import FramePair +from tier4_calibration_msgs.msg import CalibrationResult +from tier4_calibration_msgs.srv import ExtrinsicCalibrator + + +class CalibratorServiceWrapper(QObject): + data_changed = Signal() + status_signal = Signal(bool) + status_changed_signal = Signal() + result_signal = Signal(list) + + def __init__( + self, + ros_interface: RosInterface, + service_name: str, + expected_calibration_frames: List[FramePair], + ): + super().__init__() + + self.ros_interface = ros_interface + self.service_name = service_name + self.expected_calibration_frames = expected_calibration_frames + + self.calibration_results = defaultdict(lambda: defaultdict(Transform)) + self.service_status = False + self.service_called = False + + self.children_to_id = {} + self.parents = [] + self.children = [] + self.scores = [] + self.status_messages = [] + self.finished_list = [] + self.elapsed_times = [] + self.start_times = [] + + for i, (parent_frame, child_frame) in enumerate(expected_calibration_frames): + self.children_to_id[child_frame] = i + self.parents.append(parent_frame) + self.children.append(child_frame) + self.scores.append("") + self.status_messages.append("") + self.finished_list.append(False) + self.scores.append("") + self.elapsed_times.append("") + self.start_times.append(0) + + ros_interface.register_calibration_service( + self.service_name, self.result_ros_callback, self.status_ros_callback + ) + self.result_signal.connect(self.on_result) + self.status_signal.connect(self.on_status) + + self.timer = QTimer() + self.timer.timeout.connect(self.on_timer) + + # Create the services with a lock on the interface + # register a callback and use the signal trick to execute in the UI thread + # use partials + + # register the status checkers + + def start(self): + self.ros_interface.call_calibration_service(self.service_name) + + t0 = time.time() + self.start_times = [t0 for _ in range(len(self.parents))] + self.status_messages = [ + "Service called. Waiting for calibration to end" for _ in range(len(self.parents)) + ] + self.service_called = True + + self.data_changed.emit() + self.timer.start(100) + + def on_timer(self): + tf = time.time() + + for i in range(len(self.elapsed_times)): + self.elapsed_times[i] = f"{tf - self.start_times[i]:.2f}" + + self.data_changed.emit() + + def on_result(self, calibration_results: List[CalibrationResult]): + received_calibration_ids = [] + tf = time.time() + + for calibration_result in calibration_results: + parent_frame = calibration_result.transform_stamped.header.frame_id + child_frame = calibration_result.transform_stamped.child_frame_id + + if parent_frame not in self.parents or child_frame not in self.children: + logging.error( + f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers" + ) + continue + + i = self.children_to_id[child_frame] + + if self.parents[i] != parent_frame: + logging.error( + f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers" + ) + continue + + self.calibration_results[parent_frame][ + child_frame + ] = calibration_result.transform_stamped.transform + + self.scores[i] = calibration_result.score + self.status_messages[i] = calibration_result.message.data + self.finished_list[i] = True + self.elapsed_times[i] = f"{tf - self.start_times[i]:.2f}" + received_calibration_ids.append(i) + + for i in range(len(self.parents)): + if i not in received_calibration_ids: + self.status_messages[i] = "Error. tf not included in the result" + + self.timer.stop() + self.data_changed.emit() + + def on_status(self, status: bool): + if self.service_status != status: + self.service_status = status + self.data_changed.emit() + self.status_changed_signal.emit() + + def is_available(self) -> bool: + return self.service_status + + def finished(self): + return all(self.finished_list) + + def result_ros_callback(self, result: ExtrinsicCalibrator.Response): + logging.debug(f"{threading.get_ident() }: result_ros_callback") + self.result_signal.emit(result.results) + + def status_ros_callback(self, status: bool): + self.status_signal.emit(status) + + def get_data(self, index) -> list: + if not self.service_called: + status_message = "Service ready" if self.service_status else "Service not available" + else: + status_message = self.status_messages[index] + + return [ + self.service_name, + self.parents[index], + self.children[index], + self.elapsed_times[index], + self.scores[index], + status_message, + ] + + def get_number_of_frames(self) -> int: + return len(self.parents) + + def get_calibration_results(self) -> Dict[str, Dict[str, Transform]]: + return self.calibration_results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py new file mode 100644 index 00000000..0d7af762 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py @@ -0,0 +1,6 @@ +from .default_project import * # noqa: F401, F403 +from .rdv import * # noqa: F401, F403 +from .x1 import * # noqa: F401, F403 +from .x2 import * # noqa: F401, F403 +from .xx1 import * # noqa: F401, F403 +from .xx1_15 import * # noqa: F401, F403 diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py new file mode 100644 index 00000000..530b5460 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py @@ -0,0 +1,21 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator +from .lidar_lidar_2d_calibrator import LidarLidar2DCalibrator +from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator +from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator +from .marker_radar_lidar_calibrator import MarkerRadarLidarCalibrator +from .tag_based_pnp_calibrator import TagBasedPNPCalibrator +from .tag_based_sfm_base_lidar_calibrator import TagBasedSfmBaseLidarCalibrator +from .tag_based_sfm_base_lidars_calibrator import TagBasedSfmBaseLidarsCalibrator +from .tag_based_sfm_base_lidars_cameras_calibrator import TagBasedSfmBaseLidarsCamerasCalibrator + +__all__ = [ + "GroundPlaneCalibrator", + "LidarLidar2DCalibrator", + "MappingBasedBaseLidarCalibrator", + "MappingBasedLidarLidarCalibrator", + "MarkerRadarLidarCalibrator", + "TagBasedPNPCalibrator", + "TagBasedSfmBaseLidarCalibrator", + "TagBasedSfmBaseLidarsCalibrator", + "TagBasedSfmBaseLidarsCamerasCalibrator", +] diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/ground_plane_calibrator.py new file mode 100644 index 00000000..851935f6 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/ground_plane_calibrator.py @@ -0,0 +1,43 @@ +#!/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 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="ground_plane_calibrator" +) +class GroundPlaneCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame: str = kwargs["base_frame"] + self.lidar_frame: str = kwargs["lidar_frame"] + + self.required_frames.extend([self.base_frame, self.lidar_frame]) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.base_frame, child=self.lidar_frame), + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py new file mode 100644 index 00000000..ce82c261 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py @@ -0,0 +1,43 @@ +#!/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 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="lidar_lidar_2d_calibrator" +) +class LidarLidar2DCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame: str = kwargs["base_frame"] + self.source_frame: str = kwargs["source_frame"] + self.target_frame: str = kwargs["target_frame"] + + self.required_frames.extend([self.base_frame, self.source_frame, self.target_frame]) + + self.add_calibrator( + service_name="calibrate_lidar_lidar", + expected_calibration_frames=[ + FramePair(parent=self.target_frame, child=self.source_frame), + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_base_lidar_calibrator.py new file mode 100644 index 00000000..99501c23 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_base_lidar_calibrator.py @@ -0,0 +1,42 @@ +#!/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 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="mapping_based_base_lidar_calibrator" +) +class MappingBasedBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.mapping_lidar_frame = kwargs["mapping_lidar_frame"] + + self.required_frames.extend([self.base_frame, self.mapping_lidar_frame]) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.mapping_lidar_frame, child=self.base_frame) + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..68570624 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,44 @@ +#!/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 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="mapping_based_lidar_lidar_calibrator" +) +class MappingBasedLidarLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.mapping_lidar_frame = kwargs["mapping_lidar_frame"] + self.calibration_lidar_frames = kwargs["calibration_lidar_frames"] + + self.required_frames.extend([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 + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py new file mode 100644 index 00000000..c0165fe6 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_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="marker_radar_lidar_calibrator" +) +class MarkerRadarLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.radar_parallel_frame = kwargs["radar_parallel_frame"] + self.radar_frame = kwargs["radar_frame"] + self.lidar_frame = kwargs["lidar_frame"] + + self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) + + self.add_calibrator( + service_name="calibrate_radar_lidar", + expected_calibration_frames=[ + FramePair(parent=self.radar_frame, child=self.lidar_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + lidar_to_radar_parallel_transform = self.get_transform_matrix( + self.lidar_frame, self.radar_parallel_frame + ) + + radar_parallel_to_radar_transform = np.linalg.inv( + calibration_transforms[self.radar_frame][self.lidar_frame] + @ lidar_to_radar_parallel_transform + ) + + results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..753da0c5 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py @@ -0,0 +1,46 @@ +#!/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 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 + +# import numpy as np + + +@CalibratorRegistry.register_calibrator( + project_name="default_project", calibrator_name="tag_based_pnp_calibrator" +) +class TagBasedPNPCalibrator(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), + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..1311d8db --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py @@ -0,0 +1,42 @@ +#!/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 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="tag_based_sfm_base_lidar_calibrator" +) +class TagBasedSfmBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + + self.required_frames.extend([self.base_frame, self.main_sensor_frame]) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame) + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py new file mode 100644 index 00000000..70b78bcd --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py @@ -0,0 +1,58 @@ +#!/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 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="tag_based_sfm_base_lidars_calibrator" +) +class TagBasedSfmBaseLidarsCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + self.calibration_lidar_frames = [ + kwargs["calibration_lidar_1_frame"], + kwargs["calibration_lidar_2_frame"], + kwargs["calibration_lidar_3_frame"], + ] + + self.required_frames.extend( + [ + self.base_frame, + self.main_sensor_frame, + *self.calibration_lidar_frames, + ] + ) + + self.add_calibrator( + service_name="calibrate_base_lidars", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame), + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_lidar_frames + ], + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..51614df7 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,73 @@ +#!/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 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="tag_based_sfm_base_lidars_cameras_calibrator" +) +class TagBasedSfmBaseLidarsCamerasCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + self.calibration_lidar_frames = [ + kwargs["calibration_lidar_1_frame"], + kwargs["calibration_lidar_2_frame"], + kwargs["calibration_lidar_3_frame"], + ] + + self.calibration_camera_optical_link_frames = [ + kwargs["calibration_camera_0_frame"], + kwargs["calibration_camera_1_frame"], + kwargs["calibration_camera_2_frame"], + kwargs["calibration_camera_3_frame"], + kwargs["calibration_camera_4_frame"], + kwargs["calibration_camera_5_frame"], + kwargs["calibration_camera_6_frame"], + ] + + self.required_frames.extend( + [ + self.base_frame, + self.main_sensor_frame, + *self.calibration_lidar_frames, + *self.calibration_camera_optical_link_frames, + ] + ) + + self.add_calibrator( + service_name="calibrate_base_lidars_cameras", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame), + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_lidar_frames + ], + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_camera_optical_link_frames + ], + ], + ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/__init__.py new file mode 100644 index 00000000..3e4f24c5 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/__init__.py @@ -0,0 +1,17 @@ +from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator +from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator +from .marker_radar_lidar_calibrator import MarkerRadarLidarCalibrator +from .tag_based_pnp_calibrator import TagBasedPNPCalibrator +from .tag_based_sfm_base_lidar_calibrator import TagBasedSfmBaseLidarCalibrator +from .tag_based_sfm_base_lidars_calibrator import TagBasedSfmBaseLidarsCalibrator +from .tag_based_sfm_base_lidars_cameras_calibrator import TagBasedSfmBaseLidarsCamerasCalibrator + +__all__ = [ + "MappingBasedBaseLidarCalibrator", + "MappingBasedLidarLidarCalibrator", + "MarkerRadarLidarCalibrator", + "TagBasedPNPCalibrator", + "TagBasedSfmBaseLidarCalibrator", + "TagBasedSfmBaseLidarsCalibrator", + "TagBasedSfmBaseLidarsCamerasCalibrator", +] diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py new file mode 100644 index 00000000..efcd1904 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py @@ -0,0 +1,62 @@ +#!/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="rdv", calibrator_name="mapping_based_base_lidar_calibrator" +) +class MappingBasedBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = "base_link" + self.sensor_kit_frame = "sensor_kit_base_link" + + self.mapping_lidar_frame = "pandar_top" + + self.required_frames.extend( + [self.base_frame, self.sensor_kit_frame, self.mapping_lidar_frame] + ) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.mapping_lidar_frame, child=self.base_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.mapping_lidar_frame + ) + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame][self.base_frame] + ) + results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..dde171b3 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,91 @@ +#!/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="rdv", 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 diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py new file mode 100644 index 00000000..45fbb11f --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_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="rdv", calibrator_name="marker_radar_lidar_calibrator" +) +class MarkerRadarLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.radar_parallel_frame = kwargs["radar_parallel_frame"] + self.radar_frame = kwargs["radar_frame"] + self.lidar_frame = kwargs["lidar_frame"] + + self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) + + self.add_calibrator( + service_name="calibrate_radar_lidar", + expected_calibration_frames=[ + FramePair(parent=self.radar_frame, child=self.lidar_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + lidar_to_radar_parallel_transform = self.get_transform_matrix( + self.lidar_frame, self.radar_parallel_frame + ) + + radar_parallel_to_radar_transform = np.linalg.inv( + calibration_transforms[self.radar_frame][self.lidar_frame] + @ lidar_to_radar_parallel_transform + ) + + results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..18a8952b --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_pnp_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="rdv", calibrator_name="tag_based_pnp_calibrator" +) +class TagBasedPNPCalibrator(CalibratorBase): + required_frames = ["sensor_kit_base_link", "pandar_top_base_link", "pandar_top"] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.camera_name = kwargs["camera_name"] + 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="pandar_top"), + ], + ) + + 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" + ]["pandar_top"] + + sensor_kit_to_lidar_transform = self.get_transform_matrix( + "sensor_kit_base_link", "pandar_top" + ) + + camera_to_optical_link_transform = self.get_transform_matrix( + f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link" + ) + + sensor_kit_camera_link_transform = np.linalg.inv( + camera_to_optical_link_transform + @ optical_link_to_lidar_transform + @ np.linalg.inv(sensor_kit_to_lidar_transform) + ) + + result = { + "sensor_kit_base_link": { + f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform + } + } + return result diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..c5832354 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py @@ -0,0 +1,62 @@ +#!/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="rdv", calibrator_name="tag_based_sfm_base_lidar_calibrator" +) +class TagBasedSfmBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.sensor_kit_frame = "sensor_kit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + + self.required_frames.extend( + [self.base_frame, self.sensor_kit_frame, self.main_sensor_frame] + ) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.main_sensor_frame + ) + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][self.base_frame] + ) + results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py new file mode 100644 index 00000000..7fab90aa --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py @@ -0,0 +1,97 @@ +#!/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="rdv", calibrator_name="tag_based_sfm_base_lidars_calibrator" +) +class TagBasedSfmBaseLidarsCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.sensor_kit_frame = "sensor_kit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + self.calibration_lidar_frames = [ + kwargs["calibration_lidar_1_frame"], + kwargs["calibration_lidar_2_frame"], + kwargs["calibration_lidar_3_frame"], + ] + self.calibration_lidar_base_frames = [ + lidar_frame + "_base_link" for lidar_frame in self.calibration_lidar_frames + ] + + self.required_frames.extend( + [ + self.base_frame, + self.sensor_kit_frame, + self.main_sensor_frame, + *self.calibration_lidar_frames, + *self.calibration_lidar_base_frames, + ] + ) + + self.add_calibrator( + service_name="calibrate_base_lidars", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame), + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_lidar_frames + ], + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.main_sensor_frame + ) + + lidar_to_lidar_base_transforms = [ + self.get_transform_matrix(lidar_frame, lidar_base_frame) + for lidar_frame, lidar_base_frame in zip( + self.calibration_lidar_frames, self.calibration_lidar_base_frames + ) + ] + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][self.base_frame] + ) + results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}} + results[self.sensor_kit_frame] = {} + + for lidar_frame, lidar_to_lidar_base_transform in zip( + self.calibration_lidar_frames, lidar_to_lidar_base_transforms + ): + results[self.sensor_kit_frame][lidar_frame + "_base_link"] = ( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][lidar_frame] + @ lidar_to_lidar_base_transform + ) + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..42b6e3a5 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,135 @@ +#!/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="rdv", calibrator_name="tag_based_sfm_base_lidars_cameras_calibrator" +) +class TagBasedSfmBaseLidarsCamerasCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.sensor_kit_frame = "sensor_kit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + self.calibration_lidar_frames = [ + kwargs["calibration_lidar_1_frame"], + kwargs["calibration_lidar_2_frame"], + kwargs["calibration_lidar_3_frame"], + ] + self.calibration_lidar_base_frames = [ + lidar_frame + "_base_link" for lidar_frame in self.calibration_lidar_frames + ] + + self.calibration_camera_optical_link_frames = [ + kwargs["calibration_camera_0_frame"], + kwargs["calibration_camera_1_frame"], + kwargs["calibration_camera_2_frame"], + kwargs["calibration_camera_3_frame"], + kwargs["calibration_camera_4_frame"], + kwargs["calibration_camera_5_frame"], + kwargs["calibration_camera_6_frame"], + ] + self.calibration_camera_link_frames = [ + camera_frame.replace("camera_optical_link", "camera_link") + for camera_frame in self.calibration_camera_optical_link_frames + ] + + self.required_frames.extend( + [ + self.base_frame, + self.sensor_kit_frame, + self.main_sensor_frame, + *self.calibration_lidar_frames, + *self.calibration_lidar_base_frames, + *self.calibration_camera_optical_link_frames, + *self.calibration_camera_link_frames, + ] + ) + + self.add_calibrator( + service_name="calibrate_base_lidars_cameras", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame), + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_lidar_frames + ], + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_camera_optical_link_frames + ], + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.main_sensor_frame + ) + + lidar_to_lidar_base_transforms = [ + self.get_transform_matrix(lidar_frame, lidar_base_frame) + for lidar_frame, lidar_base_frame in zip( + self.calibration_lidar_frames, self.calibration_lidar_base_frames + ) + ] + + optical_link_to_camera_link_transforms = [ + self.get_transform_matrix(camera_optical_link_frame, camera_link_frame) + for camera_optical_link_frame, camera_link_frame in zip( + self.calibration_camera_optical_link_frames, self.calibration_camera_link_frames + ) + ] + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][self.base_frame] + ) + results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}} + results[self.sensor_kit_frame] = {} + + for lidar_frame, lidar_to_lidar_base_transform in zip( + self.calibration_lidar_frames, lidar_to_lidar_base_transforms + ): + results[self.sensor_kit_frame][lidar_frame + "_base_link"] = ( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][lidar_frame] + @ lidar_to_lidar_base_transform + ) + + for camera_frame, optical_link_to_camera_link_transform in zip( + self.calibration_camera_optical_link_frames, optical_link_to_camera_link_transforms + ): + results[self.sensor_kit_frame][ + camera_frame.replace("camera_optical_link", "camera_link") + ] = ( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][camera_frame] + @ optical_link_to_camera_link_transform + ) + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/__init__.py new file mode 100644 index 00000000..0db7adc9 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/__init__.py @@ -0,0 +1,3 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator + +__all__ = ["GroundPlaneCalibrator"] diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/ground_plane_calibrator.py new file mode 100644 index 00000000..b25e3af6 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/ground_plane_calibrator.py @@ -0,0 +1,61 @@ +#!/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="x1", calibrator_name="ground_plane_calibrator" +) +class GroundPlaneCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = "base_link" + self.sensor_kit_frame = "sensor_kit_base_link" + self.lidar_frame = "velodyne_top" + + self.required_frames.extend([self.base_frame, self.sensor_kit_frame, self.lidar_frame]) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.base_frame, child=self.lidar_frame), + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + base_to_lidar_transform = calibration_transforms[self.base_frame][self.lidar_frame] + + sensor_kit_to_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.lidar_frame + ) + + base_to_sensor_kit_transform = base_to_lidar_transform @ np.linalg.inv( + sensor_kit_to_lidar_transform + ) + + result = {self.base_frame: {self.sensor_kit_frame: base_to_sensor_kit_transform}} + + return result diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/__init__.py new file mode 100644 index 00000000..783eb337 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/__init__.py @@ -0,0 +1,19 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator +from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator +from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator +from .marker_radar_lidar_calibrator import MarkerRadarLidarCalibrator +from .tag_based_pnp_calibrator import TagBasedPNPCalibrator +from .tag_based_sfm_base_lidar_calibrator import TagBasedSfmBaseLidarCalibrator +from .tag_based_sfm_base_lidars_calibrator import TagBasedSfmBaseLidarsCalibrator +from .tag_based_sfm_base_lidars_cameras_calibrator import TagBasedSfmBaseLidarsCamerasCalibrator + +__all__ = [ + "GroundPlaneCalibrator", + "MappingBasedBaseLidarCalibrator", + "MappingBasedLidarLidarCalibrator", + "MarkerRadarLidarCalibrator", + "TagBasedPNPCalibrator", + "TagBasedSfmBaseLidarCalibrator", + "TagBasedSfmBaseLidarsCalibrator", + "TagBasedSfmBaseLidarsCamerasCalibrator", +] diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/ground_plane_calibrator.py new file mode 100644 index 00000000..f2b6fd8e --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/ground_plane_calibrator.py @@ -0,0 +1,61 @@ +#!/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="x2", calibrator_name="ground_plane_calibrator" +) +class GroundPlaneCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = "base_link" + self.sensor_kit_frame = "top_unit_base_link" + self.lidar_frame = "pandar_40p_left" + + self.required_frames.extend([self.base_frame, self.sensor_kit_frame, self.lidar_frame]) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.base_frame, child=self.lidar_frame), + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + base_to_lidar_transform = calibration_transforms[self.base_frame][self.lidar_frame] + + sensor_kit_to_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.lidar_frame + ) + + base_to_sensor_kit_transform = base_to_lidar_transform @ np.linalg.inv( + sensor_kit_to_lidar_transform + ) + + result = {self.base_frame: {self.sensor_kit_frame: base_to_sensor_kit_transform}} + + return result diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py new file mode 100644 index 00000000..123bb166 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py @@ -0,0 +1,62 @@ +#!/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="x2", calibrator_name="mapping_based_base_lidar_calibrator" +) +class MappingBasedBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = "base_link" + self.top_sensor_kit_frame = "top_unit_base_link" + + self.mapping_lidar_frame = "pandar_40p_left" + + self.required_frames.extend( + [self.base_frame, self.top_sensor_kit_frame, self.mapping_lidar_frame] + ) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.mapping_lidar_frame, child=self.base_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + top_sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.top_sensor_kit_frame, self.mapping_lidar_frame + ) + + base_to_top_sensor_kit_transform = np.linalg.inv( + top_sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame][self.base_frame] + ) + results = {self.base_frame: {self.top_sensor_kit_frame: base_to_top_sensor_kit_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..55222bd3 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,153 @@ +#!/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 collections import defaultdict +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="x2", 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_frame = "base_link" + self.top_sensor_kit_frame = "top_unit_base_link" + self.front_sensor_kit_frame = "front_unit_base_link" + self.rear_sensor_kit_frame = "rear_unit_base_link" + + self.mapping_lidar_frame = "pandar_40p_left" + self.calibration_lidar_frames = [ + "pandar_qt_left", + "pandar_40p_right", + "pandar_qt_right", + "pandar_40p_front", + "pandar_qt_front", + "pandar_40p_rear", + "pandar_qt_rear", + ] + + self.calibration_base_lidar_frames = [ + "pandar_qt_left", + "pandar_40p_right", + "pandar_qt_right", + "pandar_40p_front", + "pandar_qt_front", + "pandar_40p_rear", + "pandar_qt_rear", + ] + + self.required_frames.extend( + [ + self.base_frame, + self.top_sensor_kit_frame, + self.front_sensor_kit_frame, + self.rear_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]]): + top_sensor_kit_to_main_lidar_transform = self.get_transform_matrix( + self.top_sensor_kit_frame, self.mapping_lidar_frame + ) + + front_sensor_kit_to_main_lidar_transform = self.get_transform_matrix( + self.front_sensor_kit_frame, "pandar_40p_front" + ) + + rear_sensor_kit_to_main_lidar_transform = self.get_transform_matrix( + self.rear_sensor_kit_frame, "pandar_40p_rear" + ) + + base_to_mapping_lidar_transform = self.get_transform_matrix( + self.base_frame, self.mapping_lidar_frame + ) + + calibration_lidar_to_base_lidar_transforms = { + calibration_lidar_frame: 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 + ) + } + + results = defaultdict(lambda: defaultdict(np.array)) + + # Top unit lidars + results[self.top_sensor_kit_frame]["pandar_qt_left_base_link"] = ( + top_sensor_kit_to_main_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame]["pandar_qt_left"] + @ calibration_lidar_to_base_lidar_transforms["pandar_qt_left"] + ) + results[self.top_sensor_kit_frame]["pandar_40p_right_base_link"] = ( + top_sensor_kit_to_main_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame]["pandar_40p_right"] + @ calibration_lidar_to_base_lidar_transforms["pandar_40p_right"] + ) + results[self.top_sensor_kit_frame]["pandar_qt_right_base_link"] = ( + top_sensor_kit_to_main_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame]["pandar_qt_right"] + @ calibration_lidar_to_base_lidar_transforms["pandar_qt_right"] + ) + + # Front unit lidars + results[self.base_frame][self.front_sensor_kit_frame] = ( + base_to_mapping_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame]["pandar_40p_front"] + @ np.linalg.inv(front_sensor_kit_to_main_lidar_transform) + ) + results[self.front_sensor_kit_frame]["pandar_qt_front_base_link"] = ( + np.linalg.inv(results[self.base_frame][self.front_sensor_kit_frame]) + @ base_to_mapping_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame]["pandar_qt_front"] + @ calibration_lidar_to_base_lidar_transforms["pandar_qt_front"] + ) + + # Rear unit lidars + results[self.base_frame][self.rear_sensor_kit_frame] = ( + base_to_mapping_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame]["pandar_40p_rear"] + @ np.linalg.inv(rear_sensor_kit_to_main_lidar_transform) + ) + results[self.rear_sensor_kit_frame]["pandar_qt_rear_base_link"] = ( + np.linalg.inv(results[self.base_frame][self.rear_sensor_kit_frame]) + @ base_to_mapping_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame]["pandar_qt_rear"] + @ calibration_lidar_to_base_lidar_transforms["pandar_qt_rear"] + ) + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py new file mode 100644 index 00000000..96188498 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_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="x2", calibrator_name="marker_radar_lidar_calibrator" +) +class MarkerRadarLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.radar_parallel_frame = kwargs["radar_parallel_frame"] + self.radar_frame = kwargs["radar_frame"] + self.lidar_frame = kwargs["lidar_frame"] + + self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) + + self.add_calibrator( + service_name="calibrate_radar_lidar", + expected_calibration_frames=[ + FramePair(parent=self.radar_frame, child=self.lidar_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + lidar_to_radar_parallel_transform = self.get_transform_matrix( + self.lidar_frame, self.radar_parallel_frame + ) + + radar_parallel_to_radar_transform = np.linalg.inv( + calibration_transforms[self.radar_frame][self.lidar_frame] + @ lidar_to_radar_parallel_transform + ) + + results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..e0857724 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py @@ -0,0 +1,91 @@ +#!/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="x2", 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"] + + camera_name_to_sensor_kit_frame = { + "camera0": "top_unit_base_link", + "camera1": "top_unit_base_link", + "camera2": "top_unit_base_link", + "camera3": "rear_unit_base_link", + "camera4": "top_unit_base_link", + "camera5": "top_unit_base_link", + "camera6": "front_unit_base_link", + "camera7": "top_unit_base_link", + } + + self.sensor_kit_frame = camera_name_to_sensor_kit_frame[self.camera_name] + + self.required_frames.extend( + [ + self.lidar_frame, + self.sensor_kit_frame, + f"{self.camera_name}/camera_link", + 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] + + sensor_kit_to_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, 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" + ) + + sensor_kit_camera_link_transform = np.linalg.inv( + camera_to_optical_link_transform + @ optical_link_to_lidar_transform + @ np.linalg.inv(sensor_kit_to_lidar_transform) + ) + + result = { + self.sensor_kit_frame: { + f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform + } + } + return result diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..11536eff --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_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="x2", calibrator_name="tag_based_sfm_base_lidar_calibrator" +) +class TagBasedSfmBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.top_unit_frame = "top_unit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + + self.required_frames.extend([self.base_frame, self.top_unit_frame, self.main_sensor_frame]) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.top_unit_frame, self.main_sensor_frame + ) + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][self.base_frame] + ) + results = {self.base_frame: {self.top_unit_frame: base_to_top_sensor_kit_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py new file mode 100644 index 00000000..e54b3cec --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py @@ -0,0 +1,110 @@ +#!/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="x2", calibrator_name="tag_based_sfm_base_lidars_calibrator" +) +class TagBasedSfmBaseLidarsCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.top_unit_frame = "top_unit_base_link" + self.front_unit_frame = "front_unit_base_link" + self.rear_unit_frame = "rear_unit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + self.calibration_lidar_frames = [ + kwargs["calibration_lidar_1_frame"], + kwargs["calibration_lidar_2_frame"], + kwargs["calibration_lidar_3_frame"], + ] + self.calibration_lidar_base_frames = [ + lidar_frame + "_base_link" for lidar_frame in self.calibration_lidar_frames + ] + + self.required_frames.extend( + [ + self.base_frame, + self.top_unit_frame, + self.front_unit_frame, + self.rear_unit_frame, + self.main_sensor_frame, + *self.calibration_lidar_frames, + *self.calibration_lidar_base_frames, + ] + ) + + self.add_calibrator( + service_name="calibrate_base_lidars", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame), + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_lidar_frames + ], + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + main_sensor_to_base_transform = calibration_transforms[self.main_sensor_frame][ + self.base_frame + ] + + top_kit_to_main_lidar_transform = self.get_transform_matrix( + self.top_unit_frame, self.main_sensor_frame + ) + + front_kit_to_front_lower_lidar_transform = self.get_transform_matrix( + self.front_unit_frame, "pandar_40p_front" + ) + + rear_kit_to_rear_lower_lidar_transform = self.get_transform_matrix( + self.rear_unit_frame, "pandar_40p_rear" + ) + + base_to_top_kit_transform = np.linalg.inv( + top_kit_to_main_lidar_transform @ main_sensor_to_base_transform + ) + + base_to_front_kit_transform = ( + np.linalg.inv(main_sensor_to_base_transform) + @ calibration_transforms[self.main_sensor_frame]["pandar_40p_front"] + @ np.linalg.inv(front_kit_to_front_lower_lidar_transform) + ) + base_to_rear_kit_transform = ( + np.linalg.inv(main_sensor_to_base_transform) + @ calibration_transforms[self.main_sensor_frame]["pandar_40p_rear"] + @ np.linalg.inv(rear_kit_to_rear_lower_lidar_transform) + ) + + results = {self.base_frame: {}} + results[self.base_frame][self.top_unit_frame] = base_to_top_kit_transform + results[self.base_frame][self.front_unit_frame] = base_to_front_kit_transform + results[self.base_frame][self.rear_unit_frame] = base_to_rear_kit_transform + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..4373c4a2 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,175 @@ +#!/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 collections import defaultdict +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="x2", calibrator_name="tag_based_sfm_base_lidars_cameras_calibrator" +) +class TagBasedSfmBaseLidarsCamerasCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.top_unit_frame = "top_unit_base_link" + self.front_unit_frame = "front_unit_base_link" + self.rear_unit_frame = "rear_unit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + self.calibration_lidar_frames = [ + kwargs["calibration_lidar_1_frame"], + kwargs["calibration_lidar_2_frame"], + kwargs["calibration_lidar_3_frame"], + ] + self.calibration_lidar_base_frames = [ + lidar_frame + "_base_link" for lidar_frame in self.calibration_lidar_frames + ] + + self.calibration_camera_optical_link_frames = [ + kwargs["calibration_camera_0_frame"], + kwargs["calibration_camera_1_frame"], + kwargs["calibration_camera_2_frame"], + kwargs["calibration_camera_3_frame"], + kwargs["calibration_camera_4_frame"], + kwargs["calibration_camera_5_frame"], + kwargs["calibration_camera_6_frame"], + ] + self.calibration_camera_link_frames = [ + camera_frame.replace("camera_optical_link", "camera_link") + for camera_frame in self.calibration_camera_optical_link_frames + ] + + self.required_frames.extend( + [ + self.base_frame, + self.top_unit_frame, + self.front_unit_frame, + self.rear_unit_frame, + self.main_sensor_frame, + *self.calibration_lidar_frames, + *self.calibration_lidar_base_frames, + *self.calibration_camera_optical_link_frames, + *self.calibration_camera_link_frames, + ] + ) + + self.add_calibrator( + service_name="calibrate_base_lidars_cameras", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame), + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_lidar_frames + ], + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_camera_optical_link_frames + ], + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + main_sensor_to_base_transform = calibration_transforms[self.main_sensor_frame][ + self.base_frame + ] + + top_kit_to_main_lidar_transform = self.get_transform_matrix( + self.top_unit_frame, self.main_sensor_frame + ) + + front_kit_to_front_lower_lidar_transform = self.get_transform_matrix( + self.front_unit_frame, "pandar_40p_front" + ) + + rear_kit_to_rear_lower_lidar_transform = self.get_transform_matrix( + self.rear_unit_frame, "pandar_40p_rear" + ) + + optical_link_to_camera_link_transforms = { + camera_optical_link_frame: self.get_transform_matrix( + camera_optical_link_frame, camera_link_frame + ) + for camera_optical_link_frame, camera_link_frame in zip( + self.calibration_camera_optical_link_frames, self.calibration_camera_link_frames + ) + } + + base_to_top_kit_transform = np.linalg.inv( + top_kit_to_main_lidar_transform @ main_sensor_to_base_transform + ) + + results = {self.base_frame: {}} + + base_to_front_kit_transform = ( + np.linalg.inv(main_sensor_to_base_transform) + @ calibration_transforms[self.main_sensor_frame]["pandar_40p_front"] + @ np.linalg.inv(front_kit_to_front_lower_lidar_transform) + ) + base_to_rear_kit_transform = ( + np.linalg.inv(main_sensor_to_base_transform) + @ calibration_transforms[self.main_sensor_frame]["pandar_40p_rear"] + @ np.linalg.inv(rear_kit_to_rear_lower_lidar_transform) + ) + + results = defaultdict(dict) + results[self.base_frame][self.top_unit_frame] = base_to_top_kit_transform + results[self.base_frame][self.front_unit_frame] = base_to_front_kit_transform + results[self.base_frame][self.rear_unit_frame] = base_to_rear_kit_transform + + top_cameras = ["camera0", "camera1", "camera2", "camera4", "camera5"] + front_cameras = ["camera6"] + rear_cameras = ["camera3"] + + for top_camera in top_cameras: + results[self.top_unit_frame][f"{top_camera}/camera_link"] = ( + top_kit_to_main_lidar_transform + @ calibration_transforms[self.main_sensor_frame][ + f"{top_camera}/camera_optical_link" + ] + @ optical_link_to_camera_link_transforms[f"{top_camera}/camera_optical_link"] + ) + + for front_camera in front_cameras: + results[self.front_unit_frame][f"{front_camera}/camera_link"] = ( + np.linalg.inv(base_to_front_kit_transform) + @ np.linalg.inv(main_sensor_to_base_transform) + @ calibration_transforms[self.main_sensor_frame][ + f"{front_camera}/camera_optical_link" + ] + @ optical_link_to_camera_link_transforms[f"{front_camera}/camera_optical_link"] + ) + + for rear_camera in rear_cameras: + results[self.rear_unit_frame][f"{rear_camera}/camera_link"] = ( + np.linalg.inv(base_to_rear_kit_transform) + @ np.linalg.inv(main_sensor_to_base_transform) + @ calibration_transforms[self.main_sensor_frame][ + f"{rear_camera}/camera_optical_link" + ] + @ optical_link_to_camera_link_transforms[f"{rear_camera}/camera_optical_link"] + ) + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/__init__.py new file mode 100644 index 00000000..b5c0103c --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/__init__.py @@ -0,0 +1,13 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator +from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator +from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator +from .tag_based_pnp_calibrator import TagBasedPNPCalibrator +from .tag_based_sfm_base_lidar_calibrator import TagBasedSfmBaseLidarCalibrator + +__all__ = [ + "GroundPlaneCalibrator", + "MappingBasedBaseLidarCalibrator", + "MappingBasedLidarLidarCalibrator", + "TagBasedPNPCalibrator", + "TagBasedSfmBaseLidarCalibrator", +] diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/ground_plane_calibrator.py new file mode 100644 index 00000000..fda4d2dc --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/ground_plane_calibrator.py @@ -0,0 +1,61 @@ +#!/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="xx1", calibrator_name="ground_plane_calibrator" +) +class GroundPlaneCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = "base_link" + self.sensor_kit_frame = "sensor_kit_base_link" + self.lidar_frame = "velodyne_top" + + self.required_frames.extend([self.base_frame, self.sensor_kit_frame, self.lidar_frame]) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.base_frame, child=self.lidar_frame), + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + base_to_lidar_transform = calibration_transforms[self.base_frame][self.lidar_frame] + + sensor_kit_to_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.lidar_frame + ) + + base_to_sensor_kit_transform = base_to_lidar_transform @ np.linalg.inv( + sensor_kit_to_lidar_transform + ) + + result = {self.base_frame: {self.sensor_kit_frame: base_to_sensor_kit_transform}} + + return result diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py new file mode 100644 index 00000000..b73d27b7 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py @@ -0,0 +1,62 @@ +#!/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="xx1", calibrator_name="mapping_based_base_lidar_calibrator" +) +class MappingBasedBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = "base_link" + self.sensor_kit_frame = "sensor_kit_base_link" + + self.mapping_lidar_frame = "velodyne_top" + + self.required_frames.extend( + [self.base_frame, self.sensor_kit_frame, self.mapping_lidar_frame] + ) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.mapping_lidar_frame, child=self.base_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.mapping_lidar_frame + ) + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.mapping_lidar_frame][self.base_frame] + ) + results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..be36cb88 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,87 @@ +#!/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="xx1", 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 = "velodyne_top" + self.calibration_lidar_frames = ["velodyne_left", "velodyne_right"] + self.calibration_base_lidar_frames = ["velodyne_left_base_link", "velodyne_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 diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..5098df1d --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py @@ -0,0 +1,67 @@ +#!/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="xx1", calibrator_name="tag_based_pnp_calibrator" +) +class TagBasedPNPCalibrator(CalibratorBase): + required_frames = ["sensor_kit_base_link", "velodyne_top_base_link", "velodyne_top"] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.camera_name = kwargs["camera_name"] + 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="velodyne_top"), + ], + ) + + 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" + ]["velodyne_top"] + sensor_kit_to_lidar_transform = self.get_transform_matrix( + "sensor_kit_base_link", "velodyne_top" + ) + camera_to_optical_link_transform = self.get_transform_matrix( + f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link" + ) + sensor_kit_camera_link_transform = np.linalg.inv( + camera_to_optical_link_transform + @ optical_link_to_lidar_transform + @ np.linalg.inv(sensor_kit_to_lidar_transform) + ) + + result = { + "sensor_kit_base_link": { + f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform + } + } + return result diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..dbafd6f6 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py @@ -0,0 +1,62 @@ +#!/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="xx1", calibrator_name="tag_based_sfm_base_lidar_calibrator" +) +class TagBasedSfmBaseLidarCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.sensor_kit_frame = "sensor_kit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + + self.required_frames.extend( + [self.base_frame, self.sensor_kit_frame, self.main_sensor_frame] + ) + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame) + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.main_sensor_frame + ) + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][self.base_frame] + ) + results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}} + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/__init__.py new file mode 100644 index 00000000..69c0988f --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/__init__.py @@ -0,0 +1,7 @@ +from .tag_based_pnp_calibrator import TagBasedPNPCalibrator +from .tag_based_sfm_base_lidars_cameras_calibrator import TagBasedSfmBaseLidarsCamerasCalibrator + +__all__ = [ + "TagBasedPNPCalibrator", + "TagBasedSfmBaseLidarsCamerasCalibrator", +] diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..f69841c2 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py @@ -0,0 +1,67 @@ +#!/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="xx1_15", calibrator_name="tag_based_pnp_calibrator" +) +class TagBasedPNPCalibrator(CalibratorBase): + required_frames = ["sensor_kit_base_link", "velodyne_top_base_link", "velodyne_top"] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.camera_name = kwargs["camera_name"] + 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="velodyne_top"), + ], + ) + + 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" + ]["velodyne_top"] + sensor_kit_to_lidar_transform = self.get_transform_matrix( + "sensor_kit_base_link", "velodyne_top" + ) + camera_to_optical_link_transform = self.get_transform_matrix( + f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link" + ) + sensor_kit_camera_link_transform = np.linalg.inv( + camera_to_optical_link_transform + @ optical_link_to_lidar_transform + @ np.linalg.inv(sensor_kit_to_lidar_transform) + ) + + result = { + "sensor_kit_base_link": { + f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform + } + } + return result diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..53c73cc3 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,107 @@ +#!/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 +from typing import List + +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="xx1_15", calibrator_name="tag_based_sfm_base_lidars_cameras_calibrator" +) +class TagBasedSfmBaseLidarsCamerasCalibrator(CalibratorBase): + required_frames = [] + + def __init__(self, ros_interface: RosInterface, **kwargs): + super().__init__(ros_interface) + + self.base_frame = kwargs["base_frame"] + self.sensor_kit_frame = "sensor_kit_base_link" + + self.main_sensor_frame = kwargs["main_calibration_sensor_frame"] + + self.calibration_camera_optical_link_frames: List[str] = [ + kwargs["calibration_camera_0_frame"], + kwargs["calibration_camera_1_frame"], + kwargs["calibration_camera_2_frame"], + kwargs["calibration_camera_3_frame"], + kwargs["calibration_camera_4_frame"], + kwargs["calibration_camera_5_frame"], + kwargs["calibration_camera_6_frame"], + ] + self.calibration_camera_link_frames = [ + camera_frame.replace("camera_optical_link", "camera_link") + for camera_frame in self.calibration_camera_optical_link_frames + ] + + self.required_frames.extend( + [ + self.base_frame, + self.sensor_kit_frame, + self.main_sensor_frame, + *self.calibration_camera_optical_link_frames, + *self.calibration_camera_link_frames, + ] + ) + + self.add_calibrator( + service_name="calibrate_base_lidars_cameras", + expected_calibration_frames=[ + FramePair(parent=self.main_sensor_frame, child=self.base_frame), + *[ + FramePair(parent=self.main_sensor_frame, child=calibration_frame) + for calibration_frame in self.calibration_camera_optical_link_frames + ], + ], + ) + + def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): + sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.main_sensor_frame + ) + + optical_link_to_camera_link_transforms = [ + self.get_transform_matrix(camera_optical_link_frame, camera_link_frame) + for camera_optical_link_frame, camera_link_frame in zip( + self.calibration_camera_optical_link_frames, self.calibration_camera_link_frames + ) + ] + + base_to_top_sensor_kit_transform = np.linalg.inv( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][self.base_frame] + ) + results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}} + results[self.sensor_kit_frame] = {} + + for camera_frame, optical_link_to_camera_link_transform in zip( + self.calibration_camera_optical_link_frames, optical_link_to_camera_link_transforms + ): + results[self.sensor_kit_frame][ + camera_frame.replace("camera_optical_link", "camera_link") + ] = ( + sensor_kit_to_mapping_lidar_transform + @ calibration_transforms[self.main_sensor_frame][camera_frame] + @ optical_link_to_camera_link_transform + ) + + return results diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py new file mode 100644 index 00000000..e6f49c41 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py @@ -0,0 +1,136 @@ +#!/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 collections import defaultdict +import threading + +import rclpy +from rclpy.duration import Duration +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from tf2_msgs.msg import TFMessage +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tier4_calibration_msgs.srv import ExtrinsicCalibrator + + +class RosInterface(Node): + def __init__(self): + super().__init__("sensor_calibration_manager") + + self.lock = threading.RLock() + self.ros_executor = None + + # ROS Interface configuration + self.publish_tf = None + + # UI interface. + self.tfs_available_ui_callback = None + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.tf_qos_profile = rclpy.qos.QoSProfile( + reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, + durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, + history=rclpy.qos.HistoryPolicy.KEEP_LAST, + depth=100, + ) + + self.tf_sub = self.create_subscription( + TFMessage, + "/tf_static", + self.tf_callback, + self.tf_qos_profile, + ) + + self.tf_graph_available = False + self.tf_msg = defaultdict(lambda: defaultdict(list)) + + self.calibration_services_dict = {} + self.calibration_futures_dict = {} + self.calibration_result_callback_dict = {} + self.calibration_status_callback_dict = {} + self.calibration_service_start_dict = {} + + self.timer = self.create_timer(1.0, self.timer_callback) + + def set_tfs_graph_callback(self, callback): + self.tfs_graph_ui_callback = callback + + def tf_callback(self, msg): + for transform in msg.transforms: + self.tf_msg[transform.header.frame_id][transform.child_frame_id] = transform.transform + + self.tfs_graph_ui_callback(self.tf_msg) + + def can_transform(self, parent: str, child: str): + return self.tf_buffer.can_transform( + parent, child, rclpy.time.Time(), timeout=Duration(seconds=0.0) + ) + + def get_transform(self, parent: str, child: str): + assert self.tf_buffer.can_transform( + parent, child, rclpy.time.Time(), timeout=Duration(seconds=0.0) + ) + with self.lock: + return self.tf_buffer.lookup_transform( + parent, child, rclpy.time.Time(), timeout=Duration(seconds=0.0) + ).transform + + def timer_callback(self): + with self.lock: + for service_name, client in self.calibration_services_dict.items(): + if self.calibration_service_start_dict[service_name]: + continue + + service_status = client.service_is_ready() + self.calibration_status_callback_dict[service_name](service_status) + + for service_name in list(self.calibration_futures_dict.keys()): + future = self.calibration_futures_dict[service_name] + + if future.done(): + self.calibration_result_callback_dict[service_name](future.result()) + self.calibration_futures_dict.pop(service_name) + + def register_calibration_service(self, service_name, result_callback, status_callback): + with self.lock: + self.calibration_services_dict[service_name] = self.create_client( + ExtrinsicCalibrator, service_name + ) + + self.calibration_result_callback_dict[service_name] = result_callback + self.calibration_status_callback_dict[service_name] = status_callback + self.calibration_service_start_dict[service_name] = False + + def call_calibration_service(self, service_name): + with self.lock: + req = ExtrinsicCalibrator.Request() + future = self.calibration_services_dict[service_name].call_async(req) + self.calibration_futures_dict[service_name] = future + self.calibration_service_start_dict[service_name] = True + + # We stop listening for tfs after calibration starts (depending on the calibrator, tfs may change and we do not want them to affect the behavior of this node) + self.tf_listener.unregister() + + def spin(self): + self.ros_executor = SingleThreadedExecutor() + self.ros_executor.add_node(self) + + self.thread = threading.Thread(target=self.executor.spin, args=()) + self.thread.setDaemon(True) + self.thread.start() diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py new file mode 100644 index 00000000..2a298736 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py @@ -0,0 +1,317 @@ +#!/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 collections import defaultdict +import copy +from functools import partial +import logging +import os +import signal +import subprocess +import sys +import threading +from typing import Dict + +from PySide2.QtCore import Signal +from PySide2.QtWidgets import QApplication +from PySide2.QtWidgets import QFileDialog +from PySide2.QtWidgets import QGroupBox +from PySide2.QtWidgets import QHBoxLayout +from PySide2.QtWidgets import QLabel +from PySide2.QtWidgets import QMainWindow +from PySide2.QtWidgets import QPushButton +from PySide2.QtWidgets import QTableView +from PySide2.QtWidgets import QVBoxLayout +from PySide2.QtWidgets import QWidget +from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.actions.set_launch_configuration import SetLaunchConfiguration +from launch.frontend import Parser +from launch.launch_description import LaunchDescription +import rclpy +from sensor_calibration_manager.calibration_manager_model import CalibratorManagerModel +from sensor_calibration_manager.calibrator_base import CalibratorState +from sensor_calibration_manager.calibrator_registry import CalibratorRegistry +import sensor_calibration_manager.calibrators # noqa: F401 Force imports +from sensor_calibration_manager.ros_interface import RosInterface +from sensor_calibration_manager.views.calibrator_selector_view import CalibrationSelectorView +from sensor_calibration_manager.views.launcher_configuration_view import LauncherConfigurationView +from sensor_calibration_manager.views.tf_view import TfView + + +class SensorCalibrationManager(QMainWindow): + tfs_graph_signal = Signal(object) + + def __init__(self): + super().__init__() + + self.central_widget = QWidget(self) + # self.central_widget.resize(1000,1000) + + self.setCentralWidget(self.central_widget) + # self.setWindowTitle("New extrinsic calibration manager") + + self.ros_interface: RosInterface = None + self.tfs_dict: Dict[str, Dict[str, None]] = defaultdict(lambda: defaultdict(None)) + + # Threading variables + self.lock = threading.RLock() + + # Control widgets + self.action_button = QPushButton("Calibrate") + self.action_button.setEnabled(False) + self.status_label = QLabel("Not ready") + + # MVC + self.calibrators_view = QTableView() + + # Tf Views + self.initial_tfs_view = TfView() + self.calibration_tfs_view = TfView() + self.final_tfs_view = TfView() + + self.initial_tfs_group = QGroupBox("Initial TF tree") + initial_tfs_layout = QVBoxLayout() + initial_tfs_layout.addWidget(self.initial_tfs_view) + self.initial_tfs_group.setLayout(initial_tfs_layout) + + self.calibration_tfs_group = QGroupBox("Calibration tree") + calibration_tfs_layout = QVBoxLayout() + calibration_tfs_layout.addWidget(self.calibration_tfs_view) + self.calibration_tfs_group.setLayout(calibration_tfs_layout) + + self.final_tfs_group = QGroupBox("Final TF tree") + final_tfs_layout = QVBoxLayout() + final_tfs_layout.addWidget(self.final_tfs_view) + self.final_tfs_group.setLayout(final_tfs_layout) + + self.main_layout = QHBoxLayout(self.central_widget) + + self.control_layout = QHBoxLayout() + self.control_layout.addWidget(self.status_label) + self.control_layout.addStretch() + self.control_layout.addWidget(self.action_button) + + left_layout = QVBoxLayout() + left_layout.addLayout(self.control_layout) + left_layout.addWidget(self.calibrators_view) + # left_layout.SetFixedWidth(600) + + right_layout = QVBoxLayout() + right_layout.addWidget(self.initial_tfs_group) + right_layout.addWidget(self.calibration_tfs_group) + right_layout.addWidget(self.final_tfs_group) + + self.main_layout.addLayout(left_layout) + self.main_layout.addLayout(right_layout) + + self.tfs_graph_signal.connect(self.tf_graph_callback2) + + selector_view = CalibrationSelectorView() + selector_view.selected_calibrator_signal.connect(self.on_selected_calibrator) + + self.hide() + + def on_selected_calibrator(self, project_name, calibrator_name): + logging.info( + f"on_selected_calibrator: project_name={project_name} calibrator_name={calibrator_name}" + ) + self.launcher_configuration_view = LauncherConfigurationView(project_name, calibrator_name) + self.launcher_configuration_view.launcher_parameters.connect( + partial(self.launch_calibrators, project_name, calibrator_name) + ) + + def launch_calibrators( + self, project_name: str, calibrator_name: str, launch_argument_dict: Dict + ): + # Show the main UI + self.show() + + # Execute the launcher + argument_list = [f"{k}:={v}" for k, v in launch_argument_dict.items()] + + package_share_directory = get_package_share_directory("sensor_calibration_manager") + launcher_path = ( + package_share_directory + + "/launch/" + + project_name + + "/" + + calibrator_name + + ".launch.xml" + ) + + command_list = ["ros2", "launch", launcher_path] + argument_list + logging.info(f"Launching calibrator with the following command: {command_list}") + self.process = subprocess.Popen(command_list) + + # Recover all the launcher arguments (in addition to user defined in launch_arguments) + try: + with open(launcher_path) as f: + root_entity, parser = Parser.load(f) + except Exception as e: + logging.error("Failed reading xml file. Either not-existent or invalid") + raise e + + ld: LaunchDescription = parser.parse_description(root_entity) + context = LaunchContext() + context.launch_configurations.update(launch_argument_dict) + + for e in ld.entities: + if isinstance(e, (DeclareLaunchArgument, SetLaunchConfiguration)): + e.visit(context) + + # Start the ROS interface + self.ros_interface = RosInterface() + self.ros_interface.set_tfs_graph_callback(self.tfs_graph_callback) + + # Create the calibrator wrapper + self.calibrator = CalibratorRegistry.create_calibrator( + project_name, + calibrator_name, + ros_interface=self.ros_interface, + **context.launch_configurations, + ) + self.calibrator.state_changed_signal.connect(self.on_calibrator_state_changed) + self.calibrator.calibration_finished_signal.connect(self.on_calibration_finished) + + self.calibrators_model = CalibratorManagerModel(self.calibrator.get_service_wrappers()) + self.calibrators_view.setModel(self.calibrators_model) + self.calibrators_view.resizeColumnsToContents() + self.calibrators_view.setFixedWidth(800) + + self.ros_interface.spin() + + def on_calibrator_state_changed(self, state: CalibratorState): + text_dict = { + CalibratorState.WAITING_TFS: "Waiting for the required TFs to be available", + CalibratorState.WAITING_SERVICES: "Waiting for calibration services", + CalibratorState.READY: "Ready to calibrate", + CalibratorState.CALIBRATING: "Calibrating...", + CalibratorState.FINISHED: "Calibration finished", + } + + self.status_label.setText(text_dict[state]) + + if state == CalibratorState.READY: + self.action_button.clicked.connect(self.on_calibration_request) + self.action_button.setEnabled(True) + elif state == CalibratorState.FINISHED: + self.action_button.clicked.disconnect() + self.action_button.clicked.connect(self.on_save_request) + self.action_button.setEnabled(True) + self.action_button.setText("Save calibration") + else: + self.action_button.setEnabled(False) + + def on_calibration_request(self): + logging.debug("on_calibration_request") + self.calibrator.start_calibration() + + def on_calibration_finished(self): + tf_dict = self.calibrator.get_calibration_results() + processed_tf_dict = self.calibrator.get_processed_calibration_results() + self.calibration_tfs_view.setTfs(tf_dict) + + # There are two main cases: + # 1) The processed_tf_dict modify the original tfs + # 2) The processed tf_dict makes a new structure, hence it must be displayed as such + final_tfs_dict = copy.deepcopy(self.tfs_dict) + + changed_frames_dict = self.calibration_results = defaultdict(set) + + for parent, child_and_tf in processed_tf_dict.items(): + if parent not in final_tfs_dict: + continue + for ( + child, + transform, + ) in child_and_tf.items(): + if child in final_tfs_dict[parent]: + final_tfs_dict[parent][child] = transform + changed_frames_dict[parent].add(child) + + if len(changed_frames_dict) == len(processed_tf_dict): + self.final_tfs_view.setTfs( + final_tfs_dict, + changed_frames_dict=changed_frames_dict, + required_frames=self.calibrator.required_frames, + ) + else: + self.final_tfs_view.setTfs( + processed_tf_dict, + changed_frames_dict=None, + required_frames=self.calibrator.required_frames, + ) + + def on_save_request(self): + logging.debug("on_save_request") + + output_path = QFileDialog.getSaveFileName( + None, "Save File", f"{os.getcwd()}/calibration_results.yaml", "YAML files (*.yaml)" + ) + + logging.debug(output_path) + output_path = output_path[0] + + if output_path is None or output_path == "": + logging.error("Invalid path") + return + + logging.info(f"Saving calibration results to {output_path}") + self.calibrator.save_results(output_path, use_rpy=True) + + def tfs_graph_callback(self, tfs_dict): + with self.lock: + tfs_dict = copy.deepcopy(tfs_dict) + + self.tfs_graph_signal.emit(copy.deepcopy(tfs_dict)) + + def tf_graph_callback2(self, tfs_dict): + if self.calibrator.state != CalibratorState.WAITING_TFS: + return + + for parent, children_and_tf_dict in tfs_dict.items(): + self.tfs_dict[parent].update(children_and_tf_dict) + + if self.tfs_dict and len(self.tfs_dict) > 0 and self.calibrator: + self.initial_tfs_view.setTfs( + self.tfs_dict, required_frames=self.calibrator.required_frames + ) + + +def main(args=None): + os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = "" + app = QApplication(sys.argv) + + rclpy.init(args=args) + + try: + signal.signal(signal.SIGINT, sigint_handler) + calibration_manager = SensorCalibrationManager() # noqa: F841 + + sys.exit(app.exec_()) + except (KeyboardInterrupt, SystemExit): + logging.info("Received sigint. Quitting...") + rclpy.shutdown() + + +def sigint_handler(*args): + QApplication.quit() + + +if __name__ == "__main__": + main() diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py new file mode 100644 index 00000000..6f612736 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py @@ -0,0 +1,31 @@ +#!/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 enum import Enum +from typing import NamedTuple + + +class CalibratorState(Enum): + WAITING_TFS = 1 + WAITING_SERVICES = 2 + READY = 3 + CALIBRATING = 4 + FINISHED = 5 + + +class FramePair(NamedTuple): + parent: str + child: str diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py new file mode 100644 index 00000000..c79e0835 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py @@ -0,0 +1,56 @@ +#!/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 geometry_msgs.msg import Transform +import numpy as np +import transforms3d + + +def tf_message_to_transform_matrix(msg): + transform_matrix = np.eye(4) + + q = msg.rotation + rotation_matrix = transforms3d.quaternions.quat2mat((q.w, q.x, q.y, q.z)) + + transform_matrix[0:3, 0:3] = rotation_matrix + transform_matrix[0, 3] = msg.translation.x + transform_matrix[1, 3] = msg.translation.y + transform_matrix[2, 3] = msg.translation.z + + return transform_matrix + + +def transform_matrix_to_tf_message(transform_matrix): + q = transforms3d.quaternions.mat2quat(transform_matrix[0:3, 0:3]) + + msg = Transform() + msg.translation.x = transform_matrix[0, 3] + msg.translation.y = transform_matrix[1, 3] + msg.translation.z = transform_matrix[2, 3] + msg.rotation.x = q[1] + msg.rotation.y = q[2] + msg.rotation.z = q[3] + msg.rotation.w = q[0] + + return msg + + +def decompose_transformation_matrix(transformation): + return transformation[0:3, 3].reshape(3, 1), transformation[0:3, 0:3] + + +def stamp_to_seconds(time): + return time.sec + 1e-9 * time.nanosec diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/views/calibrator_selector_view.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/calibrator_selector_view.py new file mode 100644 index 00000000..8a9451a0 --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/calibrator_selector_view.py @@ -0,0 +1,93 @@ +#!/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. + +import logging + +from PySide2.QtCore import Signal +from PySide2.QtWidgets import QComboBox +from PySide2.QtWidgets import QGroupBox +from PySide2.QtWidgets import QPushButton +from PySide2.QtWidgets import QVBoxLayout +from PySide2.QtWidgets import QWidget +from sensor_calibration_manager.calibrator_registry import CalibratorRegistry + + +class CalibrationSelectorView(QWidget): + """Initial widget to let the user configure the calibrator.""" + + selected_calibrator_signal = Signal(str, str) + + def __init__(self): + super().__init__() + + self.setWindowTitle("Calibrator selection menu") + self.setMinimumWidth(300) + + self.layout = QVBoxLayout(self) + + # calibrator + self.calibrator_group = QGroupBox("Calibrator:") + self.calibrator_group.setFlat(True) + + self.calibrator_combobox = QComboBox() + + calibrator_layout = QVBoxLayout() + calibrator_layout.addWidget(self.calibrator_combobox) + self.calibrator_group.setLayout(calibrator_layout) + + def onNewProjectName(new_project_name): + self.calibrator_combobox.clear() + + for calibrator_name in CalibratorRegistry.getProjectCalibrators(new_project_name): + self.calibrator_combobox.addItem(calibrator_name) + + # Project + + self.project_group = QGroupBox("Project:") + self.project_group.setFlat(True) + + self.project_combobox = QComboBox() + self.project_combobox.currentTextChanged.connect(onNewProjectName) + + for project_name in CalibratorRegistry.getProjects(): + self.project_combobox.addItem(project_name) + + project_layout = QVBoxLayout() + project_layout.addWidget(self.project_combobox) + self.project_group.setLayout(project_layout) + + self.start_button = QPushButton("Continue") + self.start_button.clicked.connect(self.on_click) + + self.layout.addWidget(self.project_group) + self.layout.addWidget(self.calibrator_group) + self.layout.addWidget(self.start_button) + + self.show() + + def on_click(self): + """Start the calibration process after receiving the user settings.""" + self.selected_calibrator_signal.emit( + self.project_combobox.currentText(), self.calibrator_combobox.currentText() + ) + self.close() + + def closeEvent(self, event): + """When the window is closed, the widget is marked for deletion.""" + # self.closed.emit() + event.accept() + self.deleteLater() + logging.info("Closing calibrator selector view") diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/views/launcher_configuration_view.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/launcher_configuration_view.py new file mode 100644 index 00000000..da4de64f --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/launcher_configuration_view.py @@ -0,0 +1,244 @@ +#!/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 functools import reduce +import logging +from typing import Dict + +from PySide2.QtCore import Signal +from PySide2.QtWidgets import QComboBox +from PySide2.QtWidgets import QGridLayout +from PySide2.QtWidgets import QGroupBox +from PySide2.QtWidgets import QLabel +from PySide2.QtWidgets import QLineEdit +from PySide2.QtWidgets import QPushButton +from PySide2.QtWidgets import QScrollArea +from PySide2.QtWidgets import QVBoxLayout +from PySide2.QtWidgets import QWidget +from ament_index_python.packages import get_package_share_directory +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.frontend import Parser +from launch.launch_description import LaunchDescription + + +class LauncherConfigurationView(QWidget): + """A simple widget to visualize and edit a ParameterizedClass's parameters.""" + + closed = Signal() + launcher_parameters = Signal(dict) + + def __init__(self, project_name, calibrator_name): + super().__init__() + + self.setWindowTitle("Launcher configuration") + + self.outer_layout = QVBoxLayout() + self.inner_layout = QVBoxLayout() + + self.required_argument_layout = QGridLayout() + self.optional_argument_layout = QGridLayout() + + self.required_arguments_group = QGroupBox("Required arguments:") + self.required_arguments_group.setFlat(True) + + self.optional_arguments_group = QGroupBox("Optional arguments:") + self.optional_arguments_group.setFlat(True) + + self.optional_arguments_dict = {} + self.required_arguments_dict = {} + self.arguments_widgets_dict = {} + + package_share_directory = get_package_share_directory("sensor_calibration_manager") + launcher_path = ( + package_share_directory + + "/launch/" + + project_name + + "/" + + calibrator_name + + ".launch.xml" + ) + + logging.info(f"Reading xml from: {launcher_path}") + + try: + with open(launcher_path) as f: + root_entity, parser = Parser.load(f) + except Exception as e: + logging.error("Failed reading xml file. Either not-existent or invalid") + raise e + + ld: LaunchDescription = parser.parse_description(root_entity) + + for e in ld.entities: + if not isinstance(e, DeclareLaunchArgument): + continue + + description = e.description if e.description != "no description given" else "" + + if e.default_value is not None and len(e.default_value) > 0: + default_value = e.default_value[-1].text.replace( + " ", "" + ) # KL: not sure if should the first or last default value + + self.optional_arguments_dict[e.name] = { + "value": default_value, + "description": description, + "choices": e.choices, + } + else: + self.required_arguments_dict[e.name] = { + "value": "", + "description": description, + "choices": e.choices, + } + + self.required_argument_layout.addWidget(QLabel("Name"), 0, 0) + self.required_argument_layout.addWidget(QLabel("Value"), 0, 1) + self.required_argument_layout.addWidget(QLabel("Help"), 0, 2) + + for i, (argument_name, argument_data) in enumerate(self.required_arguments_dict.items()): + name_label = QLabel(argument_name) + name_label.setMaximumWidth(400) + + default_value = argument_data["value"].replace(" ", "") + + if argument_data["choices"] is None or len(argument_data["choices"]) == 0: + self.arguments_widgets_dict[argument_name] = QLineEdit(default_value) + self.arguments_widgets_dict[argument_name].textChanged.connect( + self.check_argument_status + ) + + else: + combo_box = QComboBox() + + for choice in argument_data["choices"]: + combo_box.addItem(choice) + + combo_box.currentTextChanged.connect(self.check_argument_status) + self.arguments_widgets_dict[argument_name] = combo_box + + self.arguments_widgets_dict[argument_name].setMinimumWidth(400) + self.arguments_widgets_dict[argument_name].setMaximumWidth(800) + + description_label = QLabel(argument_data["description"]) + description_label.setMaximumWidth(400) + description_label.setToolTip(argument_data["description"]) + description_label.setText(argument_data["description"]) + + self.required_argument_layout.addWidget(name_label, i + 1, 0) + self.required_argument_layout.addWidget( + self.arguments_widgets_dict[argument_name], i + 1, 1 + ) + self.required_argument_layout.addWidget(description_label, i + 1, 2) + + self.optional_argument_layout.addWidget(QLabel("Name"), 0, 0) + self.optional_argument_layout.addWidget(QLabel("Value"), 0, 1) + self.optional_argument_layout.addWidget(QLabel("Help"), 0, 2) + + for i, (argument_name, argument_data) in enumerate(self.optional_arguments_dict.items()): + name_label = QLabel(argument_name) + name_label.setMaximumWidth(400) + + if argument_data["choices"] is None or len(argument_data["choices"]) == 0: + self.arguments_widgets_dict[argument_name] = QLineEdit(argument_data["value"]) + self.arguments_widgets_dict[argument_name].textChanged.connect( + self.check_argument_status + ) + + else: + combo_box = QComboBox() + + for choice in argument_data["choices"]: + combo_box.addItem(choice) + + combo_box.currentTextChanged.connect(self.check_argument_status) + self.arguments_widgets_dict[argument_name] = combo_box + + self.arguments_widgets_dict[argument_name].setMinimumWidth(400) + self.arguments_widgets_dict[argument_name].setMaximumWidth(800) + + description_label = QLabel(argument_data["description"]) + description_label.setMaximumWidth(400) + description_label.setToolTip(argument_data["description"]) + description_label.setText(argument_data["description"]) + + self.optional_argument_layout.addWidget(name_label, i + 1, 0) + self.optional_argument_layout.addWidget( + self.arguments_widgets_dict[argument_name], i + 1, 1 + ) + self.optional_argument_layout.addWidget(description_label, i + 1, 2) + + self.required_arguments_group.setLayout(self.required_argument_layout) + self.optional_arguments_group.setLayout(self.optional_argument_layout) + + self.launch_button = QPushButton("Launch") + self.launch_button.clicked.connect(self.on_click) + + self.inner_layout.addWidget(self.required_arguments_group) + self.inner_layout.addWidget(self.optional_arguments_group) + self.inner_layout.addWidget(self.launch_button) + + self.widget = QWidget() + self.widget.setLayout(self.inner_layout) + + scroll_area = QScrollArea() + scroll_area.setWidget(self.widget) + + self.outer_layout.addWidget(scroll_area) + self.setLayout(self.outer_layout) + + self.check_argument_status() + self.setMinimumWidth(self.widget.width() + 50) + self.show() + + def check_argument_status(self, text=None): + self.launch_button.setEnabled( + reduce( + lambda a, b: a and b, + [ + len(widget.text()) > 0 if hasattr(widget, "text") else widget.currentText() + for widget in self.arguments_widgets_dict.values() + ], + ) + ) + logging.debug("check_argument_status") + + def on_click(self): + args_dict: Dict[str, str] = { + arg_name: ( + args_widget.text() if hasattr(args_widget, "text") else args_widget.currentText() + ) + for arg_name, args_widget in self.arguments_widgets_dict.items() + } + + def is_list(arg: str): + return len(arg) >= 2 and arg[0] == "[" and arg[-1] == "]" + + for key, value in args_dict.items(): + if is_list(value): + args_dict[key]: Dict[str, str] = [ + item.strip() for item in value.strip("[]").split(",") + ] + args_dict[key] = [int(v2) if v2.isnumeric() else v2 for v2 in args_dict[key]] + + self.launcher_parameters.emit(args_dict) + self.close() + + def closeEvent(self, event): + """When the widget is closed it should be marked for deletion and notify the event.""" + self.closed.emit() + event.accept() + self.deleteLater() diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py new file mode 100644 index 00000000..34854afa --- /dev/null +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py @@ -0,0 +1,358 @@ +#!/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 collections import defaultdict +import logging +from typing import Dict +from typing import List +from typing import Optional +from typing import Set + +from PySide2.QtCore import QXmlStreamReader +from PySide2.QtCore import Qt +from PySide2.QtGui import QPainter +from PySide2.QtSvg import QSvgRenderer +from PySide2.QtWidgets import QGraphicsScene +from PySide2.QtWidgets import QGraphicsView +from PySide2.QtWidgets import QWidget +import pydot +import transforms3d + + +class TfNode: + def __init__(self, frame, transform): + self.frame = frame + self.transform = transform + self.children = [] + + def asDict(self): + d = defaultdict(lambda: defaultdict(list)) + + for child in self.children: + d[self.frame][child.frame] = child.transform + d.update(child.asDict()) + + return d + + +class TfTree: + def __init__(self, tf_dict): + self.roots = [] + self.from_tf_dict(tf_dict) + + def from_tf_dict(self, tf_dicts): + child_to_parent_dict = {} + + for parent, children in tf_dicts.items(): + for child in children.keys(): + child_to_parent_dict[child] = parent + + root_frames = [] + for child in child_to_parent_dict.keys(): + aux = child + while aux in child_to_parent_dict: + aux = child_to_parent_dict[aux] + + root_frames.append(aux) + + root_frames = list(set(root_frames)) + + self.roots = [self.parseNode(root_frame, tf_dicts) for root_frame in root_frames] + + def parseNode(self, frame, tf_dicts): + node = TfNode(frame, None) + + for child_frame, transform in tf_dicts[frame].items(): + if child_frame not in tf_dicts: + node.children.append(TfNode(child_frame, transform)) + else: + child = self.parseNode(child_frame, tf_dicts) + child.transform = transform + node.children.append(child) + + return node + + def getSlicedTree( + self, current_node: TfNode, target_frames: List[str], current_frames: List[str] = [] + ): + if len(current_node.children) == 0: + return ( + (current_node, [current_node.frame]) + if current_node.frame in target_frames + else (None, []) + ) + + sliced_children = [] + sliced_children_frames = [] + + for children in current_node.children: + sliced_node, sliced_frames = self.getSlicedTree(children, target_frames, current_frames) + + if len(sliced_frames) == len(target_frames): + return sliced_node, sliced_frames + elif sliced_node is not None: + sliced_children.append(sliced_node) + sliced_children_frames += sliced_frames + + sliced_node = TfNode(current_node.frame, current_node.transform) + sliced_node.children = sliced_children + + if current_node.frame in target_frames: + sliced_children_frames = sliced_children_frames + [current_node.frame] + + if len(sliced_children) > 0: + return (sliced_node, sliced_children_frames) + elif current_node.frame in target_frames: + return (sliced_node, [current_node.frame]) + else: + return (None, sliced_children_frames) + + def getSlicesTrees(self, target_frames): + sliced_trees = [self.getSlicedTree(root, target_frames)[0] for root in self.roots] + + return [sliced_tree for sliced_tree in sliced_trees if sliced_tree is not None] + + +class TfPlot(QWidget): + def __init__(self): + super(TfPlot, self).__init__() + + self.renderer = None + self.setStyleSheet("background-color:white;") + + def setTfs( + self, + tfs_dict: Dict[Dict, str], + changed_frames_dict: Optional[Dict[str, Set[str]]] = None, + required_frames: Optional[List] = None, + ): + if required_frames: + tree = TfTree(tfs_dict) + sliced_nodes = tree.getSlicesTrees(required_frames) + sliced_dict = {} + [sliced_dict.update(sliced_node.asDict()) for sliced_node in sliced_nodes] + tfs_dict = sliced_dict + + graph = pydot.Dot("my_graph", graph_type="digraph", bgcolor="white") + + parents_set = set(tfs_dict.keys()) + children_set = { + child_frame for children_frames in tfs_dict.values() for child_frame in children_frames + } + + nodes = list(parents_set.union(children_set)) + edges = [ + (parent_frame, child_frame) + for parent_frame, children_frames in tfs_dict.items() + for child_frame in children_frames + ] + + # Add nodes + # for node in nodes: + # graph.add_node(pydot.Node(node)) + + # for parent, child in edges: + # graph.add_edge(pydot.Edge(parent, child)) + + graph_list = [] + graph_list.append("digraph G {\n") + + for parent, child in edges: + transform = tfs_dict[parent][child] + translation = transform.translation + quat = transform.rotation + x, y, z = translation.x, translation.y, translation.z + roll, pitch, yaw = transforms3d.euler.quat2euler([quat.w, quat.x, quat.y, quat.z]) + color = ( + "red" + if changed_frames_dict is not None + and parent in changed_frames_dict + and child in changed_frames_dict[parent] + else "black" + ) + graph_list.append( + f'"{parent}" -> "{child}"[color={color} label=" x={x:.4f}\\ny={y:.4f}\\nz={z:.4f}\\nyaw={yaw:.4f}\\npitch={pitch:.4f}\\nroll={roll:.4f}\\n"];\n' + ) + + if changed_frames_dict is not None: + changed_nodes = set() + [ + changed_nodes.add(parent) or changed_nodes.add(child) + for parent, children in changed_frames_dict.items() + for child in children + ] + for node in nodes: + color = "red" if node in changed_nodes else "black" + graph_list.append(f'"{node}" [color="{color}"]') + + graph_list.append("}") + graph_string = "".join(graph_list) + # print(f"graph_string={graph_string}") + graphs = pydot.graph_from_dot_data(graph_string) + graph = graphs[0] + + # graph.write_png("frames.png") + # graph.write_pdf("frames.pdf") + + # cspell: ignore imgdata + imgdata = graph.create_svg() + + # cSpell:ignore savefig + # imgdata = StringIO() + # figure.savefig(imgdata, format='svg') + # imgdata.seek(0) + xmlreader = QXmlStreamReader(imgdata) + self.renderer = QSvgRenderer(xmlreader) + self.renderer.setAspectRatioMode(Qt.AspectRatioMode.KeepAspectRatio) + + def paintEvent(self, event): + if self.renderer is None: + return + + p = QPainter() + p.begin(self) + self.renderer.render(p) + p.end() + # print(self.geometry()) + + +class TfView(QGraphicsView): + def __init__(self): + QGraphicsView.__init__(self) + + scene = QGraphicsScene(self) + self.scene = scene + + self.plot = TfPlot() + scene.addWidget(self.plot) + + self.setScene(scene) + + self.is_pan_active = False + self.pan_start_x = None + self.pan_start_y = None + + self.setMinimumWidth(400) + + def setTfs( + self, + tfs_dict, + changed_frames_dict: Optional[Dict[str, Set[str]]] = None, + required_frames: Optional[List] = None, + ): + self.plot.setTfs(tfs_dict, changed_frames_dict, required_frames) + # Reset the view + self.fitInView(0, 0, self.plot.width(), self.plot.height(), Qt.KeepAspectRatio) + + def resizeEvent(self, event): + logging.debug( + f"PRE resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}" + ) + super().resizeEvent(event) + logging.debug( + f"POST resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}" + ) + + # scaled_pix_size = self.pix.size() + # scaled_pix_size.scale(self.data_renderer.widget_size, Qt.KeepAspectRatio) + + for item in self.scene.items(): + item.prepareGeometryChange() + item.update() + + def mouseMoveEvent(self, event): + if self.is_pan_active: + self.horizontalScrollBar().setValue( + self.horizontalScrollBar().value() - (event.x() - self.pan_start_x) + ) + self.verticalScrollBar().setValue( + self.verticalScrollBar().value() - (event.y() - self.pan_start_y) + ) + self.pan_start_x = event.x() + self.pan_start_y = event.y() + event.accept() + return + + event.ignore() + + def mousePressEvent(self, event): + if event.button() == Qt.LeftButton: + self.is_pan_active = True + self.pan_start_x = event.x() + self.pan_start_y = event.y() + self.setCursor(Qt.ClosedHandCursor) + event.accept() + return + + event.ignore() + + def mouseDoubleClickEvent(self, event): + if event.button() == Qt.LeftButton: + self.fitInView(0, 0, self.plot.width(), self.plot.height(), Qt.KeepAspectRatio) + return + + event.ignore() + + def mouseReleaseEvent(self, event): + if event.button() == Qt.LeftButton: + self.is_pan_active = False + self.pan_start_x = None + self.pan_start_y = None + self.setCursor(Qt.ArrowCursor) + event.accept() + return + + event.ignore() + + def wheelEvent(self, event): + zoom_in_factor = 1.25 + zoom_out_factor = 1 / zoom_in_factor + + for item in self.scene.items(): + item.prepareGeometryChange() + item.update() + + self.setTransformationAnchor(QGraphicsView.NoAnchor) + self.setResizeAnchor(QGraphicsView.NoAnchor) + + old_pos = self.mapToScene(event.pos()) + + # Zoom + if event.delta() > 0: + zoom_factor = zoom_in_factor + else: + zoom_factor = zoom_out_factor + + self.scale(zoom_factor, zoom_factor) + + ##### + # targetScenePos = self.mapToScene(event.pos()) + # oldAnchor = self.transformationAnchor() + # self.setTransformationAnchor(QGraphicsView.NoAnchor) + + # matrix = self.transform() + # matrix.translate(targetScenePos.x(), targetScenePos.y()).scale(zoom_factor, zoom_factor).translate(-targetScenePos.x(), -targetScenePos.y()) + # self.setTransform(matrix) + + # self.setTransformationAnchor(oldAnchor) + #### + + # Get the new position + new_pos = self.mapToScene(event.pos()) + + # Move scene to old position + delta = new_pos - old_pos + self.translate(delta.x(), delta.y()) diff --git a/sensor/sensor_calibration_manager/setup.cfg b/sensor/sensor_calibration_manager/setup.cfg new file mode 100644 index 00000000..866f29b7 --- /dev/null +++ b/sensor/sensor_calibration_manager/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sensor_calibration_manager +[install] +install_scripts=$base/lib/sensor_calibration_manager diff --git a/sensor/sensor_calibration_manager/setup.py b/sensor/sensor_calibration_manager/setup.py new file mode 100644 index 00000000..d6010f63 --- /dev/null +++ b/sensor/sensor_calibration_manager/setup.py @@ -0,0 +1,51 @@ +import os + +from setuptools import setup + +package_name = "sensor_calibration_manager" + +data_files = [ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), +] + + +def package_files(data_files, directory_list): + paths_dict = {} + + for directory in directory_list: + for path, _, filenames in os.walk(directory): + for filename in filenames: + file_path = os.path.join(path, filename) + install_path = os.path.join("share", package_name, path) + + if install_path in paths_dict.keys(): + paths_dict[install_path].append(file_path) + + else: + paths_dict[install_path] = [file_path] + + for key in paths_dict.keys(): + data_files.append((key, paths_dict[key])) + + return data_files + + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=package_files(data_files, ["launch/"]), + install_requires=["setuptools"], + zip_safe=True, + maintainer="Kenzo Lobos Tsunekawa", + maintainer_email="kenzo.lobos@tier4.jp", + description="the sensor_calibration_manager", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "sensor_calibration_manager = sensor_calibration_manager.sensor_calibration_manager:main" + ], + }, +) diff --git a/sensor/sensor_calibration_manager/test/test_pep257.py b/sensor/sensor_calibration_manager/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/sensor/sensor_calibration_manager/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/sensor/sensor_calibration_tools/CMakeLists.txt b/sensor/sensor_calibration_tools/CMakeLists.txt new file mode 100644 index 00000000..9f46aea8 --- /dev/null +++ b/sensor/sensor_calibration_tools/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 3.14) +project(sensor_calibration_tools) + +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/sensor/sensor_calibration_tools/package.xml b/sensor/sensor_calibration_tools/package.xml new file mode 100644 index 00000000..639efa9b --- /dev/null +++ b/sensor/sensor_calibration_tools/package.xml @@ -0,0 +1,31 @@ + + + sensor_calibration_tools + 0.0.1 + Meta package for sensor calibration related packages + Kenzo Lobos Tsunekawa + Apache License 2.0 + + ament_cmake + + extrinsic_manual_calibrator + extrinsic_map_based_calibrator + ground_plane_calibrator + interactive_camera_lidar_calibrator + intrinsic_camera_calibrator + lidar_to_lidar_2d_calibrator + mapping_based_calibrator + marker_radar_lidar_calibrator + sensor_calibration_manager + tag_based_pnp_calibrator + tag_based_sfm_calibrator + tier4_calibration_msgs + tier4_calibration_pcl_extensions + tier4_calibration_views + tier4_ground_plane_utils + tier4_tag_utils + + + ament_cmake + + diff --git a/sensor/extrinsic_tag_based_calibrator/CMakeLists.txt b/sensor/tag_based_pnp_calibrator/CMakeLists.txt similarity index 57% rename from sensor/extrinsic_tag_based_calibrator/CMakeLists.txt rename to sensor/tag_based_pnp_calibrator/CMakeLists.txt index 63c39e47..237134f3 100644 --- a/sensor/extrinsic_tag_based_calibrator/CMakeLists.txt +++ b/sensor/tag_based_pnp_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_tag_based_calibrator) +project(tag_based_pnp_calibrator) find_package(autoware_cmake REQUIRED) find_package(OpenCV REQUIRED) @@ -12,17 +12,15 @@ ament_export_include_directories( ${OpenCV_INCLUDE_DIRS} ) - # COMPILE THE SOURCE -#======================================================================== -ament_auto_add_executable(extrinsic_tag_based_calibrator +ament_auto_add_executable(tag_based_pnp_calibrator src/brute_force_matcher.cpp src/calibration_estimator.cpp - src/extrinsic_tag_based_calibrator.cpp + src/tag_based_pnp_calibrator.cpp src/tag_calibrator_visualizer.cpp src/main.cpp ) -target_link_libraries(extrinsic_tag_based_calibrator +target_link_libraries(tag_based_pnp_calibrator ${OpenCV_LIBS} ) diff --git a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/brute_force_matcher.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp similarity index 74% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/brute_force_matcher.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp index a5e30958..53c4a1bd 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/brute_force_matcher.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ -#define EXTRINSIC_TAG_BASED_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ +#ifndef TAG_BASED_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ +#define TAG_BASED_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ #include #include @@ -26,12 +26,13 @@ typedef pcl::PointNormal PointNT; typedef pcl::PointCloud PointCloudT; -typedef pcl::FPFHSignature33 FeatureT; -typedef pcl::FPFHEstimationOMP FeatureEstimationT; +typedef pcl::FPFHSignature33 FeatureT; // cSpell:ignore FPFH +typedef pcl::FPFHEstimationOMP + FeatureEstimationT; // cSpell:ignore FPFH typedef pcl::PointCloud FeatureCloudT; bool bruteForceMatcher( PointCloudT::Ptr & source, PointCloudT::Ptr & target, double thresh, std::vector & source_indexes, std::vector & target_indexes, bool debug = false); -#endif // EXTRINSIC_TAG_BASED_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ diff --git a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/calibration_estimator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp similarity index 63% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/calibration_estimator.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp index 56e617c7..8447d96a 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/calibration_estimator.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ -#define EXTRINSIC_TAG_BASED_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#ifndef TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#define TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ #include #include @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -31,6 +30,7 @@ #include #include +#include #include #include @@ -40,17 +40,12 @@ class CalibrationEstimator CalibrationEstimator(); void update(const apriltag_msgs::msg::AprilTagDetectionArray & msg); - void update(const lidartag_msgs::msg::LidarTagDetectionArray & msg); - void update(const apriltag_msgs::msg::AprilTagDetection & msg, const rclcpp::Time & time_stamp); - void update(const lidartag_msgs::msg::LidarTagDetection & msg, const rclcpp::Time & time_stamp); - bool update(const rclcpp::Time & timestamp); - void getCalibrationPoints( - std::vector & object_points, std::vector & image_pointsbool, + std::tuple, std::vector> getCalibrationPoints( bool use_estimated); bool calibrate(); @@ -69,13 +64,12 @@ class CalibrationEstimator const; void setCameraModel(const sensor_msgs::msg::CameraInfo & camera_info); - tf2::Transform getCurrentPose() const; - void getCurrentPose(cv::Matx31d & trans_vector, cv::Matx33d & rot_matrix) const; - tf2::Transform getFilteredPose() const; - void getFilteredPose(cv::Matx31d & trans_vector, cv::Matx33d & rot_matrix) const; + tf2::Transform getCurrentPoseAsTF() const; + std::tuple getCurrentPose() const; + tf2::Transform getFilteredPoseAsTF() const; + std::tuple getFilteredPose() const; // Parameters setters - void setDynamicsModel(tier4_tag_utils::DynamicsModel dynamics_mode); void setCrossvalidationTrainingRatio(double ratio); void setCalibrationConvergenceCriteria(int min_pairs, double min_area_percentage); void setMinPnpPairs(int min_pairs); @@ -85,42 +79,40 @@ class CalibrationEstimator void setTagSizes(std::vector & tag_id, std::vector & tag_sizes); void setLidartagMaxConvergenceThreshold( - double transl, double tansl_dot, double angle, double angle_dot); - void setLidartagNewHypothesisThreshold(double transl, double angle); - void setLidartagMeasurementNoise(double transl, double angle); - void setLidartagProcessNoise(double transl, double transl_dot, double rot, double rot_dot); + double translation, double translation_dot, double angle, double angle_dot); + void setLidartagNewHypothesisThreshold(double translation, double angle); + void setLidartagMeasurementNoise(double translation, double angle); + void setLidartagProcessNoise( + double translation, double translation_dot, double rotation, double rotation_dot); - void setApriltagMaxConvergenceThreshold(double transl); - void setApriltagNewHypothesisThreshold(double transl); - void setApriltagMeasurementNoise(double transl); - void setApriltagProcessNoise(double transl); + void setApriltagMaxConvergenceThreshold(double translation); + void setApriltagNewHypothesisThreshold(double translation); + void setApriltagMeasurementNoise(double translation); + void setApriltagProcessNoise(double translation); double getNewHypothesisDistance() const; double getCalibrationCoveragePercentage() const; int getCurrentCalibrationPairsNumber() const; - double getCrossValidationReprojError() const; + double getCrossValidationReprojectionError() const; + int getConvergencePairNumber() const; private: - void getCalibrationPointsIdBased( - std::vector & object_points, std::vector & image_points, + std::tuple, std::vector> getCalibrationPointsIdBased( bool use_estimated); - void getCalibrationPointsIdless( - std::vector & object_points, std::vector & image_points, + std::tuple, std::vector> getCalibrationPointsIdless( bool use_estimated); - bool calibrate( - const std::vector & object_points, const std::vector & image_points, - cv::Matx31d & translation_vector, cv::Matx33d & rotation_matrix); + std::tuple calibrate( + const std::vector & object_points, const std::vector & image_points); tf2::Transform toTf2( const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix) const; - void computeCrossValidationReprojError( + void computeCrossValidationReprojectionError( const std::vector & object_points, const std::vector & image_points); // Parameters - tier4_tag_utils::DynamicsModel dynamics_model_; // Calibration convergence criteria int convergence_min_pairs_; @@ -134,25 +126,25 @@ class CalibrationEstimator double new_hypothesis_distance_; // Lidartag estimation parameters - double lidartag_convergence_transl_; - double lidartag_convergence_transl_dot_; - double lidartag_convergence_rot_; - double lidartag_convergence_rot_dot_; - double lidartag_new_hypothesis_transl_; - double lidartag_new_hypothesis_rot_; - - double lidartag_process_noise_transl_; - double lidartag_process_noise_transl_dot_; - double lidartag_process_noise_rot_; - double lidartag_process_noise_rot_dot_; - double lidartag_measurement_noise_transl_; - double lidartag_measurement_noise_rot_; + double lidartag_convergence_translation_; + double lidartag_convergence_translation_dot_; + double lidartag_convergence_rotation_; + double lidartag_convergence_rotation_dot_; + double lidartag_new_hypothesis_translation_; + double lidartag_new_hypothesis_rotation_; + + double lidartag_process_noise_translation_; + double lidartag_process_noise_translation_dot_; + double lidartag_process_noise_rotation_; + double lidartag_process_noise_rotation_dot_; + double lidartag_measurement_noise_translation_; + double lidartag_measurement_noise_rotation_; // Apriltag estimation parameters - double apriltag_convergence_transl_; - double apriltag_new_hypothesis_transl_; - double apriltag_process_noise_transl_; - double apriltag_measurement_noise_transl_; + double apriltag_convergence_translation_; + double apriltag_new_hypothesis_translation_; + double apriltag_process_noise_translation_; + double apriltag_measurement_noise_translation_; image_geometry::PinholeCameraModel pinhole_camera_model_; @@ -168,10 +160,10 @@ class CalibrationEstimator std::vector> converged_apriltag_hypotheses_; // Output - double crossval_reproj_error_; + double crossvalidation_reprojection_error_; bool valid_; cv::Matx33d hypothesis_rotation_matrix_, observation_rotation_matrix_; cv::Matx31d hypothesis_translation_vector_, observation_translation_vector_; }; -#endif // EXTRINSIC_TAG_BASED_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/extrinsic_tag_based_calibrator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp similarity index 84% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/extrinsic_tag_based_calibrator.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp index 26c23fb6..aa266f9e 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/extrinsic_tag_based_calibrator.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_CALIBRATOR__EXTRINSIC_TAG_BASED_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_CALIBRATOR__EXTRINSIC_TAG_BASED_CALIBRATOR_HPP_ +#ifndef TAG_BASED_PNP_CALIBRATOR__TAG_BASED_PNP_CALIBRATOR_HPP_ +#define TAG_BASED_PNP_CALIBRATOR__TAG_BASED_PNP_CALIBRATOR_HPP_ -#include -#include #include #include #include +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include #include @@ -37,12 +38,6 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#else -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#endif - #include #include #include @@ -50,10 +45,10 @@ #include #include -class ExtrinsicTagBasedCalibrator : public rclcpp::Node +class ExtrinsicTagBasedPNPCalibrator : public rclcpp::Node { public: - explicit ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptions & options); + explicit ExtrinsicTagBasedPNPCalibrator(const rclcpp::NodeOptions & options); protected: void cameraImageCallback( @@ -87,11 +82,10 @@ class ExtrinsicTagBasedCalibrator : public rclcpp::Node const rclcpp::Time & timestamp); // Parameters - std::string parent_frame_; - std::string child_frame_; std::string base_frame_; - std::string calibration_mode_; float calib_rate_; + bool use_receive_time_; + bool use_rectified_image_; // Filter parameters double min_tag_size_; @@ -126,6 +120,7 @@ class ExtrinsicTagBasedCalibrator : public rclcpp::Node rclcpp::Service::SharedPtr service_server_; // Threading, sync, and result + bool request_received_; std::mutex mutex_; // Rviz visualizations @@ -142,15 +137,9 @@ class ExtrinsicTagBasedCalibrator : public rclcpp::Node std::string lidar_frame_; std::string optical_frame_; - tf2::Transform parent_to_lidar_tf2_; - tf2::Transform optical_axis_to_camera_tf2_; tf2::Transform initial_optical_axis_to_lidar_tf2_; tf2::Transform base_to_lidar_tf2_; bool got_initial_transform; - - double initial_reproj_error_; - double current_reproj_error_; - double filtered_reproj_error_; }; -#endif // EXTRINSIC_TAG_BASED_CALIBRATOR__EXTRINSIC_TAG_BASED_CALIBRATOR_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__TAG_BASED_PNP_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/tag_calibrator_visualizer.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp similarity index 86% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/tag_calibrator_visualizer.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp index a0a7734e..b3a376ce 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/tag_calibrator_visualizer.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ -#define EXTRINSIC_TAG_BASED_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ +#ifndef TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ +#define TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ #include -#include #include #include +#include #include #include @@ -73,8 +73,8 @@ class TagCalibratorVisualizer void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); void setLidartagMaxConvergenceThreshold( - double transl, double tansl_dot, double angle, double angle_dot); - void setApriltagMaxConvergenceThreshold(double transl); + double translation, double transl_dot, double angle, double angle_dot); + void setApriltagMaxConvergenceThreshold(double translation); private: void drawLidartagHypotheses( @@ -94,11 +94,11 @@ class TagCalibratorVisualizer double min_convergence_time_; double max_no_observation_time_; - double lidartag_convergence_transl_; - double lidartag_convergence_transl_dot_; - double lidartag_convergence_rot_; - double lidartag_convergence_rot_dot_; - double apriltag_convergence_transl_; + double lidartag_convergence_translation_; + double lidartag_convergence_translation_dot_; + double lidartag_convergence_rotation_; + double lidartag_convergence_rotation_dot_; + double apriltag_convergence_translation_; bool valid_base_lidar_transform_; bool valid_camera_lidar_transform_; @@ -123,4 +123,4 @@ class TagCalibratorVisualizer cv::Affine3d camera_base_transform_; }; -#endif // EXTRINSIC_TAG_BASED_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ diff --git a/sensor/extrinsic_tag_based_calibrator/launch/apriltag_16h5.launch.py b/sensor/tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/launch/apriltag_16h5.launch.py rename to sensor/tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py diff --git a/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml new file mode 100644 index 00000000..4cec786a --- /dev/null +++ b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/extrinsic_tag_based_calibrator/package.xml b/sensor/tag_based_pnp_calibrator/package.xml similarity index 87% rename from sensor/extrinsic_tag_based_calibrator/package.xml rename to sensor/tag_based_pnp_calibrator/package.xml index c797b8a5..dff93dcf 100644 --- a/sensor/extrinsic_tag_based_calibrator/package.xml +++ b/sensor/tag_based_pnp_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_tag_based_calibrator + tag_based_pnp_calibrator 0.0.1 - The extrinsic_tag_based_calibrator package + The tag_based_pnp_calibrator package Kenzo Lobos Tsunekawa BSD @@ -36,7 +36,6 @@ apriltag_ros lidartag - ament_cmake diff --git a/sensor/extrinsic_mapping_based_calibrator/rviz/x1.rviz b/sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz similarity index 67% rename from sensor/extrinsic_mapping_based_calibrator/rviz/x1.rviz rename to sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz index 16b45eb0..9b8fc094 100644 --- a/sensor/extrinsic_mapping_based_calibrator/rviz/x1.rviz +++ b/sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz @@ -1,15 +1,16 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Status1 - - /mapping velodyne pointcloud1/Topic1 - - /Frame Path1/Topic1 - Splitter Ratio: 0.5 - Tree Height: 746 + - /(Optimized) Binary Transformed Points1/Topic1 + - "/Cluster info: detail code1/Topic1" + - "/Cluster info: detail code1/Namespaces1" + - /Tag calibration markers (filtered)1/Namespaces1 + Splitter Ratio: 0.6812933087348938 + Tree Height: 797 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -21,21 +22,20 @@ Panels: - Class: rviz_common/Views Expanded: - /Current View1 - - /Current View1/Position1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: output map + SyncSource: Raw Pointcloud Visualization Manager: Class: "" Displays: - - Alpha: 0.10000000149011612 + - Alpha: 0.5 Cell Size: 1 Class: rviz_default_plugins/Grid - Color: 160; 160; 164 + Color: 226; 226; 226 Enabled: true Line Style: Line Width: 0.029999999329447746 @@ -47,7 +47,32 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 100 + Plane Cell Count: 20 + Reference Frame: + Value: true + - Alpha: 0.5 + Cell Size: 0.13500000536441803 + Class: rviz_default_plugins/Grid + Color: 226; 226; 226 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid Template + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: YZ + Plane Cell Count: 6 + Reference Frame: + Value: false + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 Reference Frame: Value: true - Alpha: 1 @@ -62,34 +87,27 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: mapping velodyne pointcloud + Name: Raw Pointcloud Position Transformer: XYZ Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 - Style: Flat Squares + Style: Points Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/top/outlier_filtered/pointcloud + Value: /pointcloud_topic_placeholder Use Fixed Frame: true Use rainbow: true - Value: false - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -101,127 +119,122 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: output map + Name: Points of Interest Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Size (m): 0.029999999329447746 + Style: Spheres Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/output_map + Value: /lidartag/whole_edged_pc Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.10000000149011612 - Head Length: 0.12999999523162842 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Frame Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 0; 0 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.019999999552965164 - Shaft Length: 0.05000000074505806 + 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: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 101 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Clusters + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Spheres Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/frame_path - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path + Value: /lidartag/cluster_edge_pc + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.20000000298023224 + 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 - Enabled: true - Head Diameter: 0.07999999821186066 - Head Length: 0.12999999523162842 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Frame Predicted Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 255; 255 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.019999999552965164 - Shaft Length: 0.03999999910593033 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 196 + Min Color: 0; 0; 0 + Min Intensity: 2 + Name: Filled Clusters + Position Transformer: XYZ + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/frame_predicted_path - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.10000000149011612 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: KeyFrame Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 0; 255; 0 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.05999999865889549 - Shaft Length: 0.10000000149011612 + Value: /lidartag/detected_pc + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Filled Clusters + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/keyframe_path - Value: true + Value: /lidartag/boundary_marker + Value: false - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: KeyFrame Markers + Enabled: false + Name: Filled Cluster B&W Namespaces: - frame_id: true - keyframe_id: true + {} Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/keyframe_markers - Value: true + Value: /lidartag/cluster_marker + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -231,31 +244,31 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor + Color: 255; 255; 255 + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 101 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_left_initial_source_aligned + Min Intensity: 33 + Name: Boundary Points Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Size (m): 0.019999999552965164 + Style: Boxes Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_left/initial_source_aligned_map + Value: /lidartag/boundary_pts Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -265,31 +278,31 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 0 - Color Transformer: FlatColor + Color: 255; 255; 255 + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 2.369355800876173e-38 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_left_calibrated_source_aligned + Min Intensity: 2.369355800876173e-38 + Name: Estimated Corners (PCA) Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Size (m): 0.05000000074505806 + Style: Spheres Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_left/calibrated_source_aligned_map + Value: /lidartag/transformed_points_tag Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -300,30 +313,30 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 50 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_left_target + Min Intensity: 50 + Name: Initial guess Corners Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Size (m): 0.10000000149011612 + Style: Spheres Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_left/target_map + Value: /lidartag/transformed_points Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -333,16 +346,16 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor + Color: 255; 255; 255 + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 101 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_right_initial_source_aligned + Min Intensity: 1 + Name: Initial Transformed Points Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -354,43 +367,34 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_right/initial_source_aligned_map + Value: /lidartag/initial_template_points Use Fixed Frame: true Use rainbow: true - 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: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 + Value: false + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_right_calibrated_source_aligned - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: Tag Frame + Namespaces: + "": true Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_right/calibrated_source_aligned_map - Use Fixed Frame: true - Use rainbow: true + Value: /lidartag/tag_frame + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ID + Namespaces: + Text3: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidartag/id_markers Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -402,15 +406,15 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 158 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_right_target + Min Intensity: 1 + Name: (Optimized) Transformed Point Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -422,10 +426,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_right/target_map + Value: /lidartag/template_points Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -435,16 +439,16 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor + Color: 255; 255; 255 + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 216 Min Color: 0; 0; 0 Min Intensity: 0 - Name: livox_center_initial_source_aligned + Name: (Optimized) Binary Transformed Points Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -456,10 +460,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_center/initial_source_aligned_map + Value: /lidartag/template_points_3d Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -469,16 +473,16 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 0 - Color Transformer: FlatColor + Color: 255; 255; 255 + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 200 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: livox_center_calibrated_source_aligned + Min Intensity: 50 + Name: Template Points Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -490,10 +494,34 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_center/calibrated_source_aligned_map + Value: /lidartag/associated_pattern_3d Use Fixed Frame: true Use rainbow: true - Value: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Template Frame + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidartag/ideal_frame + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray (Unused) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidartag/detail_valid_marker + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -504,19 +532,19 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: livox_center_target + Name: (Before Transformed) Edge Pointcloud Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.029999999329447746 Style: Flat Squares Topic: Depth: 5 @@ -524,10 +552,46 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/livox_front_center/target_map + Value: /lidartag/before_transformed_edge_pc Use Fixed Frame: true Use rainbow: true - Value: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Intersection Markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidartag/intersection_markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: "Cluster info: detail code" + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidartag/cluster_buff_index_number_markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: "Cluster info: size" + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidartag/cluster_buff_points_size_markers + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -538,19 +602,19 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: FlatColor + Color Transformer: RGB8 Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: camera0_target + Name: Colored Cluster Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.03999999910593033 Style: Flat Squares Topic: Depth: 5 @@ -558,9 +622,40 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera0/camera_optical_link/target_map + Value: /lidartag/colored_cluster_buff Use Fixed Frame: true Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Tag calibration markers (unfiltered) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /current_projections + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Tag calibration markers (filtered) + Namespaces: + active_center: true + active_lidartag_frame: true + active_lidartag_id: true + active_lidartag_status: true + calibration_status: true + converged_center: true + converged_lidartag_frame: true + converged_lidartag_id: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_projections Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -574,17 +669,17 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 0 Min Color: 0; 0; 0 Min Intensity: 0 - Name: camera1_target + Name: edges1 Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -592,10 +687,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera1/camera_optical_link/target_map + Value: /lidartag/edge_group_1 Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -605,20 +700,20 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 255 + Color: 255; 255; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 0 Min Color: 0; 0; 0 Min Intensity: 0 - Name: camera2_target + Name: edges2 Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -626,10 +721,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera2/camera_optical_link/target_map + Value: /lidartag/edge_group_2 Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -639,20 +734,20 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 + Color: 255; 0; 255 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 0 Min Color: 0; 0; 0 Min Intensity: 0 - Name: camera3_target + Name: edges3 Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -660,10 +755,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera3/camera_optical_link/target_map + Value: /lidartag/edge_group_3 Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -673,20 +768,20 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 + Color: 0; 255; 255 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 0 Min Color: 0; 0; 0 Min Intensity: 0 - Name: camera4_target + Name: edges4 Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -694,10 +789,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera4/camera_optical_link/target_map + Value: /lidartag/edge_group_4 Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -708,19 +803,19 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 2.369355800876173e-38 Min Color: 0; 0; 0 - Min Intensity: 0 - Name: camera5_target + Min Intensity: 2.369355800876173e-38 + Name: Initial Corners Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.07999999821186066 Style: Flat Squares Topic: Depth: 5 @@ -728,86 +823,14 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /camera5/camera_optical_link/target_map + Value: /lidartag/initial_corners Use Fixed Frame: true Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_0_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera0/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_1_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera1/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_2_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera2/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_3_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera3/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_4_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera4/camera_optical_link/target_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: camera_5_markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera5/camera_optical_link/target_markers - Value: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: calibration_map + Fixed Frame: lidar_frame Frame Rate: 30 Name: root Tools: @@ -858,22 +881,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6499999165534973 + Pitch: 0.38499999046325684 Position: - X: -3.9148199558258057 - Y: 1.8611059188842773 - Z: 6.485511302947998 + X: -0.02556505985558033 + Y: 5.670583724975586 + Z: 4.358150005340576 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 5.768178939819336 + Yaw: 4.710000038146973 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1016 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002df00000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004510000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002060000035afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000000000000000fb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004240000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -881,7 +904,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true + collapsed: false Width: 1846 - X: 74 - Y: 0 + X: 1994 + Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/src/brute_force_matcher.cpp b/sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp similarity index 97% rename from sensor/extrinsic_tag_based_calibrator/src/brute_force_matcher.cpp rename to sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp index 12574393..16c6d415 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/brute_force_matcher.cpp +++ b/sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include @@ -59,7 +59,7 @@ bool bruteForceMatcher( ransac_align.setInputTarget(target); ransac_align.setTargetFeatures(target_features); ransac_align.setMaximumIterations( - 10000); // Due to the problem dimensionality, this shold be high + 10000); // Due to the problem dimensionality, this should be high ransac_align.setNumberOfSamples(4); ransac_align.setCorrespondenceRandomness(5); ransac_align.setSimilarityThreshold(0.9f); // This will reject most hypotheses @@ -182,7 +182,7 @@ bool bruteForceMatcher( return false; } - // Check if the ICP aligning satisties the convergence criteria + // Check if the ICP aligning satisfies the convergence criteria for (std::size_t i = 0; i < icp_correspondences->size(); ++i) { int source_id = (*icp_correspondences)[i].index_query; diff --git a/sensor/extrinsic_tag_based_calibrator/src/calibration_estimator.cpp b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp similarity index 73% rename from sensor/extrinsic_tag_based_calibrator/src/calibration_estimator.cpp rename to sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp index 6fc8f51f..ca6e4be6 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/calibration_estimator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,47 +12,41 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include // note: this header must come before #include #include #include -#include +#include +#include +#include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include CalibrationEstimator::CalibrationEstimator() -: dynamics_model_(tier4_tag_utils::DynamicsModel::Static), - min_pnp_pairs_(4), +: min_pnp_pairs_(4), min_convergence_time_(5.0), max_no_observation_time_(2.0), - lidartag_convergence_transl_(0.05), - lidartag_convergence_transl_dot_(0.001), - lidartag_convergence_rot_(), - lidartag_convergence_rot_dot_(0.0001), - lidartag_new_hypothesis_transl_(0.2), - lidartag_new_hypothesis_rot_(CV_PI * 45 / 180.0), - lidartag_process_noise_transl_(0.005), - lidartag_process_noise_transl_dot_(0.005), - lidartag_process_noise_rot_(CV_PI * 0.5 / 180.0), - lidartag_process_noise_rot_dot_(CV_PI * 0.5 / 180.0), - lidartag_measurement_noise_transl_(0.05), - lidartag_measurement_noise_rot_(CV_PI * 5 / 180.0), - apriltag_convergence_transl_(0.5), - apriltag_new_hypothesis_transl_(10.0), - apriltag_process_noise_transl_(0.5), - apriltag_measurement_noise_transl_(2.0), - crossval_reproj_error_(std::numeric_limits::infinity()), + lidartag_convergence_translation_(0.05), + lidartag_convergence_translation_dot_(0.001), + lidartag_convergence_rotation_(), + lidartag_convergence_rotation_dot_(0.0001), + lidartag_new_hypothesis_translation_(0.2), + lidartag_new_hypothesis_rotation_(CV_PI * 45 / 180.0), + lidartag_process_noise_translation_(0.005), + lidartag_process_noise_translation_dot_(0.005), + lidartag_process_noise_rotation_(CV_PI * 0.5 / 180.0), + lidartag_process_noise_rotation_dot_(CV_PI * 0.5 / 180.0), + lidartag_measurement_noise_translation_(0.05), + lidartag_measurement_noise_rotation_(CV_PI * 5 / 180.0), + apriltag_convergence_translation_(0.5), + apriltag_new_hypothesis_translation_(10.0), + apriltag_process_noise_translation_(0.5), + apriltag_measurement_noise_translation_(2.0), + crossvalidation_reprojection_error_(std::numeric_limits::infinity()), valid_(false) { } @@ -84,15 +78,15 @@ void CalibrationEstimator::update( corners.push_back(cv::Point2d(c.x, c.y)); } - // 1) Create a new hypothesis for comparison convenienve + // 1) Create a new hypothesis for comparison convenience auto new_h = std::make_shared(detection.id, pinhole_camera_model_); - new_h->setMaxConvergenceThreshold(apriltag_convergence_transl_); + new_h->setMaxConvergenceThreshold(apriltag_convergence_translation_); new_h->setMaxNoObservationTime(max_no_observation_time_); - new_h->setMeasurementNoise(apriltag_measurement_noise_transl_); + new_h->setMeasurementNoise(apriltag_measurement_noise_translation_); new_h->setMinConvergenceTime(min_convergence_time_); - new_h->setNewHypothesisThreshold(apriltag_new_hypothesis_transl_); - new_h->setProcessNoise(apriltag_process_noise_transl_); + new_h->setNewHypothesisThreshold(apriltag_new_hypothesis_translation_); + new_h->setProcessNoise(apriltag_process_noise_translation_); new_h->setTagSize(tag_sizes_map_[detection.id]); new_h->update(corners, stamp); @@ -118,7 +112,7 @@ void CalibrationEstimator::update( return; } - // 4) Add the new hypotheses to the acive list + // 4) Add the new hypotheses to the active list active_apriltag_hypotheses_[detection.id] = new_h; } @@ -137,21 +131,22 @@ void CalibrationEstimator::update( cv::eigen2cv(translation_eigen, translation_cv); cv::eigen2cv(rotation_eigen, rotation_cv); - // 1) Create a new hypothesis for conevnience + // 1) Create a new hypothesis for convenience int hypothesis_id = detection.id >= 0 ? detection.id : (-active_lidartag_hypotheses_.size() - 1); auto new_h = std::make_shared(hypothesis_id); - new_h->setDynamicsModel(dynamics_model_); new_h->setMaxNoObservationTime(max_no_observation_time_); new_h->setMinConvergenceTime(min_convergence_time_); new_h->setMaxConvergenceThreshold( - lidartag_convergence_transl_, lidartag_convergence_transl_dot_, lidartag_convergence_rot_, - lidartag_convergence_rot_dot_); - new_h->setMeasurementNoise(lidartag_measurement_noise_transl_, lidartag_measurement_noise_rot_); - new_h->setNewHypothesisThreshold(lidartag_new_hypothesis_transl_, lidartag_new_hypothesis_rot_); + lidartag_convergence_translation_, lidartag_convergence_translation_dot_, + lidartag_convergence_rotation_, lidartag_convergence_rotation_dot_); + new_h->setMeasurementNoise( + lidartag_measurement_noise_translation_, lidartag_measurement_noise_rotation_); + new_h->setNewHypothesisThreshold( + lidartag_new_hypothesis_translation_, lidartag_new_hypothesis_rotation_); new_h->setProcessNoise( - lidartag_process_noise_transl_, lidartag_process_noise_transl_dot_, lidartag_process_noise_rot_, - lidartag_process_noise_rot_dot_); + lidartag_process_noise_translation_, lidartag_process_noise_translation_dot_, + lidartag_process_noise_rotation_, lidartag_process_noise_rotation_dot_); new_h->update(translation_cv, rotation_cv, detection.size, stamp); // 2) Compare with converged hypotheses @@ -267,9 +262,8 @@ bool CalibrationEstimator::update(const rclcpp::Time & stamp) return true; } -void CalibrationEstimator::getCalibrationPoints( - std::vector & object_points, std::vector & image_points, - bool use_estimated) +std::tuple, std::vector> +CalibrationEstimator::getCalibrationPoints(bool use_estimated) { bool negative_id = false; @@ -282,17 +276,18 @@ void CalibrationEstimator::getCalibrationPoints( } if (negative_id) { - return getCalibrationPointsIdless(object_points, image_points, use_estimated); + return getCalibrationPointsIdless(use_estimated); } else { - return getCalibrationPointsIdBased(object_points, image_points, use_estimated); + return getCalibrationPointsIdBased(use_estimated); } } -void CalibrationEstimator::getCalibrationPointsIdBased( - std::vector & lidartag_object_points, - std::vector & apriltag_image_points, bool use_estimated) +std::tuple, std::vector> +CalibrationEstimator::getCalibrationPointsIdBased(bool use_estimated) { assert(converged_lidartag_hypotheses_.size() == converged_apriltag_hypotheses_.size()); + std::vector lidartag_object_points; + std::vector apriltag_image_points; for (std::size_t i = 0; i < converged_lidartag_hypotheses_.size(); ++i) { std::shared_ptr & lidartag_h = @@ -317,12 +312,16 @@ void CalibrationEstimator::getCalibrationPointsIdBased( apriltag_image_points.insert( apriltag_image_points.end(), h_apriltag_image_points.begin(), h_apriltag_image_points.end()); } + + return std::make_tuple(lidartag_object_points, apriltag_image_points); } -void CalibrationEstimator::getCalibrationPointsIdless( - std::vector & object_points, std::vector & image_points, - bool use_estimated) +std::tuple, std::vector> +CalibrationEstimator::getCalibrationPointsIdless(bool use_estimated) { + std::vector object_points; + std::vector image_points; + std::vector apriltag_image_points; std::vector apriltag_object_points; std::vector apriltag_object_normals; @@ -426,78 +425,79 @@ void CalibrationEstimator::getCalibrationPointsIdless( if ( apriltag_cloud->size() != lidartag_cloud->size() || static_cast(apriltag_cloud->size()) < min_pnp_pairs_) { - return; + return std::make_tuple(object_points, image_points); } if (!bruteForceMatcher( apriltag_cloud, lidartag_cloud, thresh, apriltag_indexes, lidartag_indexes, false)) { - return; + return std::make_tuple(object_points, image_points); } assert(apriltag_indexes.size() == lidartag_indexes.size()); - object_points.clear(); - image_points.clear(); for (std::size_t i = 0; i < apriltag_indexes.size(); i++) { object_points.push_back(lidartag_object_points[lidartag_indexes[i]]); image_points.push_back(apriltag_image_points[apriltag_indexes[i]]); } + + return std::make_tuple(object_points, image_points); } bool CalibrationEstimator::calibrate() { - std::vector observation_object_points, estimated_object_points; - std::vector observation_image_points, estimated_image_points; - - getCalibrationPoints(observation_object_points, observation_image_points, false); - getCalibrationPoints(estimated_object_points, estimated_image_points, true); + auto [observation_object_points, observation_image_points] = getCalibrationPoints(false); + auto [estimated_object_points, estimated_image_points] = getCalibrationPoints(true); - bool observation_status = calibrate( - observation_object_points, observation_image_points, observation_translation_vector_, - observation_rotation_matrix_); - bool estimation_status = calibrate( - estimated_object_points, estimated_image_points, hypothesis_translation_vector_, - hypothesis_rotation_matrix_); + auto [observation_status, observation_translation_vector, observation_rotation_matrix] = + calibrate(observation_object_points, observation_image_points); + auto [estimation_status, hypothesis_translation_vector, hypothesis_rotation_matrix] = + calibrate(estimated_object_points, estimated_image_points); bool status = observation_status && estimation_status; valid_ |= status; - computeCrossValidationReprojError(estimated_object_points, estimated_image_points); + if (status) { + observation_translation_vector_ = observation_translation_vector; + observation_rotation_matrix_ = observation_rotation_matrix; + hypothesis_translation_vector_ = hypothesis_translation_vector; + hypothesis_rotation_matrix_ = hypothesis_rotation_matrix; + } + + computeCrossValidationReprojectionError(estimated_object_points, estimated_image_points); return status; } -bool CalibrationEstimator::calibrate( - const std::vector & object_points, const std::vector & image_points, - cv::Matx31d & translation_vector, cv::Matx33d & rotation_matrix) +std::tuple CalibrationEstimator::calibrate( + const std::vector & object_points, const std::vector & image_points) { + cv::Matx31d translation_vector; + cv::Matx33d rotation_matrix; + if ( object_points.size() != image_points.size() || static_cast(object_points.size()) < min_pnp_pairs_) { - return false; + return std::tuple(false, translation_vector, rotation_matrix); } auto camera_intrinsics = pinhole_camera_model_.intrinsicMatrix(); auto distortion_coeffs = pinhole_camera_model_.distortionCoeffs(); - std::vector undistorted_points; - cv::undistortPoints(image_points, undistorted_points, camera_intrinsics, distortion_coeffs); + cv::Mat rvec, tvec; - cv::sqpnp::PoseSolver solver; - std::vector rvec_vec, tvec_vec; - solver.solve(object_points, undistorted_points, rvec_vec, tvec_vec); + bool success = cv::solvePnP( + object_points, image_points, camera_intrinsics, distortion_coeffs, rvec, tvec, false, + cv::SOLVEPNP_SQPNP); - if (tvec_vec.size() == 0) { - return false; + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("calibration_estimator"), "PNP failed"); + return std::tuple(false, translation_vector, rotation_matrix); } - cv::Mat rvec = rvec_vec[0]; - cv::Mat tvec = tvec_vec[0]; - translation_vector = tvec; cv::Rodrigues(rvec, rotation_matrix); - return true; + return std::tuple(true, translation_vector, rotation_matrix); } tf2::Transform CalibrationEstimator::toTf2( @@ -507,21 +507,21 @@ tf2::Transform CalibrationEstimator::toTf2( tf2::Vector3 tf2_trans( translation_vector(0, 0), translation_vector(0, 1), translation_vector(0, 2)); - tf2::Matrix3x3 tf2_rot_matrix( + tf2::Matrix3x3 tf2_rotation_matrix( rotation_matrix(0, 0), rotation_matrix(0, 1), rotation_matrix(0, 2), rotation_matrix(1, 0), rotation_matrix(1, 1), rotation_matrix(1, 2), rotation_matrix(2, 0), rotation_matrix(2, 1), rotation_matrix(2, 2)); - return tf2::Transform(tf2_rot_matrix, tf2_trans); + return tf2::Transform(tf2_rotation_matrix, tf2_trans); } -void CalibrationEstimator::computeCrossValidationReprojError( +void CalibrationEstimator::computeCrossValidationReprojectionError( const std::vector & object_points, const std::vector & image_points) { // Iterate a number of times - // Permutate the imageo object + // Permutate the image object // Separate into train and test - const int trials = 30; + constexpr int trials = 30; std::vector indexes(object_points.size()); std::iota(indexes.begin(), indexes.end(), 0); @@ -552,12 +552,10 @@ void CalibrationEstimator::computeCrossValidationReprojError( } } - cv::Matx31d iter_translation_vector; - cv::Matx33d iter_rotation_matrix; std::vector eval_projected_points; - calibrate( - training_object_points, training_image_points, iter_translation_vector, iter_rotation_matrix); + [[maybe_unused]] auto [status, iter_translation_vector, iter_rotation_matrix] = + calibrate(training_object_points, training_image_points); cv::Matx31d iter_rvec; cv::Rodrigues(iter_rotation_matrix, iter_rvec); @@ -567,17 +565,17 @@ void CalibrationEstimator::computeCrossValidationReprojError( pinhole_camera_model_.intrinsicMatrix(), pinhole_camera_model_.distortionCoeffs(), eval_projected_points); - double reproj_error = 0.0; + double reprojection_error = 0.0; for (std::size_t i = 0; i < test_image_points.size(); i++) { double dist = cv::norm(test_image_points[i] - eval_projected_points[i]); - reproj_error += dist; + reprojection_error += dist; } - reproj_error /= test_image_points.size(); - error += reproj_error; + reprojection_error /= test_image_points.size(); + error += reprojection_error; } - crossval_reproj_error_ = error / trials; + crossvalidation_reprojection_error_ = error / trials; } bool CalibrationEstimator::converged() const @@ -631,33 +629,26 @@ void CalibrationEstimator::setCameraModel(const sensor_msgs::msg::CameraInfo & c pinhole_camera_model_.fromCameraInfo(camera_info); } -tf2::Transform CalibrationEstimator::getCurrentPose() const +tf2::Transform CalibrationEstimator::getCurrentPoseAsTF() const { return toTf2(observation_translation_vector_, observation_rotation_matrix_); } -void CalibrationEstimator::getCurrentPose( - cv::Matx31d & trans_vector, cv::Matx33d & rot_matrix) const +std::tuple CalibrationEstimator::getCurrentPose() const { - trans_vector = observation_translation_vector_; - rot_matrix = observation_rotation_matrix_; + return std::tuple( + observation_translation_vector_, observation_rotation_matrix_); } -tf2::Transform CalibrationEstimator::getFilteredPose() const +tf2::Transform CalibrationEstimator::getFilteredPoseAsTF() const { return toTf2(hypothesis_translation_vector_, hypothesis_rotation_matrix_); } -void CalibrationEstimator::getFilteredPose( - cv::Matx31d & trans_vector, cv::Matx33d & rot_matrix) const +std::tuple CalibrationEstimator::getFilteredPose() const { - trans_vector = hypothesis_translation_vector_; - rot_matrix = hypothesis_rotation_matrix_; -} - -void CalibrationEstimator::setDynamicsModel(tier4_tag_utils::DynamicsModel dynamics_mode) -{ - dynamics_model_ = dynamics_mode; + return std::tuple( + hypothesis_translation_vector_, hypothesis_rotation_matrix_); } void CalibrationEstimator::setCrossvalidationTrainingRatio(double ratio) @@ -697,53 +688,54 @@ void CalibrationEstimator::setTagSizes( } void CalibrationEstimator::setLidartagMaxConvergenceThreshold( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double transl_dot, double rotation, double rotation_dot) { - lidartag_convergence_transl_ = transl; - lidartag_convergence_transl_dot_ = transl_dot; - lidartag_convergence_rot_ = CV_PI * rot / 180.0; - lidartag_convergence_rot_dot_ = CV_PI * rot_dot / 180.0; + lidartag_convergence_translation_ = translation; + lidartag_convergence_translation_dot_ = transl_dot; + lidartag_convergence_rotation_ = CV_PI * rotation / 180.0; + lidartag_convergence_rotation_dot_ = CV_PI * rotation_dot / 180.0; } -void CalibrationEstimator::setLidartagNewHypothesisThreshold(double max_transl, double max_rot) +void CalibrationEstimator::setLidartagNewHypothesisThreshold( + double max_translation, double max_rotation) { - lidartag_new_hypothesis_transl_ = max_transl; - lidartag_new_hypothesis_rot_ = CV_PI * max_rot / 180.0; + lidartag_new_hypothesis_translation_ = max_translation; + lidartag_new_hypothesis_rotation_ = CV_PI * max_rotation / 180.0; } -void CalibrationEstimator::setLidartagMeasurementNoise(double transl, double rot) +void CalibrationEstimator::setLidartagMeasurementNoise(double translation, double rotation) { - lidartag_measurement_noise_transl_ = transl; - lidartag_measurement_noise_rot_ = CV_PI * rot / 180.0; + lidartag_measurement_noise_translation_ = translation; + lidartag_measurement_noise_rotation_ = CV_PI * rotation / 180.0; } void CalibrationEstimator::setLidartagProcessNoise( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { - lidartag_process_noise_transl_ = transl; - lidartag_process_noise_transl_dot_ = transl_dot; - lidartag_process_noise_rot_ = CV_PI * rot / 180.0; - lidartag_process_noise_rot_dot_ = CV_PI * rot_dot / 180.0; + lidartag_process_noise_translation_ = translation; + lidartag_process_noise_translation_dot_ = translation_dot; + lidartag_process_noise_rotation_ = CV_PI * rotation / 180.0; + lidartag_process_noise_rotation_dot_ = CV_PI * rotation_dot / 180.0; } -void CalibrationEstimator::setApriltagMaxConvergenceThreshold(double transl) +void CalibrationEstimator::setApriltagMaxConvergenceThreshold(double translation) { - apriltag_convergence_transl_ = transl; + apriltag_convergence_translation_ = translation; } -void CalibrationEstimator::setApriltagNewHypothesisThreshold(double max_transl) +void CalibrationEstimator::setApriltagNewHypothesisThreshold(double max_translation) { - apriltag_new_hypothesis_transl_ = max_transl; + apriltag_new_hypothesis_translation_ = max_translation; } -void CalibrationEstimator::setApriltagMeasurementNoise(double transl) +void CalibrationEstimator::setApriltagMeasurementNoise(double translation) { - apriltag_measurement_noise_transl_ = transl; + apriltag_measurement_noise_translation_ = translation; } -void CalibrationEstimator::setApriltagProcessNoise(double transl) +void CalibrationEstimator::setApriltagProcessNoise(double translation) { - apriltag_process_noise_transl_ = transl; + apriltag_process_noise_translation_ = translation; } double CalibrationEstimator::getNewHypothesisDistance() const { return new_hypothesis_distance_; } @@ -774,7 +766,9 @@ int CalibrationEstimator::getCurrentCalibrationPairsNumber() const return converged_lidartag_hypotheses_.size(); } -double CalibrationEstimator::getCrossValidationReprojError() const +double CalibrationEstimator::getCrossValidationReprojectionError() const { - return crossval_reproj_error_; + return crossvalidation_reprojection_error_; } + +int CalibrationEstimator::getConvergencePairNumber() const { return convergence_min_pairs_; } diff --git a/sensor/extrinsic_tag_based_calibrator/src/main.cpp b/sensor/tag_based_pnp_calibrator/src/main.cpp similarity index 78% rename from sensor/extrinsic_tag_based_calibrator/src/main.cpp rename to sensor/tag_based_pnp_calibrator/src/main.cpp index 8f698cb0..9be49c1e 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/main.cpp +++ b/sensor/tag_based_pnp_calibrator/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include @@ -23,8 +23,8 @@ int main(int argc, char ** argv) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::NodeOptions node_options; - std::shared_ptr node = - std::make_shared(node_options); + std::shared_ptr node = + std::make_shared(node_options); executor.add_node(node); executor.spin(); diff --git a/sensor/extrinsic_tag_based_calibrator/src/extrinsic_tag_based_calibrator.cpp b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp similarity index 61% rename from sensor/extrinsic_tag_based_calibrator/src/extrinsic_tag_based_calibrator.cpp rename to sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp index 51483f4e..b46997ad 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/extrinsic_tag_based_calibrator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,41 +12,36 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include // note: this header must come before #include #include -#include #include -#include -#include +#include +#include + +#include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptions & options) -: Node("extrinsic_tag_based_calibrator_node", options), +ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::NodeOptions & options) +: Node("tag_based_pnp_calibrator_node", options), tf_broadcaster_(this), + request_received_(false), got_initial_transform(false) { tf_buffer_ = std::make_shared(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); calib_rate_ = this->declare_parameter("calib_rate"); - parent_frame_ = this->declare_parameter("parent_frame"); - child_frame_ = this->declare_parameter("child_frame"); base_frame_ = this->declare_parameter("base_frame"); min_tag_size_ = this->declare_parameter("min_tag_size"); max_tag_distance_ = this->declare_parameter("max_tag_distance"); max_allowed_homography_error_ = this->declare_parameter("max_allowed_homography_error"); + use_receive_time_ = this->declare_parameter("use_receive_time"); + use_rectified_image_ = this->declare_parameter("use_rectified_image"); - std::string dynamics_model = this->declare_parameter("dynamics_model"); double calibration_crossvalidation_training_ratio = this->declare_parameter("calibration_crossvalidation_training_ratio"); int calibration_convergence_min_pairs = @@ -60,54 +55,55 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio std::vector tag_ids = this->declare_parameter>("tag_ids"); std::vector tag_sizes = this->declare_parameter>("tag_sizes"); - double lidartag_max_convergence_transl = - this->declare_parameter("lidartag_max_convergence_transl"); - double lidartag_max_convergence_transl_dot = - this->declare_parameter("lidartag_max_convergence_transl_dot"); - double lidartag_max_convergence_rot = - this->declare_parameter("lidartag_max_convergence_rot"); - double lidartag_max_convergence_rot_dot = - this->declare_parameter("lidartag_max_convergence_rot_dot"); - double lidartag_new_hypothesis_transl = - this->declare_parameter("lidartag_new_hypothesis_transl"); - double lidartag_new_hypothesis_rot = - this->declare_parameter("lidartag_new_hypothesis_rot"); - double lidartag_measurement_noise_transl = - this->declare_parameter("lidartag_measurement_noise_transl"); - double lidartag_measurement_noise_rot = - this->declare_parameter("lidartag_measurement_noise_rot"); - double lidartag_process_noise_transl = - this->declare_parameter("lidartag_process_noise_transl"); - double lidartag_process_noise_transl_dot = - this->declare_parameter("lidartag_process_noise_transl_dot"); - double lidartag_process_noise_rot = this->declare_parameter("lidartag_process_noise_rot"); - double lidartag_process_noise_rot_dot = - this->declare_parameter("lidartag_process_noise_rot_dot"); - - double apriltag_max_convergence_transl = - this->declare_parameter("apriltag_max_convergence_transl"); - double apriltag_new_hypothesis_transl = - this->declare_parameter("apriltag_new_hypothesis_transl"); - double apriltag_measurement_noise_transl = - this->declare_parameter("apriltag_measurement_noise_transl"); - double apriltag_process_noise_transl = - this->declare_parameter("apriltag_process_noise_transl"); + double lidartag_max_convergence_translation = + this->declare_parameter("lidartag_max_convergence_translation"); + double lidartag_max_convergence_translation_dot = + this->declare_parameter("lidartag_max_convergence_translation_dot"); + double lidartag_max_convergence_rotation = + this->declare_parameter("lidartag_max_convergence_rotation"); + double lidartag_max_convergence_rotation_dot = + this->declare_parameter("lidartag_max_convergence_rotation_dot"); + double lidartag_new_hypothesis_translation = + this->declare_parameter("lidartag_new_hypothesis_translation"); + double lidartag_new_hypothesis_rotation = + this->declare_parameter("lidartag_new_hypothesis_rotation"); + double lidartag_measurement_noise_translation = + this->declare_parameter("lidartag_measurement_noise_translation"); + double lidartag_measurement_noise_rotation = + this->declare_parameter("lidartag_measurement_noise_rotation"); + double lidartag_process_noise_translation = + this->declare_parameter("lidartag_process_noise_translation"); + double lidartag_process_noise_translation_dot = + this->declare_parameter("lidartag_process_noise_translation_dot"); + double lidartag_process_noise_rotation = + this->declare_parameter("lidartag_process_noise_rotation"); + double lidartag_process_noise_rotation_dot = + this->declare_parameter("lidartag_process_noise_rotation_dot"); + + double apriltag_max_convergence_translation = + this->declare_parameter("apriltag_max_convergence_translation"); + double apriltag_new_hypothesis_translation = + this->declare_parameter("apriltag_new_hypothesis_translation"); + double apriltag_measurement_noise_translation = + this->declare_parameter("apriltag_measurement_noise_translation"); + double apriltag_process_noise_translation = + this->declare_parameter("apriltag_process_noise_translation"); camera_info_sub_ = this->create_subscription( "camera_info", rclcpp::QoS(1).best_effort(), - std::bind(&ExtrinsicTagBasedCalibrator::cameraInfoCallback, this, std::placeholders::_1)); + std::bind(&ExtrinsicTagBasedPNPCalibrator::cameraInfoCallback, this, std::placeholders::_1)); lidartag_detections_array_sub_ = this->create_subscription( "lidartag/detections_array", 1, std::bind( - &ExtrinsicTagBasedCalibrator::lidarTagDetectionsCallback, this, std::placeholders::_1)); + &ExtrinsicTagBasedPNPCalibrator::lidarTagDetectionsCallback, this, std::placeholders::_1)); apriltag_detections_array_sub_ = this->create_subscription( "apriltag/detection_array", 1, std::bind( - &ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback, this, std::placeholders::_1)); + &ExtrinsicTagBasedPNPCalibrator::aprilTagDetectionsCallback, this, std::placeholders::_1)); filtered_projections_markers_pub_ = this->create_publisher("filtered_projections", 10); @@ -115,10 +111,6 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio calibration_points_pub_ = this->create_publisher( "calibration_points", 10); - estimator_.setDynamicsModel( - dynamics_model == "constant_velocity" ? tier4_tag_utils::DynamicsModel::ConstantVelocity - : tier4_tag_utils::DynamicsModel::Static); - estimator_.setCrossvalidationTrainingRatio(calibration_crossvalidation_training_ratio); estimator_.setCalibrationConvergenceCriteria( calibration_convergence_min_pairs, calibration_convergence_min_area_percentage); @@ -129,39 +121,28 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio estimator_.setTagSizes(tag_ids, tag_sizes); estimator_.setLidartagMaxConvergenceThreshold( - lidartag_max_convergence_transl, lidartag_max_convergence_transl_dot, - lidartag_max_convergence_rot, lidartag_max_convergence_rot_dot); + lidartag_max_convergence_translation, lidartag_max_convergence_translation_dot, + lidartag_max_convergence_rotation, lidartag_max_convergence_rotation_dot); estimator_.setLidartagNewHypothesisThreshold( - lidartag_new_hypothesis_transl, lidartag_new_hypothesis_rot); + lidartag_new_hypothesis_translation, lidartag_new_hypothesis_rotation); estimator_.setLidartagMeasurementNoise( - lidartag_measurement_noise_transl, lidartag_measurement_noise_rot); + lidartag_measurement_noise_translation, lidartag_measurement_noise_rotation); estimator_.setLidartagProcessNoise( - lidartag_process_noise_transl, lidartag_process_noise_transl_dot, lidartag_process_noise_rot, - lidartag_process_noise_rot_dot); + lidartag_process_noise_translation, lidartag_process_noise_translation_dot, + lidartag_process_noise_rotation, lidartag_process_noise_rotation_dot); - estimator_.setApriltagMaxConvergenceThreshold(apriltag_max_convergence_transl); - estimator_.setApriltagNewHypothesisThreshold(apriltag_new_hypothesis_transl); - estimator_.setApriltagMeasurementNoise(apriltag_measurement_noise_transl); - estimator_.setApriltagProcessNoise(apriltag_process_noise_transl); + estimator_.setApriltagMaxConvergenceThreshold(apriltag_max_convergence_translation); + estimator_.setApriltagNewHypothesisThreshold(apriltag_new_hypothesis_translation); + estimator_.setApriltagMeasurementNoise(apriltag_measurement_noise_translation); + estimator_.setApriltagProcessNoise(apriltag_process_noise_translation); - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(1.0 / calib_rate_)); + tf_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::duration(1.0 / calib_rate_), + std::bind(&ExtrinsicTagBasedPNPCalibrator::tfTimerCallback, this)); - auto tf_timer_callback = std::bind(&ExtrinsicTagBasedCalibrator::tfTimerCallback, this); - - tf_timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(tf_timer_callback), - this->get_node_base_interface()->get_context()); - - auto calib_timer_callback = - std::bind(&ExtrinsicTagBasedCalibrator::automaticCalibrationTimerCallback, this); - - calib_timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(calib_timer_callback), - this->get_node_base_interface()->get_context()); - - this->get_node_timers_interface()->add_timer(tf_timer_, nullptr); - this->get_node_timers_interface()->add_timer(calib_timer_, nullptr); + calib_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::duration(1.0 / calib_rate_), + std::bind(&ExtrinsicTagBasedPNPCalibrator::automaticCalibrationTimerCallback, this)); srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -169,7 +150,7 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio service_server_ = this->create_service( "extrinsic_calibration", std::bind( - &ExtrinsicTagBasedCalibrator::requestReceivedCallback, this, std::placeholders::_1, + &ExtrinsicTagBasedPNPCalibrator::requestReceivedCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srv_callback_group_); @@ -181,35 +162,38 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio visualizer_->setMinConvergenceTime(min_convergence_time); visualizer_->setMaxNoObservationTime(max_no_observation_time); visualizer_->setLidartagMaxConvergenceThreshold( - lidartag_max_convergence_transl, lidartag_max_convergence_transl_dot, - lidartag_max_convergence_rot, lidartag_max_convergence_rot_dot); - visualizer_->setApriltagMaxConvergenceThreshold(apriltag_max_convergence_transl); + lidartag_max_convergence_translation, lidartag_max_convergence_translation_dot, + lidartag_max_convergence_rotation, lidartag_max_convergence_rotation_dot); + visualizer_->setApriltagMaxConvergenceThreshold(apriltag_max_convergence_translation); } -void ExtrinsicTagBasedCalibrator::lidarTagDetectionsCallback( - const lidartag_msgs::msg::LidarTagDetectionArray::SharedPtr detections_msg) +void ExtrinsicTagBasedPNPCalibrator::lidarTagDetectionsCallback( + const lidartag_msgs::msg::LidarTagDetectionArray::SharedPtr detections_msg_ptr) { - latest_timestamp_ = rclcpp::Time(detections_msg->header.stamp); - lidar_frame_ = detections_msg->header.frame_id; - lidartag_detections_array_ = detections_msg; + lidartag_detections_array_ = detections_msg_ptr; - estimator_.update(*detections_msg); + if (use_receive_time_) { + lidartag_detections_array_->header.stamp = this->now(); + } + + latest_timestamp_ = rclcpp::Time(lidartag_detections_array_->header.stamp); + lidar_frame_ = lidartag_detections_array_->header.frame_id; + + estimator_.update(*lidartag_detections_array_); visualizer_->setLidarFrame(lidar_frame_); } -void ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback( - const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr detections_msg) +void ExtrinsicTagBasedPNPCalibrator::aprilTagDetectionsCallback( + const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr detections_msg_ptr) { - latest_timestamp_ = rclcpp::Time(detections_msg->header.stamp); - // Filter apriltag detections that are too far away from the sensor double max_distance_px = min_tag_size_ * pinhole_camera_model_.fx() / max_tag_distance_; auto filtered_detections = std::make_shared(); - filtered_detections->header = detections_msg->header; + filtered_detections->header = detections_msg_ptr->header; - for (auto & detection : detections_msg->detections) { + for (auto & detection : detections_msg_ptr->detections) { const int & corners_size = detection.corners.size(); double max_side_distance = 0.0; double max_homography_error = 0.0; @@ -229,7 +213,6 @@ void ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback( cv::Mat p_corner2 = H_inv * p_corner; - // According to the equation (x2, y2, 1) = H *(x1, y1, 1) the third component should be 1.0 double h_error = std::abs(p_corner2.at(2, 0) - 1.0); max_homography_error = std::max(max_homography_error, h_error); @@ -239,7 +222,7 @@ void ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback( // We discard detections that are theoretically detected too far away if (max_side_distance < max_distance_px) { - RCLCPP_WARN_STREAM( + RCLCPP_DEBUG_STREAM( get_logger(), "Discarding apriltag: size " << max_side_distance << " px. Expecting at least " << max_distance_px << " px"); @@ -248,7 +231,7 @@ void ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback( // We also discard detections with an unreliable homography if (max_homography_error > max_allowed_homography_error_) { - RCLCPP_WARN_STREAM( + RCLCPP_DEBUG_STREAM( get_logger(), "Discarding apriltag: homography error " << max_homography_error); continue; } @@ -258,118 +241,110 @@ void ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback( apriltag_detections_array_ = filtered_detections; + if (use_receive_time_) { + apriltag_detections_array_->header.stamp = this->now(); + } + + latest_timestamp_ = rclcpp::Time(apriltag_detections_array_->header.stamp); estimator_.update(*apriltag_detections_array_); } -/*void ExtrinsicTagBasedCalibrator::cameraImageCallback(const -sensor_msgs::msg::Image::ConstSharedPtr & msg_img, const -sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg_ci) +void ExtrinsicTagBasedPNPCalibrator::cameraInfoCallback( + const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) { - header_ = msg_ci->header; - optical_frame_ = msg_ci->header.frame_id; - camera_info_ = *msg_ci; - - pinhole_camera_model_.fromCameraInfo(camera_info_); - visualizer_->setCameraModel(camera_info_); + optical_frame_ = camera_info_msg->header.frame_id; + camera_info_ = *camera_info_msg; - if (manual_calibrator_ui) - { - cv::Mat cv_img_ = cv_bridge::toCvShare(msg_img)->image.clone(); - cv::cvtColor(cv_img_, cv_img_, CV_BayerRG2RGB); + if (use_receive_time_) { + camera_info_.header.stamp = this->now(); + } - manual_calibrator_ui->setImage(cv_img_); + if (use_rectified_image_) { + camera_info_.k[0] = camera_info_.p[0]; + camera_info_.k[2] = camera_info_.p[2]; + camera_info_.k[4] = camera_info_.p[5]; + camera_info_.k[5] = camera_info_.p[6]; + std::fill(camera_info_.d.begin(), camera_info_.d.end(), 0.0); } -}*/ -void ExtrinsicTagBasedCalibrator::cameraInfoCallback( - const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) -{ - latest_timestamp_ = rclcpp::Time(camera_info_msg->header.stamp); - header_ = camera_info_msg->header; - optical_frame_ = camera_info_msg->header.frame_id; - camera_info_ = *camera_info_msg; + header_ = camera_info_.header; + latest_timestamp_ = rclcpp::Time(header_.stamp); visualizer_->setCameraFrame(optical_frame_); - pinhole_camera_model_.fromCameraInfo(camera_info_); visualizer_->setCameraModel(camera_info_); estimator_.setCameraModel(camera_info_); } -void ExtrinsicTagBasedCalibrator::requestReceivedCallback( - const std::shared_ptr request, +void ExtrinsicTagBasedPNPCalibrator::requestReceivedCallback( + [[maybe_unused]] const std::shared_ptr + request, const std::shared_ptr response) { - CV_UNUSED(request); using std::chrono_literals::operator""s; + RCLCPP_INFO(this->get_logger(), "Received calibration request"); + + { + std::unique_lock lock(mutex_); + request_received_ = true; + } // Wait for subscription topic while (rclcpp::ok()) { rclcpp::sleep_for(1s); std::unique_lock lock(mutex_); - if (estimator_.converged() && got_initial_transform && estimator_.calibrate()) { + if (got_initial_transform && estimator_.converged() && estimator_.calibrate()) { break; } - - // RCLCPP_WARN_SKIPFIRST(this->get_logger(), "Waiting for the lidar-camera calibration to end"); } - tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPose(); - tf2::Transform parent_to_child_tf2 = - parent_to_lidar_tf2_ * optical_axis_to_lidar_tf2.inverse() * optical_axis_to_camera_tf2_; + tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPoseAsTF(); geometry_msgs::msg::Transform transform_msg; - transform_msg = tf2::toMsg(parent_to_child_tf2); - - response->success = true; - response->result_pose.position.x = transform_msg.translation.x; - response->result_pose.position.y = transform_msg.translation.y; - response->result_pose.position.z = transform_msg.translation.z; - response->result_pose.orientation = transform_msg.rotation; - - response->score = estimator_.getCrossValidationReprojError(); + transform_msg = tf2::toMsg(optical_axis_to_lidar_tf2); + + tier4_calibration_msgs::msg::CalibrationResult result; + result.success = true; + result.score = estimator_.getCrossValidationReprojectionError(); + result.message.data = + "Calibrated using " + std::to_string(estimator_.getCurrentCalibrationPairsNumber()) + " pairs"; + result.transform_stamped.transform = tf2::toMsg(optical_axis_to_lidar_tf2); + result.transform_stamped.header.frame_id = optical_frame_; + result.transform_stamped.child_frame_id = lidar_frame_; + + response->results.push_back(result); } -void ExtrinsicTagBasedCalibrator::tfTimerCallback() +void ExtrinsicTagBasedPNPCalibrator::tfTimerCallback() { - if (/*!got_initial_transform && */ lidar_frame_ != "" && optical_frame_ != "") { + if (!got_initial_transform && lidar_frame_ != "" && optical_frame_ != "") { try { - geometry_msgs::msg::TransformStamped parent_to_lidar_transform_msg; - geometry_msgs::msg::TransformStamped optical_axis_to_camera_transform_msg; geometry_msgs::msg::TransformStamped initial_optical_axis_to_lidar_transform_msg; geometry_msgs::msg::TransformStamped base_to_lidar_transform_msg; - parent_to_lidar_transform_msg = tf_buffer_->lookupTransform( - parent_frame_, lidar_frame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - - optical_axis_to_camera_transform_msg = tf_buffer_->lookupTransform( - optical_frame_, child_frame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - initial_optical_axis_to_lidar_transform_msg = tf_buffer_->lookupTransform( optical_frame_, lidar_frame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); base_to_lidar_transform_msg = tf_buffer_->lookupTransform( base_frame_, lidar_frame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - fromMsg(parent_to_lidar_transform_msg.transform, parent_to_lidar_tf2_); - fromMsg(optical_axis_to_camera_transform_msg.transform, optical_axis_to_camera_tf2_); fromMsg( initial_optical_axis_to_lidar_transform_msg.transform, initial_optical_axis_to_lidar_tf2_); fromMsg(base_to_lidar_transform_msg.transform, base_to_lidar_tf2_); // Set the fixed base-lidar tf to the visualizers - cv::Matx33d base_lidar_rot_matrix; + cv::Matx33d base_lidar_rotation_matrix; cv::Matx31d base_lidar_trans_vector; Eigen::Isometry3d base_lidar_transform_eigen = tf2::transformToEigen(tf2::toMsg(base_to_lidar_tf2_)); Eigen::Matrix3d base_lidar_rotation_eigen = base_lidar_transform_eigen.rotation(); Eigen::Vector3d base_lidar_translation_eigen = base_lidar_transform_eigen.translation(); - cv::eigen2cv(base_lidar_rotation_eigen, base_lidar_rot_matrix); + cv::eigen2cv(base_lidar_rotation_eigen, base_lidar_rotation_matrix); cv::eigen2cv(base_lidar_translation_eigen, base_lidar_trans_vector); - visualizer_->setBaseLidarTransform(base_lidar_trans_vector, base_lidar_rot_matrix); + visualizer_->setBaseLidarTransform(base_lidar_trans_vector, base_lidar_rotation_matrix); got_initial_transform = true; } catch (tf2::TransformException & ex) { @@ -384,23 +359,21 @@ void ExtrinsicTagBasedCalibrator::tfTimerCallback() return; } - tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPose(); - tf2::Transform parent_to_child_tf2 = - parent_to_lidar_tf2_ * optical_axis_to_lidar_tf2.inverse() * optical_axis_to_camera_tf2_; + tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPoseAsTF(); geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped.header.stamp = header_.stamp; - transform_stamped.header.frame_id = parent_frame_; - transform_stamped.child_frame_id = child_frame_; - transform_stamped.transform = tf2::toMsg(parent_to_child_tf2); + transform_stamped.header.frame_id = optical_frame_; + transform_stamped.child_frame_id = lidar_frame_; + transform_stamped.transform = tf2::toMsg(optical_axis_to_lidar_tf2); tf_broadcaster_.sendTransform(transform_stamped); } -void ExtrinsicTagBasedCalibrator::automaticCalibrationTimerCallback() +void ExtrinsicTagBasedPNPCalibrator::automaticCalibrationTimerCallback() { std::unique_lock lock(mutex_); - if (!apriltag_detections_array_ || !lidartag_detections_array_) { + if (!request_received_ || !apriltag_detections_array_ || !lidartag_detections_array_) { return; } @@ -408,27 +381,27 @@ void ExtrinsicTagBasedCalibrator::automaticCalibrationTimerCallback() if (estimator_.calibrate()) { // Visualization - cv::Matx33d initial_rot_matrix, current_rot_matrix, filtered_rot_matrix; - cv::Matx31d initial_trans_vector, current_trans_vector, filtered_trans_vector; + cv::Matx33d initial_rotation_matrix; + cv::Matx31d initial_trans_vector; - estimator_.getCurrentPose(current_trans_vector, current_rot_matrix); - estimator_.getFilteredPose(filtered_trans_vector, filtered_rot_matrix); + auto [current_trans_vector, current_rotation_matrix] = estimator_.getCurrentPose(); + auto [filtered_trans_vector, filtered_rotation_matrix] = estimator_.getFilteredPose(); Eigen::Isometry3d initial_transform_eigen = tf2::transformToEigen(tf2::toMsg(initial_optical_axis_to_lidar_tf2_)); Eigen::Matrix3d initial_rotation_eigen = initial_transform_eigen.rotation(); Eigen::Vector3d initial_translation_eigen = initial_transform_eigen.translation(); - cv::eigen2cv(initial_rotation_eigen, initial_rot_matrix); + cv::eigen2cv(initial_rotation_eigen, initial_rotation_matrix); cv::eigen2cv(initial_translation_eigen, initial_trans_vector); // Calculate the reprojection errors cv::Matx31d initial_rvec, current_rvec, filtered_rvec; - cv::Rodrigues(initial_rot_matrix, initial_rvec); - cv::Rodrigues(current_rot_matrix, current_rvec); - cv::Rodrigues(filtered_rot_matrix, filtered_rvec); + cv::Rodrigues(initial_rotation_matrix, initial_rvec); + cv::Rodrigues(current_rotation_matrix, current_rvec); + cv::Rodrigues(filtered_rotation_matrix, filtered_rvec); - visualizer_->setCameraLidarTransform(filtered_trans_vector, filtered_rot_matrix); + visualizer_->setCameraLidarTransform(filtered_trans_vector, filtered_rotation_matrix); std::vector current_projected_points, initial_projected_points, filtered_projected_points; @@ -437,10 +410,7 @@ void ExtrinsicTagBasedCalibrator::automaticCalibrationTimerCallback() auto distortion_coeffs = pinhole_camera_model_.distortionCoeffs(); // Obtain the calibration points - std::vector object_points; - std::vector image_points; - - estimator_.getCalibrationPoints(object_points, image_points, false); + auto [object_points, image_points] = estimator_.getCalibrationPoints(false); if (object_points.size() == 0 || image_points.size() == 0) { RCLCPP_ERROR(this->get_logger(), "Could not get the calibration points"); @@ -459,7 +429,7 @@ void ExtrinsicTagBasedCalibrator::automaticCalibrationTimerCallback() object_points, filtered_rvec, filtered_trans_vector, camera_intrinsics, distortion_coeffs, filtered_projected_points); - auto reprojection_error = [](auto & points1, auto & points2) { + auto reprojection_error = [](auto & points1, auto & points2) -> double { double error = 0.0; for (std::size_t i = 0; i < points1.size(); i++) { @@ -469,9 +439,21 @@ void ExtrinsicTagBasedCalibrator::automaticCalibrationTimerCallback() return error / points1.size(); }; - initial_reproj_error_ = reprojection_error(image_points, initial_projected_points); - current_reproj_error_ = reprojection_error(image_points, current_projected_points); - filtered_reproj_error_ = reprojection_error(image_points, filtered_projected_points); + double initial_reprojection_error = reprojection_error(image_points, initial_projected_points); + double current_reprojection_error = reprojection_error(image_points, current_projected_points); + double filtered_reprojection_error = + reprojection_error(image_points, filtered_projected_points); + + RCLCPP_INFO( + this->get_logger(), + "Partial calibration results (%d/%d pairs):", estimator_.getCurrentCalibrationPairsNumber(), + estimator_.getConvergencePairNumber()); + RCLCPP_INFO( + this->get_logger(), "\tInitial reprojection error=%.2f", initial_reprojection_error); + RCLCPP_INFO( + this->get_logger(), "\tCurrent reprojection error=%.2f", current_reprojection_error); + RCLCPP_INFO( + this->get_logger(), "\tFiltered reprojection error=%.2f", filtered_reprojection_error); // Publish calibration points publishCalibrationPoints(object_points, image_points); @@ -484,7 +466,7 @@ void ExtrinsicTagBasedCalibrator::automaticCalibrationTimerCallback() lidartag_detections_array_.reset(); } -void ExtrinsicTagBasedCalibrator::publishCalibrationPoints( +void ExtrinsicTagBasedPNPCalibrator::publishCalibrationPoints( const std::vector & object_points, const std::vector & image_points) { tier4_calibration_msgs::msg::CalibrationPoints msg; diff --git a/sensor/extrinsic_tag_based_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp similarity index 95% rename from sensor/extrinsic_tag_based_calibrator/src/tag_calibrator_visualizer.cpp rename to sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index f4a0132b..fbde860b 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include #include @@ -138,6 +137,7 @@ void TagCalibratorVisualizer::drawLidartagHypotheses( assert(corners.size() == 4); visualization_msgs::msg::Marker line_strip_lcs, id_marker_lcs, center_marker_bcs; + // Note: lcs=lidar coordinate system. bcs=base coordinate system line_strip_lcs.id = i; line_strip_lcs.header.frame_id = lidar_frame_; @@ -209,6 +209,7 @@ void TagCalibratorVisualizer::drawApriltagHypotheses( const auto & center = h->getCenter3d(); int id = h->getId(); + // Note: lcs=lidar coordinate system cv::Matx31d center_lcs(center.x, center.y, center.z); center_lcs = lidar_camera_rotation_matrix * center_lcs + lidar_camera_translation_vector; @@ -316,6 +317,7 @@ void TagCalibratorVisualizer::drawCalibrationZone( p_msg.z = 0.0; }; + // Note: bcs=base coordinate system. ccs=camera coordinate system geometry_msgs::msg::Point p_msg_bcs; cv::Matx31d p_cv_ccs, p_cv_bcs; double angle = min_angle; @@ -394,7 +396,6 @@ void TagCalibratorVisualizer::drawCalibrationStatusText( text_marker.header.frame_id = base_frame_; text_marker.header.stamp = stamp; text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.lifetime = rclcpp::Duration::from_seconds(5.0); text_marker.color.r = 1.0; text_marker.color.g = 1.0; text_marker.color.b = 1.0; @@ -405,8 +406,8 @@ void TagCalibratorVisualizer::drawCalibrationStatusText( text_marker.text = "pairs=" + std::to_string(estimator.getCurrentCalibrationPairsNumber()) + "\ncoverage=" + to_string_with_precision(estimator.getCalibrationCoveragePercentage()) + - "\ncrossval_reproj_error=" + - to_string_with_precision(estimator.getCrossValidationReprojError()); + "\ncrossvalidation_reprojection_error=" + + to_string_with_precision(estimator.getCrossValidationReprojectionError()); text_marker.pose.position.x = base_lidar_translation_vector(0); text_marker.pose.position.y = base_lidar_translation_vector(1); @@ -423,8 +424,7 @@ void TagCalibratorVisualizer::drawCalibrationStatusText( const double time_since_last_observation = h->timeSinceLastObservation(stamp); const double trans_cov = h->getTransCov(); - const double rot_cov = h->getRotCov(); - const double rot_dot_cov = h->getRotDotCov(); + const double rotation_cov = h->getRotCov(); const double speed = h->getSpeed(); cv::Matx31d center_base(center.x, center.y, center.z); @@ -440,13 +440,12 @@ void TagCalibratorVisualizer::drawCalibrationStatusText( "\nt_since_last=" + to_string_with_precision(time_since_last_observation) + "/" + to_string_with_precision(max_no_observation_time_) + "\ntrans_cov=" + to_string_with_precision(trans_cov, 3) + "/" + - to_string_with_precision(lidartag_convergence_transl_, 3) + - "\nrot_cov=" + to_string_with_precision(rot_cov, 3) + "/" + - to_string_with_precision(lidartag_convergence_rot_, 3) + + to_string_with_precision(lidartag_convergence_translation_, 3) + + "\nrotation_cov=" + to_string_with_precision(rotation_cov, 3) + "/" + + to_string_with_precision(lidartag_convergence_rotation_, 3) + "\nspeed=" + to_string_with_precision(speed, 3) + "/" + - to_string_with_precision(lidartag_convergence_transl_dot_, 3) + - "\nrot_speed=" + to_string_with_precision(rot_dot_cov, 3) + "/" + - to_string_with_precision(lidartag_convergence_rot_dot_, 3); + to_string_with_precision(lidartag_convergence_translation_dot_, 3) + + to_string_with_precision(lidartag_convergence_rotation_dot_, 3); text_marker.pose.position.x = center_base(0); text_marker.pose.position.y = center_base(1); @@ -485,6 +484,7 @@ void TagCalibratorVisualizer::drawAprilTagDetections( continue; } + // Note: ccs=camera coordinate system. ics=image coordinate system visualization_msgs::msg::Marker line_strip_ics, id_marker_ics, corners_id_marker_ics; visualization_msgs::msg::Marker line_strip_ccs, /*id_marker_ccs, */ corners_id_marker_ccs; @@ -586,6 +586,7 @@ void TagCalibratorVisualizer::drawLidarTagDetections( std::vector projected_points; std::vector source_points; + // Note: lcs=lidar coordinate system. ccs=camera coordinate system. ics=image coordinate system visualization_msgs::msg::Marker line_strip_ccs, line_strip_ics, line_strip_lcs; visualization_msgs::msg::Marker id_marker_ccs, corners_id_marker_ccs, id_marker_ics, corners_id_marker_ics; @@ -839,17 +840,13 @@ void TagCalibratorVisualizer::displayObjectPoints( std::vector TagCalibratorVisualizer::get3dpoints( apriltag_msgs::msg::AprilTagDetection & detection) { - std::vector image_points, undistorted_points; + std::vector image_points; std::vector object_points; for (auto & corner : detection.corners) { image_points.push_back(cv::Point2d(corner.x, corner.y)); } - cv::undistortPoints( - image_points, undistorted_points, pinhole_camera_model_.intrinsicMatrix(), - pinhole_camera_model_.distortionCoeffs()); - if (tag_sizes_map_.count(detection.id) == 0) { return object_points; } @@ -862,19 +859,17 @@ std::vector TagCalibratorVisualizer::get3dpoints( cv::Point3d(0.5 * board_size, -0.5 * board_size, 0.0), cv::Point3d(-0.5 * board_size, -0.5 * board_size, 0.0)}; - cv::sqpnp::PoseSolver solver; - std::vector rvec_vec, tvec_vec; - solver.solve(apriltag_template_points, undistorted_points, rvec_vec, tvec_vec); + cv::Mat rvec, tvec; + + bool success = cv::solvePnP( + apriltag_template_points, image_points, pinhole_camera_model_.intrinsicMatrix(), + pinhole_camera_model_.distortionCoeffs(), rvec, tvec, false, cv::SOLVEPNP_SQPNP); - if (tvec_vec.size() == 0) { - assert(false); + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("tier4_tag_utils"), "PNP failed"); return object_points; } - assert(rvec_vec.size() == 1); - cv::Mat rvec = rvec_vec[0]; - cv::Mat tvec = tvec_vec[0]; - cv::Matx31d translation_vector = tvec; cv::Matx33d rotation_matrix; @@ -908,15 +903,15 @@ void TagCalibratorVisualizer::setMaxNoObservationTime(double time) } void TagCalibratorVisualizer::setLidartagMaxConvergenceThreshold( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { - lidartag_convergence_transl_ = transl; - lidartag_convergence_transl_dot_ = transl_dot; - lidartag_convergence_rot_ = CV_PI * rot / 180.0; - lidartag_convergence_rot_dot_ = CV_PI * rot_dot / 180.0; + lidartag_convergence_translation_ = translation; + lidartag_convergence_translation_dot_ = translation_dot; + lidartag_convergence_rotation_ = CV_PI * rotation / 180.0; + lidartag_convergence_rotation_dot_ = CV_PI * rotation_dot / 180.0; } -void TagCalibratorVisualizer::setApriltagMaxConvergenceThreshold(double transl) +void TagCalibratorVisualizer::setApriltagMaxConvergenceThreshold(double translation) { - apriltag_convergence_transl_ = transl; + apriltag_convergence_translation_ = translation; } diff --git a/sensor/extrinsic_tag_based_base_calibrator/CMakeLists.txt b/sensor/tag_based_sfm_calibrator/CMakeLists.txt similarity index 85% rename from sensor/extrinsic_tag_based_base_calibrator/CMakeLists.txt rename to sensor/tag_based_sfm_calibrator/CMakeLists.txt index 56be9b6f..7cf42b59 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/CMakeLists.txt +++ b/sensor/tag_based_sfm_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_tag_based_base_calibrator) +project(tag_based_sfm_calibrator) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) @@ -23,13 +23,13 @@ ament_export_include_directories( # COMPILE THE SOURCE #======================================================================== -ament_auto_add_executable(extrinsic_tag_based_base_calibrator +ament_auto_add_executable(tag_based_sfm_calibrator src/ceres/calibration_problem.cpp src/intrinsics_calibration/intrinsics_calibrator.cpp src/intrinsics_calibration/apriltag_calibrator.cpp src/intrinsics_calibration/chessboard_calibrator.cpp src/calibration_scene_extractor.cpp - src/extrinsic_tag_based_base_calibrator.cpp + src/tag_based_sfm_calibrator.cpp src/apriltag_detection.cpp src/apriltag_detector.cpp src/main.cpp @@ -37,7 +37,7 @@ ament_auto_add_executable(extrinsic_tag_based_base_calibrator src/visualization.cpp ) -target_link_libraries(extrinsic_tag_based_base_calibrator +target_link_libraries(tag_based_sfm_calibrator ${Boost_LIBRARIES} ${OpenCV_LIBS} apriltag::apriltag @@ -53,6 +53,7 @@ ament_export_dependencies(ament_cmake_python) ament_auto_package( INSTALL_TO_SHARE + config launch rviz ) diff --git a/sensor/tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml b/sensor/tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml new file mode 100644 index 00000000..83d869f7 --- /dev/null +++ b/sensor/tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml @@ -0,0 +1,84 @@ +/**: + ros__parameters: + + # Tag configuration + lidartag_to_apriltag_scale: 0.75 + write_debug_images: true + + waypoint_tag: + family: "tag16h5" + rows: 1 + cols: 1 + size: 0.6 + spacing: 0.2 + ids: [-1] + + ground_tag: + family: "tag36h11" + rows: 1 + cols: 1 + size: 0.22 + spacing: 0.2 + ids: [-1] + + wheel_tag: + family: "tag16h5" + rows: 2 + cols: 2 + size: 0.166 + spacing: 0.2 + ids: [-1] + + + auxiliar_tag: + family: "tag36h11" + rows: 1 + cols: 1 + size: 0.22 + spacing: 0.2 + ids: [-1] + + # Optimization configuration + ba: + optimize_intrinsics: true + share_intrinsics: true + force_shared_ground_plane: true + virtual_lidar_f: 10000.0 + + calibration_camera_optimization_weight: 0.2 + calibration_lidar_optimization_weight: 0.2 + external_camera_optimization_weight: 0.6 + + fixed_ground_plane_model: false + + + # Initial intrinsics calibration + initial_intrinsic_calibration: + board_type: "chessboard" + tangent_distortion: true + radial_distortion_coeffs: 2 + debug: true + # Apriltag parameters + tag: + family: "tag16h5" + rows: 1 + cols: 1 + size: 0.6 + spacing: 0.2 + ids: [0, 1, 2, 3, 4, 5] + # Chessboard parameters + board_cols: 8 + board_rows: 6 + + # Apriltag detector configuration + apriltag: + max_hamming: 0 + min_margin: 50.0 + max_out_of_plane_angle: 90.0 + max_reprojection_error: 10.0 + max_homography_error: 0.5 + quad_decimate: 1.0 + quad_sigma: 0.0 + nthreads: 12 # cSpell:ignore nthreads + debug: false + refine_edges: true diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detection.hpp similarity index 84% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detection.hpp index ad9680ba..95caca41 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detection.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTION_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTION_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ -#include #include #include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { struct LidartagDetection @@ -35,6 +35,7 @@ struct LidartagDetection static LidartagDetection fromLidartagDetectionMsg( const lidartag_msgs::msg::LidarTagDetection & msg, double scale_factor); void computeObjectCorners(); + void computeTemplateCorners(double new_size); void computeTemplateCorners(); std::string family = ""; @@ -51,8 +52,8 @@ struct ApriltagDetection : public LidartagDetection const apriltag_msgs::msg::AprilTagDetection & msg, const IntrinsicParameters & intrinsics, double size); double computePose(const IntrinsicParameters & intrinsics); - double computeReprojError(const IntrinsicParameters & intrinsics) const; - double computeReprojError(double cx, double cy, double fx, double fy) const; + double computeReprojectionError(const IntrinsicParameters & intrinsics) const; + double computeReprojectionError(double cx, double cy, double fx, double fy) const; double detectionDiagonalRatio() const; std::vector image_corners; @@ -99,6 +100,6 @@ using ApriltagGridDetections = std::vector; using GroupedApriltagDetections = std::map; using GroupedApriltagGridDetections = std::map; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTION_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detector.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detector.hpp similarity index 78% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detector.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detector.hpp index 3b079ab5..136de60b 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detector.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detector.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTOR_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTOR_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTOR_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTOR_HPP_ -#include -#include #include +#include +#include #include @@ -25,7 +25,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { class ApriltagDetector @@ -50,8 +50,8 @@ class ApriltagDetector void setIntrinsics(double fx, double fy, double cx, double cy); /*! - * Detetect all the apriltags in an image filtering by hamming distance and detection margin - * If the tag size and intrinsics are known, it also estiamtes the 3d pose of the tag + * Detect all the apriltags in an image filtering by hamming distance and detection margin + * If the tag size and intrinsics are known, it also estimates the 3d pose of the tag * @param[in] img The image to obtain detections from * @return a vector of ApriltagDetection found in the img */ @@ -73,6 +73,6 @@ class ApriltagDetector static std::unordered_map tag_destroy_fn_map; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTOR_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_scene_extractor.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_scene_extractor.hpp similarity index 71% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_scene_extractor.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_scene_extractor.hpp index dd3ae504..2caa1301 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_scene_extractor.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_scene_extractor.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { class CalibrationSceneExtractor @@ -57,7 +57,6 @@ class CalibrationSceneExtractor void setExternalCameraIntrinsics(IntrinsicParameters & parameters); /*! - * TODOKL update * Process a scene, rectifying the images in the scene and obtaining the detections and 3d poses * @param[in] detections The lidartag detections * @param[in] external_camera_image_names The vector containing the images of the scene @@ -70,20 +69,6 @@ class CalibrationSceneExtractor const std::unordered_map & camera_detections_map, const std::vector & calibration_lidar_frames, const std::vector & calibration_camera_frames, - const std::string & main_sensor_frame, - const std::vector & external_camera_image_names); - - /*! - * Process a scene, rectifying the images in the scene and obtaining the detections and 3d poses - * @param[in] calibration_sensor_image_name The single image of the scene corresponding to the - * calibration camera - * @param[in] external_camera_image_names The vector containing the images of the scene - * corresponding to the external camera - */ - CalibrationScene processScene( - const std::unordered_map & calibration_cameras_image_map, - const std::vector & calibration_camera_frames, - const std::string & main_sensor_frame, const std::vector & external_camera_image_names); protected: @@ -109,6 +94,6 @@ class CalibrationSceneExtractor bool debug_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_types.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_types.hpp similarity index 85% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_types.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_types.hpp index 4e7079a2..2c4dfc7d 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_types.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_types.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_TYPES_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_TYPES_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ -#include -#include #include #include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { struct CalibrationData @@ -35,7 +35,7 @@ struct CalibrationData using Ptr = std::shared_ptr; static constexpr int POSE_OPT_DIM = 7; - static constexpr int SHRD_GROUND_TAG_POSE_DIM = 5; + static constexpr int SHRD_GROUND_TAG_POSE_DIM = 5; // cSpell:ignore SHRD static constexpr int INDEP_GROUND_TAG_POSE_DIM = 3; static constexpr int INTRINSICS_DIM = 6; @@ -79,6 +79,7 @@ struct CalibrationData std::shared_ptr initial_right_wheel_tag_pose; std::map> optimized_sensor_poses_map; + std::map optimized_sensor_residuals_map; std::map>> optimized_camera_intrinsics_map; @@ -88,6 +89,6 @@ struct CalibrationData std::shared_ptr optimized_right_wheel_tag_pose; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_TYPES_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/calibration_problem.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/calibration_problem.hpp similarity index 92% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/calibration_problem.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/calibration_problem.hpp index 31f41e4b..85ef097b 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/calibration_problem.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/calibration_problem.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ #include -#include -#include +#include +#include #include #include @@ -26,14 +26,15 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { class CalibrationProblem { public: static constexpr int POSE_OPT_DIM = CalibrationData::POSE_OPT_DIM; - static constexpr int SHRD_GROUND_TAG_POSE_DIM = CalibrationData::SHRD_GROUND_TAG_POSE_DIM; + static constexpr int SHRD_GROUND_TAG_POSE_DIM = + CalibrationData::SHRD_GROUND_TAG_POSE_DIM; // cSpell:ignore SHRD static constexpr int INDEP_GROUND_TAG_POSE_DIM = CalibrationData::INDEP_GROUND_TAG_POSE_DIM; static constexpr int INTRINSICS_DIM = CalibrationData::INTRINSICS_DIM; @@ -229,10 +230,10 @@ class CalibrationProblem UID left_wheel_tag_uid_; UID right_wheel_tag_uid_; - bool optimize_intrinsics_; - bool share_intrinsics_; - bool force_shared_ground_plane_; - bool force_fixed_ground_plane_; + bool optimize_intrinsics_{true}; + bool share_intrinsics_{true}; + bool force_shared_ground_plane_{true}; + bool force_fixed_ground_plane_{false}; cv::Affine3d fixed_ground_pose_; double calibration_camera_optimization_weight_; @@ -252,6 +253,6 @@ class CalibrationProblem std::array shared_intrinsics_opt; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp similarity index 85% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp index b667bd60..5f183bc6 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ -#include -#include -#include #include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { struct CameraResidual : public SensorResidual @@ -36,14 +36,15 @@ struct CameraResidual : public SensorResidual const UID & camera_uid, const IntrinsicParameters & intrinsics, const ApriltagDetection & detection, const std::array & fixed_camera_pose_inv, - const std::array & fixed_tag_rot_z, - bool fix_camera_pose, bool fix_tag_rot_z, bool optimize_intrinsics, bool is_ground_tag) + const std::array & + fixed_tag_rotation_z, // cSpell:ignore SHRD + bool fix_camera_pose, bool fix_tag_rotation_z, bool optimize_intrinsics, bool is_ground_tag) : camera_uid_(camera_uid), intrinsics_(intrinsics), detection_(detection), - fixed_tag_rot_z_(fixed_tag_rot_z), + fixed_tag_rotation_z_(fixed_tag_rotation_z), fix_camera_pose_(fix_camera_pose), - fix_tag_rot_z_(fix_tag_rot_z), + fix_tag_rotation_z_(fix_tag_rotation_z), optimize_intrinsics_(optimize_intrinsics), is_ground_tag_(is_ground_tag) { @@ -72,7 +73,7 @@ struct CameraResidual : public SensorResidual } /*! - * Auxiliar method to construct a 3d rotation matrix representing a 2d rotatin matrix + * Auxiliar method to construct a 3d rotation matrix representing a 2d rotation matrix * @param[in] yaw the rotation angle in the Z axis */ template @@ -121,6 +122,7 @@ struct CameraResidual : public SensorResidual {T(detection_.template_corners[3].x), T(detection_.template_corners[3].y), T(detection_.template_corners[3].z)}}; + // Note: wcs=world coordinate system. ccs=camera coordinate system Vector3 corners_wcs[NUM_CORNERS]; Vector3 corners_ccs[NUM_CORNERS]; @@ -195,28 +197,28 @@ struct CameraResidual : public SensorResidual } // Compute the reprojection error residuals - auto compute_reproj_error_point = [&]( - auto & predicted_ccs, auto observed_ics, auto * residuals) { - const T & cx = camera_intrinsics_map(INTRINSICS_CX_INDEX); - const T & cy = camera_intrinsics_map(INTRINSICS_CY_INDEX); - const T & fx = camera_intrinsics_map(INTRINSICS_FX_INDEX); - const T & fy = camera_intrinsics_map(INTRINSICS_FY_INDEX); - const T & k1 = camera_intrinsics_map(INTRINSICS_K1_INDEX); - const T & k2 = camera_intrinsics_map(INTRINSICS_K2_INDEX); - - const T xp = predicted_ccs.x() / predicted_ccs.z(); - const T yp = predicted_ccs.y() / predicted_ccs.z(); - const T r2 = xp * xp + yp * yp; - const T d = 1.0 + r2 * (k1 + k2 * r2); - const T predicted_ics_x = cx + fx * d * xp; - const T predicted_ics_y = cy + fy * d * yp; - - residuals[0] = predicted_ics_x - observed_ics.x(); - residuals[1] = predicted_ics_y - observed_ics.y(); - }; + auto compute_reprojection_error_point = + [&](auto & predicted_ccs, auto observed_ics, auto * residuals) { + const T & cx = camera_intrinsics_map(INTRINSICS_CX_INDEX); + const T & cy = camera_intrinsics_map(INTRINSICS_CY_INDEX); + const T & fx = camera_intrinsics_map(INTRINSICS_FX_INDEX); + const T & fy = camera_intrinsics_map(INTRINSICS_FY_INDEX); + const T & k1 = camera_intrinsics_map(INTRINSICS_K1_INDEX); + const T & k2 = camera_intrinsics_map(INTRINSICS_K2_INDEX); + + const T xp = predicted_ccs.x() / predicted_ccs.z(); + const T yp = predicted_ccs.y() / predicted_ccs.z(); + const T r2 = xp * xp + yp * yp; + const T d = 1.0 + r2 * (k1 + k2 * r2); + const T predicted_ics_x = cx + fx * d * xp; + const T predicted_ics_y = cy + fy * d * yp; + + residuals[0] = predicted_ics_x - observed_ics.x(); + residuals[1] = predicted_ics_y - observed_ics.y(); + }; for (int i = 0; i < NUM_CORNERS; i++) { - compute_reproj_error_point(corners_ccs[i], observed_corners_[i], residuals + 2 * i); + compute_reprojection_error_point(corners_ccs[i], observed_corners_[i], residuals + 2 * i); } return true; @@ -235,15 +237,15 @@ struct CameraResidual : public SensorResidual */ template bool operator()( - const T * const camera_pose_inv, const T * const camera_intrinsics, const T * const tag_rot_z, - const T * const tag_pose_2d, T * residuals) const + const T * const camera_pose_inv, const T * const camera_intrinsics, + const T * const tag_rotation_z, const T * const tag_pose_2d, T * residuals) const { assert(fix_camera_pose_ == false); assert(optimize_intrinsics_ == true); assert(is_ground_tag_ == true); return impl( - camera_pose_inv, camera_intrinsics, static_cast(nullptr), tag_rot_z, tag_pose_2d, + camera_pose_inv, camera_intrinsics, static_cast(nullptr), tag_rotation_z, tag_pose_2d, residuals); return false; @@ -252,7 +254,7 @@ struct CameraResidual : public SensorResidual /*! * The cost function wrapper for the following casesL * - the tag is in the ground and the intrinsics are not optimized - * -the tag is in the ground and the intrinsics are optimized, but the rot_z component is + * -the tag is in the ground and the intrinsics are optimized, but the rotation_z component is * fixed * - the camera is not fixed and the intrinsics are optimized * @param[in] arg1 The pose from the camera to the origin @@ -265,7 +267,7 @@ struct CameraResidual : public SensorResidual bool operator()( const T * const arg1, const T * const arg2, const T * const arg3, T * residuals) const { - if (is_ground_tag_ && !fix_tag_rot_z_) { + if (is_ground_tag_ && !fix_tag_rotation_z_) { // Case where the tag is in the ground and the intrinsics are not optimized assert(fix_camera_pose_ == false); assert(optimize_intrinsics_ == false); @@ -274,16 +276,18 @@ struct CameraResidual : public SensorResidual T(1.0) * fy_, T(0.0), T(0.0)}; return impl(arg1, intrinsics.data(), static_cast(nullptr), arg2, arg3, residuals); - } else if (is_ground_tag_ && fix_tag_rot_z_) { + } else if (is_ground_tag_ && fix_tag_rotation_z_) { // Case where the tag is in the ground and the intrinsics are not optimized assert(fix_camera_pose_ == false); assert(optimize_intrinsics_ == true); - std::array fixed_tag_rot_z{ - T(1.0) * fixed_tag_rot_z_[0], T(1.0) * fixed_tag_rot_z_[1], T(1.0) * fixed_tag_rot_z_[2], - T(1.0) * fixed_tag_rot_z_[3], T(1.0) * fixed_tag_rot_z_[4]}; + std::array fixed_tag_rotation_z{ + T(1.0) * fixed_tag_rotation_z_[0], T(1.0) * fixed_tag_rotation_z_[1], + T(1.0) * fixed_tag_rotation_z_[2], T(1.0) * fixed_tag_rotation_z_[3], + T(1.0) * fixed_tag_rotation_z_[4]}; - return impl(arg1, arg2, static_cast(nullptr), fixed_tag_rot_z.data(), arg3, residuals); + return impl( + arg1, arg2, static_cast(nullptr), fixed_tag_rotation_z.data(), arg3, residuals); } else { // Case where the camera is not fixed and the intrinsics are optimized assert(fix_camera_pose_ == false); @@ -322,20 +326,21 @@ struct CameraResidual : public SensorResidual } else { assert(fix_camera_pose_ == false); assert(optimize_intrinsics_ == false); - assert(fix_tag_rot_z_ == true); + assert(fix_tag_rotation_z_ == true); const T * const camera_pose_inv = arg1; const T * const tag_pose = arg2; - std::array fixed_tag_rot_z{ - T(1.0) * fixed_tag_rot_z_[0], T(1.0) * fixed_tag_rot_z_[1], T(1.0) * fixed_tag_rot_z_[2], - T(1.0) * fixed_tag_rot_z_[3], T(1.0) * fixed_tag_rot_z_[4]}; + std::array fixed_tag_rotation_z{ + T(1.0) * fixed_tag_rotation_z_[0], T(1.0) * fixed_tag_rotation_z_[1], + T(1.0) * fixed_tag_rotation_z_[2], T(1.0) * fixed_tag_rotation_z_[3], + T(1.0) * fixed_tag_rotation_z_[4]}; std::array intrinsics{T(1.0) * cx_, T(1.0) * cy_, T(1.0) * fx_, T(1.0) * fy_, T(0.0), T(0.0)}; return impl( - camera_pose_inv, intrinsics.data(), static_cast(nullptr), fixed_tag_rot_z.data(), + camera_pose_inv, intrinsics.data(), static_cast(nullptr), fixed_tag_rotation_z.data(), tag_pose, residuals); } } @@ -377,11 +382,11 @@ struct CameraResidual : public SensorResidual std::array & fixed_camera_pose_inv, bool fix_camera_pose, bool optimize_intrinsics) { - std::array null_tag_rot_z; + std::array null_tag_rotation_z; auto f = new CameraResidual( - camera_uid, intrinsics, detection, fixed_camera_pose_inv, null_tag_rot_z, fix_camera_pose, - false, optimize_intrinsics, false); + camera_uid, intrinsics, detection, fixed_camera_pose_inv, null_tag_rotation_z, + fix_camera_pose, false, optimize_intrinsics, false); if (fix_camera_pose && !optimize_intrinsics) { return (new ceres::AutoDiffCostFunction< @@ -428,14 +433,14 @@ struct CameraResidual : public SensorResidual const UID & camera_uid, const IntrinsicParameters & intrinsics, const ApriltagDetection & detection, std::array & fixed_camera_pose_inv, - const std::array & fixed_tag_rot_z, - bool fix_camera_pose, bool fix_tag_rot_z, bool optimize_intrinsics) + const std::array & fixed_tag_rotation_z, + bool fix_camera_pose, bool fix_tag_rotation_z, bool optimize_intrinsics) { auto f = new CameraResidual( - camera_uid, intrinsics, detection, fixed_camera_pose_inv, fixed_tag_rot_z, fix_camera_pose, - fix_tag_rot_z, optimize_intrinsics, true); + camera_uid, intrinsics, detection, fixed_camera_pose_inv, fixed_tag_rotation_z, + fix_camera_pose, fix_tag_rotation_z, optimize_intrinsics, true); - if (!fix_tag_rot_z) { + if (!fix_tag_rotation_z) { if (optimize_intrinsics) { return (new ceres::AutoDiffCostFunction< CameraResidual, @@ -487,13 +492,13 @@ struct CameraResidual : public SensorResidual ApriltagDetection detection_; Eigen::Vector4d fixed_camera_rotation_inv_; Eigen::Vector3d fixed_camera_translation_inv_; - std::array fixed_tag_rot_z_; + std::array fixed_tag_rotation_z_; bool fix_camera_pose_; - bool fix_tag_rot_z_; + bool fix_tag_rotation_z_; bool optimize_intrinsics_; bool is_ground_tag_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp similarity index 89% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp index 1d229a09..389aee6a 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ -#include -#include -#include #include #include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { struct LidarResidual : public SensorResidual @@ -125,9 +125,9 @@ struct LidarResidual : public SensorResidual {T(detection_.template_corners[3].x), T(detection_.template_corners[3].y), T(detection_.template_corners[3].z)}}; - Vector3 corners_wcs[NUM_CORNERS]; - Vector3 corners_lcs[NUM_CORNERS]; - Vector3 corners_lrcs[NUM_CORNERS]; + Vector3 corners_wcs[NUM_CORNERS]; // wcs=world coordinate system + Vector3 corners_lcs[NUM_CORNERS]; // lcs=lidar coordinate system + Vector3 corners_lrcs[NUM_CORNERS]; // cSpell:ignore lrcs auto transform_corners = [](auto & quaternion, auto & translation, auto & input_corners, auto & output_corners) { @@ -190,21 +190,21 @@ struct LidarResidual : public SensorResidual rotate_corners(tag_centric_rotation, corners_lcs, corners_lrcs); // Compute the reprojection error residuals - auto compute_reproj_error_point = [&]( - auto & predicted_ccs, auto observed_ics, auto * residuals) { - const T f = T(virtual_f_); + auto compute_reprojection_error_point = + [&](auto & predicted_ccs, auto observed_ics, auto * residuals) { + const T f = T(virtual_f_); - const T xp = predicted_ccs.x() / predicted_ccs.z(); - const T yp = predicted_ccs.y() / predicted_ccs.z(); - const T predicted_ics_x = f * xp; - const T predicted_ics_y = f * yp; + const T xp = predicted_ccs.x() / predicted_ccs.z(); + const T yp = predicted_ccs.y() / predicted_ccs.z(); + const T predicted_ics_x = f * xp; + const T predicted_ics_y = f * yp; - residuals[0] = predicted_ics_x - observed_ics.x(); - residuals[1] = predicted_ics_y - observed_ics.y(); - }; + residuals[0] = predicted_ics_x - observed_ics.x(); + residuals[1] = predicted_ics_y - observed_ics.y(); + }; for (int i = 0; i < NUM_CORNERS; i++) { - compute_reproj_error_point(corners_lrcs[i], observed_corners_[i], residuals + 2 * i); + compute_reprojection_error_point(corners_lrcs[i], observed_corners_[i], residuals + 2 * i); } return true; @@ -289,6 +289,6 @@ struct LidarResidual : public SensorResidual Eigen::Matrix3d tag_centric_rotation_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/sensor_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/sensor_residual.hpp similarity index 82% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/sensor_residual.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/sensor_residual.hpp index 0c15b9a2..4c4942f1 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/sensor_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/sensor_residual.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ -#include -#include +#include +#include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { struct SensorResidual @@ -39,7 +39,8 @@ struct SensorResidual using Matrix3 = Eigen::Matrix; static constexpr int POSE_OPT_DIM = CalibrationData::POSE_OPT_DIM; - static constexpr int SHRD_GROUND_TAG_POSE_DIM = CalibrationData::SHRD_GROUND_TAG_POSE_DIM; + static constexpr int SHRD_GROUND_TAG_POSE_DIM = + CalibrationData::SHRD_GROUND_TAG_POSE_DIM; // cSpell:ignore SHRD static constexpr int INDEP_GROUND_TAG_POSE_DIM = CalibrationData::INDEP_GROUND_TAG_POSE_DIM; static constexpr int INTRINSICS_DIM = CalibrationData::INTRINSICS_DIM; @@ -67,6 +68,6 @@ struct SensorResidual static constexpr int NUM_CORNERS = 4; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/apriltag_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp similarity index 66% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/apriltag_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp index 0aaa47ad..c2729369 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/apriltag_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ -#include -#include -#include -#include #include +#include +#include +#include +#include #include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { class ApriltagBasedCalibrator : public IntrinsicsCalibrator @@ -49,6 +49,6 @@ class ApriltagBasedCalibrator : public IntrinsicsCalibrator std::unordered_map> filtered_image_file_name_to_calibration_id_map_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/chessboard_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp similarity index 65% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/chessboard_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp index 6707213a..c29591ff 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/chessboard_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ -#include -#include -#include #include +#include +#include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { class ChessboardBasedCalibrator : public IntrinsicsCalibrator @@ -49,6 +49,6 @@ class ChessboardBasedCalibrator : public IntrinsicsCalibrator int cols_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp similarity index 74% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp index e73e580a..db733674 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ -#include -#include #include +#include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { class IntrinsicsCalibrator @@ -46,7 +46,7 @@ class IntrinsicsCalibrator void setCalibrationImageFiles(const std::vector & image_file_names); /*! - * Calibrate the camera intrinsicss using images containing apriltags + * Calibrate the camera intrinsics using images containing apriltags * @param[out] intrinsics the calibrated intrinsics * @returns whether or not the service callback succeeded */ @@ -69,6 +69,6 @@ class IntrinsicsCalibrator bool debug_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/math.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/math.hpp similarity index 79% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/math.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/math.hpp index af1f5968..4dc73a88 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/math.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/math.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ #include -#include -#include -#include +#include +#include +#include #include #include +#include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { /* @@ -47,22 +48,19 @@ std::array tagPoseToCorners(const cv::Affine3d & pose, double size * Computes a pose to the ground plane by fitting a plane to a set of points and then projecting the * origin to said plane * @param[in] points the points used to calculate the ground pose from - * @param[out] ground_pose the ground pose - * @returns whether or not the algorithm succeeded + * @returns the ground pose */ -bool computeGroundPlane(const std::vector & points, cv::Affine3d & ground_pose); +std::optional computeGroundPlane(const std::vector & points); /*! * Computes a pose to the ground plane by fitting a plane to a set of tag poses and then projecting * the origin to said plane * @param[in] poses the tag poses to use to compute the ground plane * @param[in] tag_size the sized of the tags - * @param[out] ground_pose the ground pose - * @returns whether or not the algorithm succeeded + * @returns the ground pose */ -bool computeGroundPlane( - const std::map> & poses, double tag_size, - cv::Affine3d & ground_pose); +std::optional computeGroundPlane( + const std::map> & poses, double tag_size); /*! * Computes the base link pose by projecting the mid point between the left and right wheel poses @@ -101,6 +99,6 @@ void estimateInitialPoses( CalibrationData & data, const UID & main_sensor_uid, UID & left_wheel_uid, UID & right_wheel_uid, int max_depth = 10, double min_allowed_diagonal_ratio = 0.4); -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/scene_types.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/scene_types.hpp similarity index 78% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/scene_types.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/scene_types.hpp index 45d381a6..c2153d70 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/scene_types.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/scene_types.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__SCENE_TYPES_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__SCENE_TYPES_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ -#include -#include +#include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { struct ExternalCameraFrame @@ -58,6 +58,6 @@ struct CalibrationScene std::vector external_camera_frames; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__SCENE_TYPES_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/serialization.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/serialization.hpp similarity index 66% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/serialization.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/serialization.hpp index 0972a4ab..53cef818 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/serialization.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/serialization.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__SERIALIZATION_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__SERIALIZATION_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ -#include -#include -#include #include +#include +#include +#include #include @@ -44,9 +44,8 @@ namespace serialization { template -void save(Archive & ar, const cv::Mat & m, const unsigned int version) +void save(Archive & ar, const cv::Mat & m, [[maybe_unused]] const unsigned int version) { - (void)version; size_t elem_size = m.elemSize(); size_t elem_type = m.type(); @@ -60,9 +59,8 @@ void save(Archive & ar, const cv::Mat & m, const unsigned int version) } template -void load(Archive & ar, cv::Mat & m, const unsigned int version) +void load(Archive & ar, cv::Mat & m, [[maybe_unused]] const unsigned int version) { - (void)version; int cols, rows; size_t elem_size, elem_type; @@ -78,9 +76,8 @@ void load(Archive & ar, cv::Mat & m, const unsigned int version) } template -void save(Archive & ar, const cv::Mat_<_Tp> & m, const unsigned int version) +void save(Archive & ar, const cv::Mat_<_Tp> & m, [[maybe_unused]] const unsigned int version) { - (void)version; size_t elem_size = m.elemSize(); ar & m.cols; @@ -92,9 +89,8 @@ void save(Archive & ar, const cv::Mat_<_Tp> & m, const unsigned int version) } template -void load(Archive & ar, cv::Mat_<_Tp> & m, const unsigned int version) +void load(Archive & ar, cv::Mat_<_Tp> & m, [[maybe_unused]] const unsigned int version) { - (void)version; int cols, rows; size_t elem_size; @@ -109,53 +105,48 @@ void load(Archive & ar, cv::Mat_<_Tp> & m, const unsigned int version) } template -inline void serialize(Archive & ar, cv::Size_<_Tp> & size, const unsigned int version) +inline void serialize( + Archive & ar, cv::Size_<_Tp> & size, [[maybe_unused]] const unsigned int version) { - (void)version; - ar & size.height; ar & size.width; } template -inline void serialize(Archive & ar, cv::Point_<_Tp> & p, const unsigned int version) +inline void serialize( + Archive & ar, cv::Point_<_Tp> & p, [[maybe_unused]] const unsigned int version) { - (void)version; - ar & p.x; ar & p.y; } template -inline void serialize(Archive & ar, cv::Point3_<_Tp> & p, const unsigned int version) +inline void serialize( + Archive & ar, cv::Point3_<_Tp> & p, [[maybe_unused]] const unsigned int version) { - (void)version; - ar & p.x; ar & p.y; ar & p.z; } template -inline void serialize(Archive & ar, cv::Matx<_Tp, _m, _n> & m, const unsigned int version) +inline void serialize( + Archive & ar, cv::Matx<_Tp, _m, _n> & m, [[maybe_unused]] const unsigned int version) { - (void)version; ar & m.val; } template -void serialize(Archive & ar, cv::Affine3<_Tp> & pose, const unsigned int version) +void serialize(Archive & ar, cv::Affine3<_Tp> & pose, [[maybe_unused]] const unsigned int version) { - (void)version; ar & pose.matrix; } template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::ApriltagDetection & detection, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::ApriltagDetection & detection, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & detection.family; ar & detection.id; ar & detection.image_corners; @@ -168,10 +159,9 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::ApriltagGridDetection & detection, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::ApriltagGridDetection & detection, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & detection.cols; ar & detection.rows; ar & detection.sub_detections; @@ -187,10 +177,9 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::LidartagDetection & detection, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::LidartagDetection & detection, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & detection.id; ar & detection.object_corners; ar & detection.template_corners; @@ -200,21 +189,18 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::ExternalCameraFrame & frame, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::ExternalCameraFrame & frame, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & frame.image_filename; ar & frame.detections; } template void serialize( - Archive & ar, - extrinsic_tag_based_base_calibrator::SingleCalibrationLidarDetections & lidar_detections, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::SingleCalibrationLidarDetections & lidar_detections, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & lidar_detections.calibration_frame; ar & lidar_detections.calibration_lidar_id; ar & lidar_detections.detections; @@ -222,11 +208,9 @@ void serialize( template void serialize( - Archive & ar, - extrinsic_tag_based_base_calibrator::SingleCalibrationCameraDetections & camera_detections, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::SingleCalibrationCameraDetections & camera_detections, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & camera_detections.calibration_frame; ar & camera_detections.calibration_camera_id; ar & camera_detections.grouped_detections; @@ -235,10 +219,9 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::CalibrationScene & scene, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::CalibrationScene & scene, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & scene.calibration_cameras_detections; ar & scene.calibration_lidars_detections; ar & scene.external_camera_frames; @@ -246,9 +229,8 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::UID & uid, const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::UID & uid, [[maybe_unused]] const unsigned int version) { - (void)version; ar & uid.type; ar & uid.sensor_type; ar & uid.tag_type; @@ -261,10 +243,9 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::IntrinsicParameters & intrinsics, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::IntrinsicParameters & intrinsics, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & intrinsics.size; ar & intrinsics.camera_matrix; ar & intrinsics.dist_coeffs; @@ -273,10 +254,9 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::CalibrationData & data, - const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::CalibrationData & data, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & data.scenes; ar & data.main_calibration_sensor_uid; ar & data.uid_connections_map; @@ -300,9 +280,10 @@ void serialize( } template -void serialize(Archive & ar, sensor_msgs::msg::CompressedImage & msg, const unsigned int version) +void serialize( + Archive & ar, sensor_msgs::msg::CompressedImage & msg, + [[maybe_unused]] const unsigned int version) { - (void)version; ar & msg.format; ar & msg.data; } @@ -310,4 +291,4 @@ void serialize(Archive & ar, sensor_msgs::msg::CompressedImage & msg, const unsi } // namespace serialization } // namespace boost -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__SERIALIZATION_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp similarity index 87% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp index 3748bc8c..d6915859 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,37 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__EXTRINSIC_TAG_BASED_BASE_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__EXTRINSIC_TAG_BASED_BASE_CALIBRATOR_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__TAG_BASED_SFM_CALIBRATOR_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__TAG_BASED_SFM_CALIBRATOR_HPP_ -#include -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include #include #include #include #include #include +#include #include #include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - #include #include #include @@ -56,7 +49,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node @@ -68,13 +61,11 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node /*! * Callback to calibrate the base_link to sensor kit using the calibration api * @param request the calibration request - * @param response the calibration reponse + * @param response the calibration response */ void calibrationRequestCallback( const std::shared_ptr request, - const std::shared_ptr response, - const std::string & sensor_kit_frame, const std::string & calibration_sensor_parent_frame, - const std::string & calibration_sensor_frame); + const std::shared_ptr response); /*! * Callback method for the image of the calibration cameras @@ -118,6 +109,17 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node */ std_msgs::msg::ColorRGBA getNextColor(); + /*! + * @return the UID corresponding to the main sensor + */ + UID getMainSensorUID() const; + + /*! + * @param pose the pose as an opencv isometry + * @return the pose as an eigen isometry + */ + Eigen::Isometry3d cvToEigenPose(const cv::Affine3d & pose); + /*! * Attempts to add external camera images to the scene * @param request A vector of files to be added as external images @@ -138,7 +140,7 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - // Intrinsics realated services + // Intrinsics related services /*! * Attempts to load the external camera intrinsics from a file * @param request Vector containing the intrinsics path @@ -160,7 +162,7 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node std::shared_ptr response); /*! - * Attempts to caibrate the external camera intrinsics from a set of images containing tags + * Attempts to calibrate the external camera intrinsics from a set of images containing tags * @param request Vector containing the path to the images to use for intrinsic calibration * @param response whether or not the service callback succeeded * @returns whether or not the service callback succeeded @@ -181,7 +183,7 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node std::shared_ptr response); /*! - * Calibrate the base link by estimating the 3d pose of all the tags usin BA and then setting the + * Calibrate the base link by estimating the 3d pose of all the tags using BA and then setting the * base_link ad the midpoint between the wheel tags * @param request Empty request * @param response Empty response @@ -227,12 +229,11 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node lidartag_detections_sub_map_; rclcpp::Publisher::SharedPtr markers_pub_; + rclcpp::Publisher::SharedPtr raw_detections_markers_pub_; // Calibration API related services - std::map srv_callback_groups_map_; - std::map< - std::string, rclcpp::Service::SharedPtr> - calibration_api_srv_map_; + rclcpp::CallbackGroup::SharedPtr calibration_api_srv_group_; + rclcpp::Service::SharedPtr calibration_api_srv_; // Scene related services rclcpp::Service::SharedPtr @@ -240,7 +241,7 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node rclcpp::Service::SharedPtr add_calibration_sensor_detections_to_scene_srv_; - // Intrinsics realated services + // Intrinsics related services rclcpp::Service::SharedPtr load_external_camera_intrinsics_srv_; rclcpp::Service::SharedPtr @@ -258,10 +259,8 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node // Calibration API parameters and variables std::string base_frame_; - std::unordered_map sensor_kit_frame_map_; - std::unordered_map calibration_sensor_parent_frame_map_; - std::unordered_map calibration_service_names_map_; - + bool publish_tfs_; + bool write_debug_images_; std::mutex mutex_; tf2_ros::StaticTransformBroadcaster tf_broadcaster_; std::shared_ptr tf_buffer_; @@ -315,10 +314,6 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node double calibration_camera_optimization_weight_; double calibration_lidar_optimization_weight_; double external_camera_optimization_weight_; - double ba_fixed_ground_plane_model_a_; - double ba_fixed_ground_plane_model_b_; - double ba_fixed_ground_plane_model_c_; - double ba_fixed_ground_plane_model_d_; double virtual_lidar_f_; // Detections @@ -349,6 +344,6 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node std::vector precomputed_colors_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__EXTRINSIC_TAG_BASED_BASE_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__TAG_BASED_SFM_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/types.hpp similarity index 93% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/types.hpp index 5ddf6198..fbadb1a2 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/types.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__TYPES_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__TYPES_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ #include #include @@ -26,14 +26,14 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { struct ApriltagDetectorParameters { int max_hamming; double max_out_of_plane_angle; - double max_reproj_error; + double max_reprojection_error; double min_margin; double max_homography_error; double quad_decimate; @@ -231,15 +231,15 @@ struct UID // types }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator namespace std { template <> -struct hash +struct hash { - std::size_t operator()(const extrinsic_tag_based_base_calibrator::UID & uid) const + std::size_t operator()(const tag_based_sfm_calibrator::UID & uid) const { return hash()(uid.toString()); } @@ -247,4 +247,4 @@ struct hash } // namespace std -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__TYPES_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/visualization.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/visualization.hpp similarity index 89% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/visualization.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/visualization.hpp index a4ee4f95..5377689e 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/visualization.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/visualization.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__VISUALIZATION_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__VISUALIZATION_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ -#include -#include #include #include +#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { /*! @@ -46,7 +46,7 @@ void addTextMarker( * Adds markers representing a tag * @param[out] markers the output markers vector * @param[in] tag_name the text to be displayed in the marker - * @param[in] parsm the parameters of the tag + * @param[in] params the parameters of the tag * @param[in] color the color of the text * @param[in] pose the pose of the tag * @param[in] base_marker the marker to use as a template @@ -115,7 +115,7 @@ void drawDetection(cv::Mat & img, const ApriltagDetection & detection, cv::Scala /*! * Draws the detections in an image * @param[inout] img the image to draw on - * @param[in] detection the detectin to draw + * @param[in] detection the detection to draw * @param[in] camera_to_tag_pose pose from the camera to the tag * @param[in] intrinsics the array containing the intrinsics */ @@ -132,6 +132,6 @@ void drawAxes( void drawAxes( cv::Mat & img, const ApriltagDetection & detection, const IntrinsicParameters & intrinsics); -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__VISUALIZATION_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/launch/apriltag_detector.launch.py b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.py similarity index 91% rename from sensor/extrinsic_tag_based_base_calibrator/launch/apriltag_detector.launch.py rename to sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.py index c0c19bc5..646e1525 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/launch/apriltag_detector.launch.py +++ b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. +# Copyright 2024 Tier IV, Inc. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -36,12 +36,11 @@ def create_parameter_dict(*args): ) families = yaml.safe_load(LaunchConfiguration("families").perform(context)) - sizes = yaml.safe_load(LaunchConfiguration("sizes").perform(context)) nodes = [] - for family, size in zip(families, sizes): - param_dict = {"family": family, "size": size, **common_param_dict} + for family in families: + param_dict = {"family": family, **common_param_dict} composable_node = ComposableNode( name=f"apriltag_{family}", @@ -88,7 +87,6 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("apriltag_detections_topic", "apriltag/detection_array") add_launch_arg("image_transport", "raw") add_launch_arg("families", "[16h5]") - add_launch_arg("sizes", "[0.162]") add_launch_arg("max_hamming", "0") add_launch_arg("z_up", "True") diff --git a/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml new file mode 100644 index 00000000..5a5a9708 --- /dev/null +++ b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml b/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml new file mode 100644 index 00000000..b7b10c2d --- /dev/null +++ b/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml @@ -0,0 +1,313 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml b/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml new file mode 100644 index 00000000..02c0f144 --- /dev/null +++ b/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/extrinsic_tag_based_base_calibrator/package.xml b/sensor/tag_based_sfm_calibrator/package.xml similarity index 86% rename from sensor/extrinsic_tag_based_base_calibrator/package.xml rename to sensor/tag_based_sfm_calibrator/package.xml index 645dcd89..39ab36fc 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/package.xml +++ b/sensor/tag_based_sfm_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_tag_based_base_calibrator + tag_based_sfm_calibrator 0.0.1 - The extrinsic_tag_based_base_calibrator package + The tag_based_sfm_calibrator package Kenzo Lobos Tsunekawa BSD @@ -15,7 +15,6 @@ apriltag apriltag_msgs - backward_ros cv_bridge eigen geometry_msgs @@ -38,12 +37,12 @@ tf2_ros tier4_autoware_utils tier4_calibration_msgs + tier4_ground_plane_utils tier4_tag_utils visualization_msgs rclpy - ament_cmake diff --git a/sensor/extrinsic_calibration_manager/config/x2/extrinsic_interactive_calibrator.rviz b/sensor/tag_based_sfm_calibrator/rviz/default.rviz similarity index 58% rename from sensor/extrinsic_calibration_manager/config/x2/extrinsic_interactive_calibrator.rviz rename to sensor/tag_based_sfm_calibrator/rviz/default.rviz index 8e375c55..4e001325 100644 --- a/sensor/extrinsic_calibration_manager/config/x2/extrinsic_interactive_calibrator.rviz +++ b/sensor/tag_based_sfm_calibrator/rviz/default.rviz @@ -6,9 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /concatenated_pointcloud1/Topic1 - Splitter Ratio: 0.7346938848495483 - Tree Height: 725 + - /calibration markers1/Namespaces1 + Splitter Ratio: 0.5 + Tree Height: 719 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -27,10 +27,17 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: concatenated_pointcloud + SyncSource: lidar0 Visualization Manager: Class: "" Displays: + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.009999999776482582 + Reference Frame: + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -43,13 +50,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: concatenated_pointcloud + Name: lidar0 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -61,10 +68,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud + Value: lidar0_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -77,13 +84,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: front_lower + Name: lidar1 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -95,10 +102,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw + Value: lidar1_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -111,13 +118,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 10 - Name: front_upper + Min Intensity: 0 + Name: lidar2 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -129,10 +136,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_upper/pointcloud_raw + Value: lidar2_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -145,13 +152,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 5 - Name: left_lower + Min Intensity: 0 + Name: lidar3 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -163,10 +170,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/left_lower/pointcloud_raw + Value: lidar3_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -179,13 +186,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: left_upper + Name: lidar4 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -197,10 +204,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/left_upper/pointcloud_raw + Value: lidar4_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -213,13 +220,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: rear_lower + Name: lidar5 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -231,10 +238,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/rear_lower/pointcloud_raw + Value: lidar5_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -247,13 +254,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 10 - Name: rear_upper + Min Intensity: 0 + Name: lidar6 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -265,10 +272,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/rear_upper/pointcloud_raw + Value: lidar6_pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -281,13 +288,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 9 - Name: right_lower + Min Intensity: 0 + Name: lidar7 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -299,286 +306,297 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/right_lower/pointcloud_raw + Value: lidar7_pointcloud Use Fixed Frame: true Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: calibration markers + Namespaces: + initial_base_link: true + initial_connections: false + initial_estimations: false + initial_ground_plane: true + optimized_base_link: true + optimized_connections: false + optimized_estimations: true + optimized_ground_plane: true + raw_detections: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: raw_detections markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /raw_detections_markers Value: false - - 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 + - Alpha: 0.5 + Cell Size: 0.10000000149011612 + Class: rviz_default_plugins/Grid + Color: 171; 171; 171 Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: right_upper - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: base_link_grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 80 + Reference Frame: base_link + Value: false + - Alpha: 0.5 + Cell Size: 0.10000000149011612 + Class: rviz_default_plugins/Grid + Color: 0; 255; 127 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: estimated_base_link_grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 80 + Reference Frame: estimated_base_link + Value: false + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 1 + Name: base link + Radius: 0.009999999776482582 + Reference Frame: base_link + Value: false + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 1 + Name: estimated_base link + Radius: 0.009999999776482582 + Reference Frame: estimated_base_link + Value: false + - Class: rviz_default_plugins/Marker + Enabled: true + Name: lidar0_detections_frames + Namespaces: + "": true Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/right_upper/pointcloud_raw - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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 + Reliability Policy: Reliable + Value: /lidar0/lidartag/tag_frame + Value: true + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: republished_pointcloud_camera0 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: lidar1_detections_frames + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /top_unit/top_unit_base_link/camera0/camera_link/calibration/pointcloud - Use Fixed Frame: true - Use rainbow: true + Reliability Policy: Reliable + Value: /lidar1/lidartag/tag_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 + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: republished_pointcloud_camera1 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: lidar2_detections_frames + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /top_unit/top_unit_base_link/camera1/camera_link/calibration/pointcloud - Use Fixed Frame: true - Use rainbow: true + Reliability Policy: Reliable + Value: /lidar2/lidartag/tag_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 + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: republished_pointcloud_camera2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: lidar3_detections_frames + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /top_unit/top_unit_base_link/camera2/camera_link/calibration/pointcloud - Use Fixed Frame: true - Use rainbow: true + Reliability Policy: Reliable + Value: /lidar3/lidartag/tag_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 + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: republished_pointcloud_camera3 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: lidar4_detections_frames + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /top_unit/rear_unit_base_link/camera3/camera_link/calibration/pointcloud - Use Fixed Frame: true - Use rainbow: true + Reliability Policy: Reliable + Value: /lidar4/lidartag/tag_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 + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: republished_pointcloud_camera4 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: lidar5_detections_frames + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /top_unit/top_unit_base_link/camera4/camera_link/calibration/pointcloud - Use Fixed Frame: true - Use rainbow: true + Reliability Policy: Reliable + Value: /lidar5/lidartag/tag_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 + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: republished_pointcloud_camera5 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: lidar6_detections_frames + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /top_unit/top_unit_base_link/camera5/camera_link/calibration/pointcloud - Use Fixed Frame: true - Use rainbow: true + Reliability Policy: Reliable + Value: /lidar6/lidartag/tag_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 + - Class: rviz_default_plugins/Marker Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: republished_pointcloud_camera6 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Name: lidar7_detections_frames + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /top_unit/top_unit_base_link/camera6/camera_link/calibration/pointcloud - Use Fixed Frame: true - Use rainbow: true + Reliability Policy: Reliable + Value: /lidar7/lidartag/tag_frame + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar0_detections_ids + Namespaces: + Text0: true + Text5: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar0/lidartag/id_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar1_detections_ids + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar1/lidartag/id_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar2_detections_ids + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar2/lidartag/id_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar3_detections_ids + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar3/lidartag/id_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar4_detections_ids + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar4/lidartag/id_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar5_detections_ids + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar5/lidartag/id_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar6_detections_ids + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar6/lidartag/id_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: lidar7_detections_ids + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar7/lidartag/id_markers Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: main_sensor Frame Rate: 30 Name: root Tools: @@ -629,14 +647,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 1.5647963285446167 Position: - X: 5 - Y: 0 - Z: 30 + X: -0.49250850081443787 + Y: 0.09322844445705414 + Z: 11.653377532958984 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 0 + Yaw: 4.707127094268799 Saved: ~ Window Geometry: Displays: @@ -644,7 +662,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001870000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004960000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001cd0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044e0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -653,6 +671,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1848 - X: 72 + Width: 1846 + X: 1994 Y: 27 diff --git a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py b/sensor/tag_based_sfm_calibrator/scripts/calibrator_ui_node.py similarity index 84% rename from sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py rename to sensor/tag_based_sfm_calibrator/scripts/calibrator_ui_node.py index d65ec068..f451627e 100755 --- a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/tag_based_sfm_calibrator/scripts/calibrator_ui_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -14,13 +14,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging import signal import sys from PySide2.QtWidgets import QApplication -from extrinsic_reflector_based_calibrator import CalibratorUI -from extrinsic_reflector_based_calibrator import RosInterface import rclpy +from tag_based_sfm_calibrator import CalibratorUI +from tag_based_sfm_calibrator import RosInterface def main(args=None): @@ -38,7 +39,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quiting...", flush=True) + logging.info("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp b/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp similarity index 84% rename from sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp rename to sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp index 098ed480..cbdc8b73 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp +++ b/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -14,23 +14,16 @@ #include #include -#include -#include #include #include #include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else +#include +#include #include -#endif - -#include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { LidartagDetection LidartagDetection::fromLidartagDetectionMsg( @@ -46,7 +39,7 @@ LidartagDetection LidartagDetection::fromLidartagDetectionMsg( // 0, -1], // [1, 0, 0], // [0, -1, 0]] - // Rot_{apriltag} = R^{T} * Rot_{lidartag} + // rotation_{apriltag} = R^{T} * rotation_{lidartag} LidartagDetection detection; detection.id = msg.id; @@ -58,10 +51,10 @@ LidartagDetection LidartagDetection::fromLidartagDetectionMsg( Eigen::Vector3d translation_eigen = pose_eigen.translation(); Eigen::Matrix3d rotation_eigen = pose_eigen.rotation(); - Eigen::Matrix3d apriltag_to_lidartag_rot; - apriltag_to_lidartag_rot << 0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, -1.0, 0.0; + Eigen::Matrix3d apriltag_to_lidartag_rotation; + apriltag_to_lidartag_rotation << 0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, -1.0, 0.0; - rotation_eigen = rotation_eigen * apriltag_to_lidartag_rot; + rotation_eigen = rotation_eigen * apriltag_to_lidartag_rotation; cv::Vec3d translation_cv; cv::Matx33d rotation_cv; @@ -75,15 +68,18 @@ LidartagDetection LidartagDetection::fromLidartagDetectionMsg( return detection; } -void LidartagDetection::computeTemplateCorners() +void LidartagDetection::computeTemplateCorners(double new_size) { - assert(size > 0.0); + assert(new_size > 0.0); + size = new_size; double hsize = 0.5 * size; template_corners = { {-hsize, hsize, 0.0}, {hsize, hsize, 0.0}, {hsize, -hsize, 0.0}, {-hsize, -hsize, 0.0}}; } +void LidartagDetection::computeTemplateCorners() { computeTemplateCorners(this->size); } + void LidartagDetection::computeObjectCorners() { object_corners.resize(template_corners.size()); @@ -110,8 +106,7 @@ ApriltagDetection ApriltagDetection::fromApriltagDetectionMsg( detection.size = size; detection.computeTemplateCorners(); detection.computeObjectCorners(); - double reprojection_error = detection.computePose(intrinsics); - CV_UNUSED(reprojection_error); + [[maybe_unused]] double reprojection_error = detection.computePose(intrinsics); return detection; } @@ -126,39 +121,31 @@ double ApriltagDetection::computePose(const IntrinsicParameters & intrinsics) assert(template_corners.size() > 0); assert(template_corners.size() == undistorted_points.size()); - cv::sqpnp::PoseSolver solver; - std::vector rvec_vec, tvec_vec; - solver.solve(template_corners, undistorted_points, rvec_vec, tvec_vec); + cv::Mat rvec, tvec; - if (tvec_vec.size() == 0) { - assert(false); - return std::numeric_limits::infinity(); - } + bool success = cv::solvePnP( + template_corners, image_corners, intrinsics.camera_matrix, intrinsics.dist_coeffs, rvec, tvec, + false, cv::SOLVEPNP_SQPNP); - assert(rvec_vec.size() == 1); - cv::Mat rvec = rvec_vec[0]; - cv::Mat tvec = tvec_vec[0]; - - // cv::Matx31d translation_vector = tvec; - // cv::Matx33d rotation_matrix; - - // translation_vector = tvec; - // cv::Rodrigues(rvec, rotation_matrix); + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("tier4_tag_utils"), "PNP failed"); + return false; + } pose = cv::Affine3d(rvec, tvec); computeObjectCorners(); - return computeReprojError(intrinsics); + return computeReprojectionError(intrinsics); } -double ApriltagDetection::computeReprojError(const IntrinsicParameters & intrinsics) const +double ApriltagDetection::computeReprojectionError(const IntrinsicParameters & intrinsics) const { - return computeReprojError( + return computeReprojectionError( intrinsics.undistorted_camera_matrix(0, 2), intrinsics.undistorted_camera_matrix(1, 2), intrinsics.undistorted_camera_matrix(0, 0), intrinsics.undistorted_camera_matrix(1, 1)); } -double ApriltagDetection::computeReprojError(double cx, double cy, double fx, double fy) const +double ApriltagDetection::computeReprojectionError(double cx, double cy, double fx, double fy) const { assert(object_corners.size() == image_corners.size()); @@ -166,11 +153,11 @@ double ApriltagDetection::computeReprojError(double cx, double cy, double fx, do for (std::size_t corner_index = 0; corner_index < object_corners.size(); corner_index++) { const auto & object_corner = object_corners[corner_index]; const auto & image_corner = image_corners[corner_index]; - double prx = cx + fx * (object_corner.x / object_corner.z); - double pry = cy + fy * (object_corner.y / object_corner.z); - double errx = std::abs(prx - image_corner.x); - double erry = std::abs(pry - image_corner.y); - error += std::sqrt(errx * errx + erry * erry); + double pr_x = cx + fx * (object_corner.x / object_corner.z); + double pr_y = cy + fy * (object_corner.y / object_corner.z); + double err_x = std::abs(pr_x - image_corner.x); + double err_y = std::abs(pr_y - image_corner.y); + error += std::sqrt(err_x * err_x + err_y * err_y); } return error / object_corners.size(); @@ -200,7 +187,7 @@ void ApriltagGridDetection::computeTemplateCorners(const TagParameters & tag_par double corner_offset_y = -1.0 * (row - 0.5 * (rows - 1)) * factor; cv::Point3d corner_offset(corner_offset_x, corner_offset_y, 0.0); - sub_detection.computeTemplateCorners(); + sub_detection.computeTemplateCorners(tag_parameters.size); for (auto & template_corner : sub_detection.template_corners) { template_corner = template_corner + corner_offset; @@ -264,7 +251,7 @@ double ApriltagGridDetection::recomputeFromSubDetections(const TagParameters & t (4 * rows * cols); Eigen::Vector3d avg_translation = Eigen::Vector3d::Zero(); - std::vector quats; + std::vector quaternions; for (auto & detection : sub_detections) { Eigen::Vector3d translation; @@ -272,13 +259,13 @@ double ApriltagGridDetection::recomputeFromSubDetections(const TagParameters & t cv::cv2eigen(detection.pose.translation(), translation); cv::cv2eigen(detection.pose.rotation(), rotation); Eigen::Quaterniond quat(rotation); - quats.emplace_back(quat.w(), quat.x(), quat.y(), quat.z()); + quaternions.emplace_back(quat.w(), quat.x(), quat.y(), quat.z()); avg_translation += translation; } avg_translation /= sub_detections.size(); - Eigen::Vector4d avg_quat = quaternionAverage(quats); + Eigen::Vector4d avg_quat = quaternionAverage(quaternions); Eigen::Matrix3d avg_rotation = Eigen::Quaterniond(avg_quat(0), avg_quat(1), avg_quat(2), avg_quat(3)).toRotationMatrix(); @@ -368,4 +355,4 @@ double ApriltagGridDetection::detectionDiagonalRatio() const (rows * cols); } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp b/sensor/tag_based_sfm_calibrator/src/apriltag_detector.cpp similarity index 87% rename from sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp rename to sensor/tag_based_sfm_calibrator/src/apriltag_detector.cpp index 637bf25c..5b1ba283 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp +++ b/sensor/tag_based_sfm_calibrator/src/apriltag_detector.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -14,10 +14,10 @@ #include #include -#include -#include #include #include +#include +#include #include #include @@ -26,7 +26,7 @@ #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { std::unordered_map @@ -57,7 +57,14 @@ ApriltagDetector::ApriltagDetector( const std::string & family_name = tag_parameters.family; assert(tag_create_fn_map.count(family_name) == 1); - if (apriltag_family_map.count(family_name) != 0 || tag_create_fn_map.count(family_name) != 1) { + if (apriltag_family_map.count(family_name) != 0) { + continue; + } + + if (tag_create_fn_map.count(family_name) != 1) { + RCLCPP_FATAL( + rclcpp::get_logger("apriltag_detector"), "The tag family %s is invalid", + family_name.c_str()); continue; } @@ -128,7 +135,8 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c // Detect individual tags and filter out invalid ones image_u8_t apriltag_img = {cv_img.cols, cv_img.rows, cv_img.cols, cv_img.data}; - zarray_t * detections = apriltag_detector_detect(apriltag_detector_, &apriltag_img); + zarray_t * detections = + apriltag_detector_detect(apriltag_detector_, &apriltag_img); // cSpell:ignore zarray for (int i = 0; i < zarray_size(detections); i++) { apriltag_detection_t * det; @@ -148,7 +156,7 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c result.image_corners.emplace_back(det->p[i][0], det->p[i][1]); } - // Extra filter since the dectector finds false positives with a high margin for out-of-plane + // Extra filter since the detector finds false positives with a high margin for out-of-plane // rotations double max_homography_error = 0.0; @@ -199,7 +207,7 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c detection_info.fy = fy_; detection_info.cx = cx_; detection_info.cy = cy_; - detection_info.tagsize = tag_size; + detection_info.tagsize = tag_size; // cSpell:ignore tagsize apriltag_pose_t pose; estimate_tag_pose(&detection_info, &pose); @@ -210,7 +218,7 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c if (std::abs(cv::determinant(rotation) - 1.0) > 1e-5) { RCLCPP_WARN( rclcpp::get_logger("apriltag_detector"), - "Detected apriltag: %s but dicarded due to its rotation not having unit determinant\t " + "Detected apriltag: %s but discarded due to its rotation not having unit determinant\t " "det=%.2f", tag_family_and_id.c_str(), std::abs(cv::determinant(rotation))); continue; @@ -223,7 +231,7 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c rclcpp::get_logger("apriltag_detector"), "det=%.2f | det=%.2f", cv::determinant(rotation), cv::determinant(result.pose.rotation())); - matd_destroy(pose.R); + matd_destroy(pose.R); // cSpell:ignore matd matd_destroy(pose.t); } @@ -234,16 +242,16 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c double rotation_angle = (180.0 / CV_PI) * std::acos(v_front.dot(v_to_tag)); result.computeObjectCorners(); - double reproj_error = result.computeReprojError(cx_, cy_, fx_, fy_); + double reprojection_error = result.computeReprojectionError(cx_, cy_, fx_, fy_); if ( fx_ > 0.0 && fy_ > 0.0 && cx_ > 0.0 && cy_ > 0.0 && - reproj_error > detector_parameters_.max_reproj_error) { + reprojection_error > detector_parameters_.max_reprojection_error) { RCLCPP_WARN( rclcpp::get_logger("apriltag_detector"), - "Detected apriltag: %s but dicarded due to its reprojection error\t margin: %.2f\t " - "hom.error=%.2f\t repr.error=%.2f out_angle=%.2f deg", - tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reproj_error, + "Detected apriltag: %s but discarded due to its reprojection error\t margin: %.2f\t " + "hom.error=%.2f\t reprojection.error=%.2f out_angle=%.2f deg", + tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reprojection_error, rotation_angle); continue; } @@ -253,17 +261,18 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c rotation_angle > detector_parameters_.max_out_of_plane_angle) { RCLCPP_WARN( rclcpp::get_logger("apriltag_detector"), - "Detected apriltag: %s but dicarded due to its out-of-plane angle\t margin: %.2f\t " - "hom.error=%.2f\t repr.error=%.2f out_angle=%.2f deg", - tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reproj_error, + "Detected apriltag: %s but discarded due to its out-of-plane angle\t margin: %.2f\t " + "hom.error=%.2f\t reprojection.error=%.2f out_angle=%.2f deg", + tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reprojection_error, rotation_angle); continue; } RCLCPP_INFO( rclcpp::get_logger("apriltag_detector"), - "Detected apriltag: %s \t margin: %.2f\t hom.error=%.2f\t repr.error=%.2f out_angle=%.2f deg", - tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reproj_error, + "Detected apriltag: %s \t margin: %.2f\t hom.error=%.2f\t reprojection.error=%.2f " + "out_angle=%.2f deg", + tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reprojection_error, rotation_angle); individual_detections_map[tag_type].emplace_back(result); @@ -324,4 +333,4 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c return grid_detections_map; } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/calibration_scene_extractor.cpp b/sensor/tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp similarity index 79% rename from sensor/extrinsic_tag_based_base_calibrator/src/calibration_scene_extractor.cpp rename to sensor/tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp index a3a90c8c..5546bace 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/calibration_scene_extractor.cpp +++ b/sensor/tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,16 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -29,10 +19,15 @@ #include #include #include +#include +#include +#include +#include #include +#include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { void CalibrationSceneExtractor::setCalibrationSensorIntrinsics(IntrinsicParameters & intrinsics) @@ -57,12 +52,9 @@ CalibrationScene CalibrationSceneExtractor::processScene( const std::unordered_map & lidar_detections_map, const std::unordered_map & camera_detections_map, const std::vector & calibration_lidar_frames, - const std::vector & calibration_camera_frames, const std::string & main_sensor_frame, + const std::vector & calibration_camera_frames, const std::vector & external_camera_image_names) { - CV_UNUSED(camera_detections_map); - CV_UNUSED(main_sensor_frame); - CalibrationScene scene; for (std::size_t calibration_lidar_id = 0; calibration_lidar_id < calibration_lidar_frames.size(); @@ -93,33 +85,6 @@ CalibrationScene CalibrationSceneExtractor::processScene( return scene; } -CalibrationScene CalibrationSceneExtractor::processScene( - const std::unordered_map & calibration_cameras_image_map, - const std::vector & calibration_camera_frames, const std::string & main_sensor_frame, - const std::vector & external_camera_image_names) -{ - CV_UNUSED(main_sensor_frame); // TODO(knzo25): delete this argument from the signature - CalibrationScene scene; - - for (std::size_t calibration_camera_id = 0; - calibration_camera_id < calibration_camera_frames.size(); calibration_camera_id++) { - SingleCalibrationCameraDetections camera_detections; - const std::string calibration_frame = calibration_camera_frames[calibration_camera_id]; - camera_detections.calibration_frame = calibration_frame; - camera_detections.calibration_camera_id = calibration_camera_id; - - camera_detections.grouped_detections = detect( - calibration_sensor_detector_, calibration_sensor_intrinsics_, - calibration_cameras_image_map.at(calibration_frame)); - - scene.calibration_cameras_detections.push_back(camera_detections); - } - - processExternalCameraImages(scene, external_camera_image_names); - - return scene; -} - void CalibrationSceneExtractor::processExternalCameraImages( CalibrationScene & scene, const std::vector & external_camera_image_names) { @@ -208,4 +173,4 @@ GroupedApriltagGridDetections CalibrationSceneExtractor::detect( return detector.detect(undistorted_img); } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/ceres/calibration_problem.cpp b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp similarity index 96% rename from sensor/extrinsic_tag_based_base_calibrator/src/ceres/calibration_problem.cpp rename to sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp index 51f3acc8..b5fe2a58 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/ceres/calibration_problem.cpp +++ b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -13,11 +13,6 @@ // limitations under the License. #include -#include -#include -#include -#include -#include #include #include #include @@ -26,6 +21,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include @@ -33,8 +33,9 @@ #include #include +#include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { void CalibrationProblem::setOptimizeIntrinsics(bool ba_optimize_intrinsics) @@ -88,13 +89,13 @@ void CalibrationProblem::setFixedSharedGroundPlane( rot.col(1) = base_y.normalized(); rot.col(2) = base_z.normalized(); - cv::Matx33d cv_rot; - cv::Vec3d cv_transl; + cv::Matx33d cv_rotation; + cv::Vec3d cv_translation; - cv::eigen2cv(x0, cv_transl); - cv::eigen2cv(rot, cv_rot); + cv::eigen2cv(x0, cv_translation); + cv::eigen2cv(rot, cv_rotation); - fixed_ground_pose_ = cv::Affine3d(cv_rot, cv_transl); + fixed_ground_pose_ = cv::Affine3d(cv_rotation, cv_translation); } void CalibrationProblem::setOptimizationWeights( @@ -127,13 +128,13 @@ void CalibrationProblem::setData(CalibrationData::Ptr & data) { data_ = data; } void CalibrationProblem::dataToPlaceholders() { // Compute the initial ground plane ! - cv::Affine3d ground_pose; - computeGroundPlane(data_->initial_ground_tag_poses_map, 0.0, ground_pose); + std::optional ground_pose; if (force_fixed_ground_plane_) { ground_pose = fixed_ground_pose_; } else { - computeGroundPlane(data_->initial_ground_tag_poses_map, 0.0, ground_pose); + ground_pose = computeGroundPlane(data_->initial_ground_tag_poses_map, 0.0); + assert(ground_pose.has_value()); } // Prepare the placeholders @@ -190,7 +191,8 @@ void CalibrationProblem::dataToPlaceholders() placeholderToPose3d(pose_opt_map[uid], data_->optimized_tag_poses_map[uid], false); } else { pose3dToGroundTagPlaceholder( - uid, *pose, ground_pose, shrd_ground_tag_pose_opt, indep_ground_tag_pose_opt_map[uid]); + uid, *pose, ground_pose.value(), shrd_ground_tag_pose_opt, + indep_ground_tag_pose_opt_map[uid]); // cSpell:ignore shrd groundTagPlaceholderToPose3d( shrd_ground_tag_pose_opt, indep_ground_tag_pose_opt_map[uid], data_->optimized_tag_poses_map[uid]); @@ -557,6 +559,8 @@ void CalibrationProblem::evaluate() ? "external_camera" : sensor_uid.toString(); + data_->optimized_sensor_residuals_map[sensor_uid] = mean_reprojection_error; + RCLCPP_INFO( rclcpp::get_logger("calibration_problem"), "\t%s reprojection errors: mean=%.2f min=%.2f max=%.2f observations=%lu", @@ -904,7 +908,7 @@ void CalibrationProblem::solve() RCLCPP_INFO_STREAM(rclcpp::get_logger("calibration_problem"), "Initial cost: " << initial_cost); ceres::Solver::Options options; - options.linear_solver_type = ceres::DENSE_SCHUR; + options.linear_solver_type = ceres::DENSE_SCHUR; // cSpell:ignore schur options.minimizer_progress_to_stdout = true; options.max_num_iterations = 500; ceres::Solver::Summary summary; @@ -953,6 +957,7 @@ void CalibrationProblem::writeDebugImage( auto project_corners = [this, &sensor_uid]( ApriltagDetection & detection, const cv::Affine3d & camera_pose, const cv::Affine3d & tag_pose, bool use_optimized_intrinsics) { + // Note: wcs=world coordinate system. ccs=camera coordinate system std::vector corners_wcs{ tag_pose * detection.template_corners[0], tag_pose * detection.template_corners[1], tag_pose * detection.template_corners[2], tag_pose * detection.template_corners[3]}; @@ -1004,13 +1009,19 @@ void CalibrationProblem::writeDebugImages() // Need to make sure all the cameras are in the map UID calibration_camera_uid = UID::makeSensorUID(SensorType::CalibrationCamera, camera_id); + if (!scene.calibration_cameras_detections[camera_id].calibration_image) { + RCLCPP_ERROR( + rclcpp::get_logger("calibration_problem"), "scene=%lu camera_id=%lu is invalid", + scene_index, camera_id); + continue; + } + cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy( *scene.calibration_cameras_detections[camera_id].calibration_image, sensor_msgs::image_encodings::BGR8); - cv::Mat undistorted_img = cv_ptr->image; - ; // we assume we use the undistorted image + cv::Mat undistorted_img = cv_ptr->image; // we assume we use the undistorted image writeDebugImage( scene_index, calibration_camera_uid, undistorted_img, @@ -1081,12 +1092,12 @@ void CalibrationProblem::placeholderToPose3d( Eigen::Matrix3d rotation = quat.toRotationMatrix(); - cv::Matx33d cv_rot; - cv::Vec3d cv_transl; - cv::eigen2cv(translation, cv_transl); - cv::eigen2cv(rotation, cv_rot); + cv::Matx33d cv_rotation; + cv::Vec3d cv_translation; + cv::eigen2cv(translation, cv_translation); + cv::eigen2cv(rotation, cv_rotation); - pose = std::make_shared(cv_rot, cv_transl); + pose = std::make_shared(cv_rotation, cv_translation); if (invert) { *pose = pose->inv(); @@ -1153,6 +1164,7 @@ void CalibrationProblem::groundTagPlaceholderToPose3d( shrd_placeholder[ROTATION_Y_INDEX] * shrd_placeholder[ROTATION_Y_INDEX] + shrd_placeholder[ROTATION_Z_INDEX] * shrd_placeholder[ROTATION_Z_INDEX]); + // cSpell:ignore WXYZ // Eigen's Quaternion constructor is in the WXYZ order but the internal data is in the XYZW format Eigen::Quaterniond quat = Eigen::Quaterniond( scale * shrd_placeholder[ROTATION_W_INDEX], scale * shrd_placeholder[ROTATION_X_INDEX], @@ -1184,12 +1196,12 @@ void CalibrationProblem::groundTagPlaceholderToPose3d( Eigen::Matrix3d pose_rotation = pose_matrix.block<3, 3>(0, 0); Eigen::Vector3d pose_translation = pose_matrix.block<3, 1>(0, 3); - cv::Matx33d cv_rot; - cv::Vec3d cv_transl; - cv::eigen2cv(pose_translation, cv_transl); - cv::eigen2cv(pose_rotation, cv_rot); + cv::Matx33d cv_rotation; + cv::Vec3d cv_translation; + cv::eigen2cv(pose_translation, cv_translation); + cv::eigen2cv(pose_rotation, cv_rotation); - pose = std::make_shared(cv_rot, cv_transl); + pose = std::make_shared(cv_rotation, cv_translation); } void CalibrationProblem::printCalibrationResults() @@ -1210,4 +1222,4 @@ void CalibrationProblem::printCalibrationResults() } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp similarity index 95% rename from sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp index 56df1ac4..2c80a782 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { void ApriltagBasedCalibrator::extractCalibrationPoints() @@ -128,4 +128,4 @@ void ApriltagBasedCalibrator::writeDebugImages(const IntrinsicParameters & intri } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp similarity index 94% rename from sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp index 9add0671..76323a7c 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { void ChessboardBasedCalibrator::extractCalibrationPoints() @@ -112,4 +112,4 @@ void ChessboardBasedCalibrator::writeDebugImages(const IntrinsicParameters & int } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp similarity index 89% rename from sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp index f261c1c1..6a17bd25 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { void IntrinsicsCalibrator::setCalibrationImageFiles( @@ -69,7 +69,7 @@ bool IntrinsicsCalibrator::calibrate(IntrinsicParameters & intrinsics) flags |= cv::CALIB_FIX_K1; } - double reproj_error = cv::calibrateCamera( + double reprojection_error = cv::calibrateCamera( object_points_, image_points_, intrinsics.size, intrinsics.camera_matrix, intrinsics.dist_coeffs, rvecs_, tvecs_, flags); @@ -79,7 +79,8 @@ bool IntrinsicsCalibrator::calibrate(IntrinsicParameters & intrinsics) rclcpp::get_logger("intrinsics_calibrator"), "Radial distortion coeffs: %d", num_radial_distortion_coeffs_); - RCLCPP_INFO(rclcpp::get_logger("intrinsics_calibrator"), "Reproj_error: %.2f", reproj_error); + RCLCPP_INFO( + rclcpp::get_logger("intrinsics_calibrator"), "reprojection_error: %.2f", reprojection_error); RCLCPP_INFO_STREAM( rclcpp::get_logger("intrinsics_calibrator"), "Camera matrix:\n" << intrinsics.camera_matrix); @@ -102,4 +103,4 @@ bool IntrinsicsCalibrator::calibrate(IntrinsicParameters & intrinsics) return true; } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp b/sensor/tag_based_sfm_calibrator/src/main.cpp old mode 100755 new mode 100644 similarity index 75% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp rename to sensor/tag_based_sfm_calibrator/src/main.cpp index 6c66c3e6..532414bd --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp +++ b/sensor/tag_based_sfm_calibrator/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include @@ -23,8 +23,8 @@ int main(int argc, char ** argv) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::NodeOptions node_options; - std::shared_ptr node = - std::make_shared(node_options); + std::shared_ptr node = + std::make_shared(node_options); executor.add_node(node); executor.spin(); diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/math.cpp b/sensor/tag_based_sfm_calibrator/src/math.cpp similarity index 86% rename from sensor/extrinsic_tag_based_base_calibrator/src/math.cpp rename to sensor/tag_based_sfm_calibrator/src/math.cpp index a26f516e..e0c2a393 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/math.cpp +++ b/sensor/tag_based_sfm_calibrator/src/math.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ -#define EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ +#ifndef tag_based_sfm_calibrator__MATH_HPP_ +#define tag_based_sfm_calibrator__MATH_HPP_ #include #include #include -#include -#include -#include #include #include #include +#include +#include +#include #include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { /* @@ -54,7 +54,7 @@ Eigen::Vector4d quaternionAverage(std::vector quaternions) A += quaternions[q] * quaternions[q].transpose(); } - // normalise with the number of quaternions + // normalize with the number of quaternions A /= quaternions.size(); // Compute the SVD of this 4x4 matrix @@ -94,12 +94,12 @@ std::array tagPoseToCorners(const cv::Affine3d & pose, double size pose * (0.5 * size * templates[2]), pose * (0.5 * size * templates[3])}; } -bool computeGroundPlane(const std::vector & points, cv::Affine3d & ground_pose) +std::optional computeGroundPlane(const std::vector & points) { int num_points = static_cast(points.size()); if (num_points == 0) { - return false; + return std::nullopt; } cv::Mat_ pca_input = cv::Mat_(num_points, 3); @@ -126,7 +126,7 @@ bool computeGroundPlane(const std::vector & points, cv::Affine3d & gr assert(std::abs(det - 1.0) < 1e5); - ground_pose = cv::Affine3d(rotation, translation); + cv::Affine3d ground_pose(rotation, translation); // Fix the ground origin to be the projection of the origin into the ground plane cv::Vec3d initial_ground_to_origin = ground_pose.inv() * cv::Vec3d(0.0, 0.0, 0.0); @@ -152,18 +152,17 @@ bool computeGroundPlane(const std::vector & points, cv::Affine3d & gr assert(std::abs(det - 1.0) < 1e5); if (std::abs(det - 1.0) > 1e5) { - return false; + return std::nullopt; } } ground_pose = cv::Affine3d(rotation, origin_to_new_ground); - return true; + return ground_pose; } -bool computeGroundPlane( - const std::map> & poses_map, double tag_size, - cv::Affine3d & ground_pose) +std::optional computeGroundPlane( + const std::map> & poses_map, double tag_size) { std::vector points; @@ -172,7 +171,7 @@ bool computeGroundPlane( points.insert(points.end(), corners.begin(), corners.end()); } - return computeGroundPlane(points, ground_pose); + return computeGroundPlane(points); } cv::Affine3d computeBaseLink( @@ -327,7 +326,7 @@ cv::Affine3d estimateInitialPosesFilterOutliers( // Estimate the average pose Eigen::Vector3d avg_translation = Eigen::Vector3d::Zero(); - std::vector quats; + std::vector quaternions; for (auto & [parent_uid, pose] : best_model_inliers) { Eigen::Vector3d translation; @@ -335,7 +334,7 @@ cv::Affine3d estimateInitialPosesFilterOutliers( cv::cv2eigen(pose.translation(), translation); cv::cv2eigen(pose.rotation(), rotation); Eigen::Quaterniond quat(rotation); - quats.emplace_back(quat.w(), quat.x(), quat.y(), quat.z()); + quaternions.emplace_back(quat.w(), quat.x(), quat.y(), quat.z()); RCLCPP_INFO( rclcpp::get_logger("pose_estimation"), @@ -349,7 +348,7 @@ cv::Affine3d estimateInitialPosesFilterOutliers( } avg_translation /= best_model_inliers.size(); - Eigen::Vector4d avg_quat = quaternionAverage(quats); + Eigen::Vector4d avg_quat = quaternionAverage(quaternions); Eigen::Matrix3d avg_rotation = Eigen::Quaterniond(avg_quat(0), avg_quat(1), avg_quat(2), avg_quat(3)).toRotationMatrix(); @@ -403,7 +402,56 @@ void estimateInitialPoses( int max_depth, double min_allowed_diagonal_ratio) { std::map estimated_poses; - std::set iteration_uids; + std::set iteration_uids; // cSpell:ignore uids + + // Show a list of the number of connections per uid + std::stringstream ss; + std::vector connections_count; + ss << "List of connections:\n"; + + for (const auto & [uid, connections] : data.uid_connections_map) { + connections_count.push_back(uid.toString() + "(" + std::to_string(connections.size()) + ")"); + } + + std::sort(connections_count.begin(), connections_count.end()); + + for (const auto & connection_str : connections_count) { + ss << connection_str << " "; + } + + // Show the connections per uid + std::unordered_map string_to_uid_to_uid_map; + std::vector uid_strings; + + for (const auto & it : data.uid_connections_map) { + string_to_uid_to_uid_map[it.first.toString()] = it.first; + uid_strings.push_back(it.first.toString()); + } + + std::sort(uid_strings.begin(), uid_strings.end()); + ss << "\nConnections per UID:\n"; + + for (const auto & uid_string : uid_strings) { + const auto uid = string_to_uid_to_uid_map[uid_string]; + const auto & connections = data.uid_connections_map[uid]; + std::vector connection_uid_strings; + + for (const auto & connection_uid : connections) { + connection_uid_strings.push_back(connection_uid.toString()); + } + + std::sort(connection_uid_strings.begin(), connection_uid_strings.end()); + + ss << uid_string << ": "; + + for (const auto & connection_uid_string : connection_uid_strings) { + ss << connection_uid_string << " "; + } + + ss << "\n"; + } + + RCLCPP_INFO(rclcpp::get_logger("pose_estimation"), "%s", ss.str().c_str()); { std::map>> raw_poses_map; @@ -445,6 +493,6 @@ void estimateInitialPoses( } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ +#endif // tag_based_sfm_calibrator__MATH_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp similarity index 78% rename from sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp index 01a72b00..cdd38ab6 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -13,21 +13,22 @@ // limitations under the License. #include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include #include #include @@ -43,12 +44,12 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( const rclcpp::NodeOptions & options) -: Node("extrinsic_tag_based_base_calibrator_node", options), +: Node("tag_based_sfm_calibrator_node", options), tf_broadcaster_(this), calibration_done_(false), data_(std::make_shared()) @@ -56,23 +57,10 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( tf_buffer_ = std::make_shared(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); + publish_tfs_ = this->declare_parameter("publish_tfs"); + write_debug_images_ = this->declare_parameter("write_debug_images"); base_frame_ = this->declare_parameter("base_frame", "base_link"); - std::vector lidar_sensor_kit_frames = - this->declare_parameter>("lidar_sensor_kit_frames"); - std::vector calibration_lidar_parent_frames = - this->declare_parameter>("calibration_lidar_parent_frames"); - - std::vector camera_sensor_kit_frames = - this->declare_parameter>("camera_sensor_kit_frames"); - std::vector calibration_camera_parent_frames = - this->declare_parameter>("calibration_camera_parent_frames"); - - std::vector lidar_calibration_service_names = - this->declare_parameter>("lidar_calibration_service_names"); - std::vector camera_calibration_service_names = - this->declare_parameter>("camera_calibration_service_names"); - main_calibration_sensor_frame_ = this->declare_parameter("main_calibration_sensor_frame"); @@ -90,10 +78,6 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( return v2; }; - lidar_sensor_kit_frames = remove_empty_strings(lidar_sensor_kit_frames); - calibration_lidar_parent_frames = remove_empty_strings(calibration_lidar_parent_frames); - camera_sensor_kit_frames = remove_empty_strings(camera_sensor_kit_frames); - calibration_camera_parent_frames = remove_empty_strings(calibration_camera_parent_frames); calibration_lidar_frames_vector_ = remove_empty_strings(calibration_lidar_frames_vector_); calibration_camera_frames_vector_ = remove_empty_strings(calibration_camera_frames_vector_); @@ -106,50 +90,33 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( std::vector calibration_lidar_detections_topics = this->declare_parameter>("calibration_lidar_detections_topics"); - std::vector calibration_image_detections_topics = - this->declare_parameter>("calibration_image_detections_topics"); + std::vector calibration_camera_detections_topics = + this->declare_parameter>("calibration_camera_detections_topics"); std::vector calibration_camera_info_topics = this->declare_parameter>("calibration_camera_info_topics"); std::vector calibration_image_topics = - this->declare_parameter>("calibration_image_topics"); + this->declare_parameter>("calibration_compressed_image_topics"); calibration_lidar_detections_topics = remove_empty_strings(calibration_lidar_detections_topics); - calibration_image_detections_topics = remove_empty_strings(calibration_image_detections_topics); + calibration_camera_detections_topics = remove_empty_strings(calibration_camera_detections_topics); calibration_camera_info_topics = remove_empty_strings(calibration_camera_info_topics); - assert(lidar_sensor_kit_frames.size() == calibration_lidar_parent_frames.size()); - assert(lidar_sensor_kit_frames.size() == calibration_lidar_frames_vector_.size()); - assert(lidar_sensor_kit_frames.size() == calibration_lidar_detections_topics.size()); - - assert(camera_sensor_kit_frames.size() == calibration_camera_parent_frames.size()); - assert(camera_sensor_kit_frames.size() == calibration_camera_frames_vector_.size()); - assert(camera_sensor_kit_frames.size() == calibration_image_detections_topics.size()); - assert(camera_sensor_kit_frames.size() == calibration_camera_info_topics.size()); - for (std::size_t lidar_index = 0; lidar_index < calibration_lidar_frames_vector_.size(); lidar_index++) { const std::string lidar_name = calibration_lidar_frames_vector_[lidar_index]; - sensor_kit_frame_map_[lidar_name] = lidar_sensor_kit_frames[lidar_index]; - calibration_sensor_parent_frame_map_[lidar_name] = calibration_lidar_parent_frames[lidar_index]; calibration_lidar_detections_topic_map_[lidar_name] = calibration_lidar_detections_topics[lidar_index]; - calibration_service_names_map_[lidar_name] = lidar_calibration_service_names[lidar_index]; } for (std::size_t camera_index = 0; camera_index < calibration_camera_frames_vector_.size(); camera_index++) { const std::string camera_name = calibration_camera_frames_vector_[camera_index]; - sensor_kit_frame_map_[camera_name] = camera_sensor_kit_frames[camera_index]; - calibration_sensor_parent_frame_map_[camera_name] = - calibration_camera_parent_frames[camera_index]; - calibration_image_detections_topic_map_[camera_name] = - calibration_image_detections_topics[camera_index]; + calibration_camera_detections_topics[camera_index]; calibration_camera_info_topic_map_[camera_name] = calibration_camera_info_topics[camera_index]; calibration_image_topic_map_[camera_name] = calibration_image_topics[camera_index]; - calibration_service_names_map_[camera_name] = camera_calibration_service_names[camera_index]; } assert( @@ -157,57 +124,54 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( calibration_sensor_frames_vector_.begin(), calibration_sensor_frames_vector_.end(), main_calibration_sensor_frame_) != calibration_sensor_frames_vector_.end()); - lidartag_to_apriltag_scale_ = this->declare_parameter("lidartag_to_apriltag_scale", 0.75); + lidartag_to_apriltag_scale_ = this->declare_parameter("lidartag_to_apriltag_scale"); auxiliar_tag_parameters_.tag_type = TagType::AuxiliarTag; - auxiliar_tag_parameters_.family = - this->declare_parameter("auxiliar_tag_family", "36h11"); - auxiliar_tag_parameters_.rows = this->declare_parameter("auxiliar_tag_rows", 1); - auxiliar_tag_parameters_.cols = this->declare_parameter("auxiliar_tag_cols", 1); - auxiliar_tag_parameters_.size = this->declare_parameter("auxiliar_tag_size", 0.2); - auxiliar_tag_parameters_.spacing = this->declare_parameter("auxiliar_tag_spacing", 0.2); + auxiliar_tag_parameters_.family = this->declare_parameter("auxiliar_tag.family"); + auxiliar_tag_parameters_.rows = this->declare_parameter("auxiliar_tag.rows"); + auxiliar_tag_parameters_.cols = this->declare_parameter("auxiliar_tag.cols"); + auxiliar_tag_parameters_.size = this->declare_parameter("auxiliar_tag.size"); + auxiliar_tag_parameters_.spacing = this->declare_parameter("auxiliar_tag.spacing"); std::vector auxiliar_tag_ids = - this->declare_parameter>("auxiliar_tag_ids"); + this->declare_parameter>("auxiliar_tag.ids"); std::for_each(auxiliar_tag_ids.begin(), auxiliar_tag_ids.end(), [&](const auto & id) { auxiliar_tag_parameters_.ids.insert(static_cast(id)); }); waypoint_tag_parameters_.tag_type = TagType::WaypointTag; - waypoint_tag_parameters_.family = - this->declare_parameter("waypoint_tag_family", "16h5"); - waypoint_tag_parameters_.rows = this->declare_parameter("waypoint_tag_rows", 1); - waypoint_tag_parameters_.cols = this->declare_parameter("waypoint_tag_cols", 1); - waypoint_tag_parameters_.size = this->declare_parameter("waypoint_tag_size", 0.6); - waypoint_tag_parameters_.spacing = this->declare_parameter("waypoint_tag_spacing", 0.2); + waypoint_tag_parameters_.family = this->declare_parameter("waypoint_tag.family"); + waypoint_tag_parameters_.rows = this->declare_parameter("waypoint_tag.rows"); + waypoint_tag_parameters_.cols = this->declare_parameter("waypoint_tag.cols"); + waypoint_tag_parameters_.size = this->declare_parameter("waypoint_tag.size"); + waypoint_tag_parameters_.spacing = this->declare_parameter("waypoint_tag.spacing"); std::vector waypoint_tag_ids = - this->declare_parameter>("waypoint_tag_ids"); + this->declare_parameter>("waypoint_tag.ids"); std::for_each(waypoint_tag_ids.begin(), waypoint_tag_ids.end(), [&](const auto & id) { waypoint_tag_parameters_.ids.insert(static_cast(id)); }); ground_tag_parameters_.tag_type = TagType::GroundTag; - ground_tag_parameters_.family = - this->declare_parameter("ground_tag_family", "36h11"); - ground_tag_parameters_.rows = this->declare_parameter("ground_tag_rows", 1); - ground_tag_parameters_.cols = this->declare_parameter("ground_tag_cols", 1); - ground_tag_parameters_.size = this->declare_parameter("ground_tag_size", 0.6); - ground_tag_parameters_.spacing = this->declare_parameter("ground_tag_spacing", 0.2); + ground_tag_parameters_.family = this->declare_parameter("ground_tag.family"); + ground_tag_parameters_.rows = this->declare_parameter("ground_tag.rows"); + ground_tag_parameters_.cols = this->declare_parameter("ground_tag.cols"); + ground_tag_parameters_.size = this->declare_parameter("ground_tag.size"); + ground_tag_parameters_.spacing = this->declare_parameter("ground_tag.spacing"); std::vector ground_tag_ids = - this->declare_parameter>("ground_tag_ids"); + this->declare_parameter>("ground_tag.ids"); std::for_each(ground_tag_ids.begin(), ground_tag_ids.end(), [&](const auto & id) { ground_tag_parameters_.ids.insert(static_cast(id)); }); wheel_tag_parameters_.tag_type = TagType::WheelTag; - wheel_tag_parameters_.family = this->declare_parameter("wheel_tag_family", "16h5"); - wheel_tag_parameters_.rows = this->declare_parameter("wheel_tag_rows", 2); - wheel_tag_parameters_.cols = this->declare_parameter("wheel_tag_cols", 2); - wheel_tag_parameters_.size = this->declare_parameter("wheel_tag_size", 0.6); - wheel_tag_parameters_.spacing = this->declare_parameter("wheel_tag_spacing", 0.2); - - left_wheel_tag_id = this->declare_parameter("left_wheel_tag_id", 3); - right_wheel_tag_id = this->declare_parameter("right_wheel_tag_id", 4); + wheel_tag_parameters_.family = this->declare_parameter("wheel_tag.family"); + wheel_tag_parameters_.rows = this->declare_parameter("wheel_tag.rows"); + wheel_tag_parameters_.cols = this->declare_parameter("wheel_tag.cols"); + wheel_tag_parameters_.size = this->declare_parameter("wheel_tag.size"); + wheel_tag_parameters_.spacing = this->declare_parameter("wheel_tag.spacing"); + + left_wheel_tag_id = this->declare_parameter("left_wheel_tag_id"); + right_wheel_tag_id = this->declare_parameter("right_wheel_tag_id"); wheel_tag_parameters_.ids.insert(left_wheel_tag_id); wheel_tag_parameters_.ids.insert(right_wheel_tag_id); @@ -220,83 +184,71 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( wheel_tag_parameters_}; // Optimization options - ba_optimize_intrinsics_ = this->declare_parameter("ba_optimize_intrinsics", false); - ba_share_intrinsics_ = this->declare_parameter("ba_share_intrinsics", false); - ba_force_shared_ground_plane_ = - this->declare_parameter("ba_force_shared_ground_plane", false); - virtual_lidar_f_ = this->declare_parameter("virtual_lidar_f", 10000.0); + ba_optimize_intrinsics_ = this->declare_parameter("ba.optimize_intrinsics"); + ba_share_intrinsics_ = this->declare_parameter("ba.share_intrinsics"); + ba_force_shared_ground_plane_ = this->declare_parameter("ba.force_shared_ground_plane"); + virtual_lidar_f_ = this->declare_parameter("ba.virtual_lidar_f"); calibration_camera_optimization_weight_ = - this->declare_parameter("calibration_camera_optimization_weight", 0.2); + this->declare_parameter("ba.calibration_camera_optimization_weight"); calibration_lidar_optimization_weight_ = - this->declare_parameter("calibration_lidar_optimization_weight", 0.2); + this->declare_parameter("ba.calibration_lidar_optimization_weight"); external_camera_optimization_weight_ = - this->declare_parameter("external_camera_optimization_weight", 0.6); + this->declare_parameter("ba.external_camera_optimization_weight"); ba_fixed_ground_plane_model_ = - this->declare_parameter("ba_fixed_ground_plane_model", false); - ba_fixed_ground_plane_model_a_ = - this->declare_parameter("ba_fixed_ground_plane_model_a", 0.0); - ba_fixed_ground_plane_model_b_ = - this->declare_parameter("ba_fixed_ground_plane_model_b", 0.0); - ba_fixed_ground_plane_model_c_ = - this->declare_parameter("ba_fixed_ground_plane_model_c", 1.0); - ba_fixed_ground_plane_model_d_ = - this->declare_parameter("ba_fixed_ground_plane_model_d", 0.0); + this->declare_parameter("ba.fixed_ground_plane_model", false); // Initial intrinsic calibration parameters initial_intrinsic_calibration_board_type_ = - this->declare_parameter("initial_intrinsic_calibration_board_type", "apriltag"); + this->declare_parameter("initial_intrinsic_calibration.board_type"); initial_intrinsic_calibration_tangent_distortion_ = - this->declare_parameter("initial_intrinsic_calibration_tangent_distortion", true); + this->declare_parameter("initial_intrinsic_calibration.tangent_distortion"); initial_intrinsic_calibration_radial_distortion_coeffs_ = - this->declare_parameter("initial_intrinsic_calibration_radial_distortion_coeffs", 2); + this->declare_parameter("initial_intrinsic_calibration.radial_distortion_coeffs"); initial_intrinsic_calibration_debug_ = - this->declare_parameter("initial_intrinsic_calibration_debug", true); + this->declare_parameter("initial_intrinsic_calibration.debug"); initial_intrinsic_calibration_tag_parameters_.tag_type = TagType::IntrinsicCalibrationTag; initial_intrinsic_calibration_tag_parameters_.family = - this->declare_parameter("initial_intrinsic_calibration_tag_family", "16h5"); + this->declare_parameter("initial_intrinsic_calibration.tag.family"); initial_intrinsic_calibration_tag_parameters_.rows = - this->declare_parameter("initial_intrinsic_calibration_tag_rows", 1); + this->declare_parameter("initial_intrinsic_calibration.tag.rows"); initial_intrinsic_calibration_tag_parameters_.cols = - this->declare_parameter("initial_intrinsic_calibration_tag_cols", 1); + this->declare_parameter("initial_intrinsic_calibration.tag.cols"); initial_intrinsic_calibration_tag_parameters_.size = - this->declare_parameter("initial_intrinsic_calibration_tag_size", 0.8); + this->declare_parameter("initial_intrinsic_calibration.tag.size"); initial_intrinsic_calibration_tag_parameters_.spacing = - this->declare_parameter("initial_intrinsic_calibration_tag_spacing", 0.2); + this->declare_parameter("initial_intrinsic_calibration.tag.spacing"); std::vector intrinsic_calibration_tag_ids = this->declare_parameter>( - "intrinsic_calibration_tag_ids", std::vector{0}); + "initial_intrinsic_calibration.tag.ids", std::vector{0}); std::for_each( intrinsic_calibration_tag_ids.cbegin(), intrinsic_calibration_tag_ids.cend(), [&](auto & id) { initial_intrinsic_calibration_tag_parameters_.ids.insert(id); }); initial_intrinsic_calibration_board_cols_ = - this->declare_parameter("initial_intrinsic_calibration_board_cols", 8); + this->declare_parameter("initial_intrinsic_calibration.board_cols"); initial_intrinsic_calibration_board_rows_ = - this->declare_parameter("initial_intrinsic_calibration_board_rows", 6); + this->declare_parameter("initial_intrinsic_calibration.board_rows"); - apriltag_detector_parameters_.max_hamming = - this->declare_parameter("apriltag_max_hamming", 0); - apriltag_detector_parameters_.min_margin = - this->declare_parameter("apriltag_min_margin", 20.0); + apriltag_detector_parameters_.max_hamming = this->declare_parameter("apriltag.max_hamming"); + apriltag_detector_parameters_.min_margin = this->declare_parameter("apriltag.min_margin"); apriltag_detector_parameters_.max_out_of_plane_angle = - this->declare_parameter("apriltag_max_out_of_plane_angle", 90.0); - apriltag_detector_parameters_.max_reproj_error = - this->declare_parameter("apriltag_max_reproj_error", 10.0); + this->declare_parameter("apriltag.max_out_of_plane_angle"); + apriltag_detector_parameters_.max_reprojection_error = + this->declare_parameter("apriltag.max_reprojection_error"); apriltag_detector_parameters_.max_homography_error = - this->declare_parameter("apriltag_max_homography_error", 0.5); + this->declare_parameter("apriltag.max_homography_error"); apriltag_detector_parameters_.quad_decimate = - this->declare_parameter("apriltag_quad_decimate", 1.0); - apriltag_detector_parameters_.quad_sigma = - this->declare_parameter("apriltag_quad_sigma", 0.0); - apriltag_detector_parameters_.nthreads = this->declare_parameter("apriltag_nthreads", 1); - apriltag_detector_parameters_.debug = this->declare_parameter("apriltag_debug", false); + this->declare_parameter("apriltag.quad_decimate"); + apriltag_detector_parameters_.quad_sigma = this->declare_parameter("apriltag.quad_sigma"); + apriltag_detector_parameters_.nthreads = this->declare_parameter("apriltag.nthreads"); + apriltag_detector_parameters_.debug = this->declare_parameter("apriltag.debug"); apriltag_detector_parameters_.refine_edges = - this->declare_parameter("apriltag_refine_edges", true); + this->declare_parameter("apriltag.refine_edges"); for (const std::string & lidar_frame : calibration_lidar_frames_vector_) { lidartag_detections_sub_map_[lidar_frame] = @@ -330,6 +282,8 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( } markers_pub_ = this->create_publisher("markers", 10); + raw_detections_markers_pub_ = + this->create_publisher("raw_detections_markers", 10); visualization_timer_ = rclcpp::create_timer( this, get_clock(), std::chrono::seconds(1), @@ -337,27 +291,14 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( // Calibration API services // The service servers runs in a dedicated threads since they are blocking - for (const auto & calibration_sensor_frame : calibration_sensor_frames_vector_) { - const std::string & sensor_kit_frame = sensor_kit_frame_map_[calibration_sensor_frame]; - const std::string & calibration_sensor_parent_frame = - calibration_sensor_parent_frame_map_[calibration_sensor_frame]; - - srv_callback_groups_map_[calibration_sensor_frame] = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - calibration_api_srv_map_[calibration_sensor_frame] = - this->create_service( - calibration_service_names_map_[calibration_sensor_frame] + "/extrinsic_calibration", - [&]( - const std::shared_ptr request, - const std::shared_ptr - response) { - calibrationRequestCallback( - request, response, sensor_kit_frame, calibration_sensor_parent_frame, - calibration_sensor_frame); - }, - rmw_qos_profile_services_default, srv_callback_groups_map_[calibration_sensor_frame]); - } + calibration_api_srv_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + calibration_api_srv_ = this->create_service( + "/extrinsic_calibration", + std::bind( + &ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, calibration_api_srv_group_); // Scene related services add_external_camera_images_srv_ = this->create_service( @@ -373,7 +314,7 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( &ExtrinsicTagBasedBaseCalibrator::addCalibrationSensorDetectionsCallback, this, std::placeholders::_1, std::placeholders::_2)); - // Intrinsics realated services + // Intrinsics related services load_external_camera_intrinsics_srv_ = this->create_service( "load_external_camera_intrinsics", @@ -436,17 +377,32 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( } void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( - __attribute__((unused)) - const std::shared_ptr + [[maybe_unused]] const std::shared_ptr request, - __attribute__((unused)) - const std::shared_ptr - response, - const std::string & sensor_kit_frame, const std::string & calibration_sensor_parent_frame, - const std::string & calibration_sensor_frame) + [[maybe_unused]] const std::shared_ptr + response) { using std::chrono_literals::operator""s; + // Get some of the initial tfs before calibration + geometry_msgs::msg::Transform initial_base_link_to_lidar_msg; + Eigen::Isometry3d initial_base_link_to_lidar_pose; + + // We calibrate the lidar base link, not the lidar, so we need to compute that pose + try { + rclcpp::Time t = rclcpp::Time(0); + rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); + + initial_base_link_to_lidar_msg = + tf_buffer_->lookupTransform(base_frame_, main_calibration_sensor_frame_, t, timeout) + .transform; + + initial_base_link_to_lidar_pose = tf2::transformToEigen(initial_base_link_to_lidar_msg); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "Could not get the necessary tfs for calibration"); + return; + } + // Loop until the calibration finishes while (rclcpp::ok()) { rclcpp::sleep_for(1s); @@ -457,7 +413,7 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( } RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 30000, "Waiting for the calibration to end"); + this->get_logger(), *this->get_clock(), 60000, "Waiting for the calibration to end"); } Eigen::Vector3d translation; @@ -475,93 +431,40 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( calibrated_main_sensor_to_base_link_pose_.inv().matrix; Eigen::Matrix4d base_link_to_lidar_transform; cv::cv2eigen(base_link_to_lidar_transform_cv, base_link_to_lidar_transform); - Eigen::Affine3d base_link_to_lidar_pose(base_link_to_lidar_transform); - - geometry_msgs::msg::Transform sensor_kit_to_lidar_base_msg; - Eigen::Affine3d sensor_kit_to_lidar_base_pose; - - geometry_msgs::msg::Transform lidar_base_to_lidar_msg; - Eigen::Affine3d lidar_base_to_lidar_pose; - - geometry_msgs::msg::Transform initial_base_link_to_lidar_msg; - Eigen::Affine3d initial_base_link_to_lidar_pose; - - // We calibrate the lidar base link, not the lidar, so we need to compute that pose - try { - rclcpp::Time t = rclcpp::Time(0); - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - - sensor_kit_to_lidar_base_msg = - tf_buffer_->lookupTransform(sensor_kit_frame, calibration_sensor_parent_frame, t, timeout) - .transform; - - sensor_kit_to_lidar_base_pose = tf2::transformToEigen(sensor_kit_to_lidar_base_msg); - - lidar_base_to_lidar_msg = - tf_buffer_ - ->lookupTransform(calibration_sensor_parent_frame, calibration_sensor_frame, t, timeout) - .transform; - - lidar_base_to_lidar_pose = tf2::transformToEigen(lidar_base_to_lidar_msg); - - initial_base_link_to_lidar_msg = - tf_buffer_->lookupTransform(base_frame_, calibration_sensor_frame, t, timeout).transform; - - initial_base_link_to_lidar_pose = tf2::transformToEigen(initial_base_link_to_lidar_msg); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(this->get_logger(), "Could not get the necessary tfs for calibration"); - response->success = false; - } - - Eigen::Affine3d base_link_to_sensor_kit_pose = base_link_to_lidar_pose * - lidar_base_to_lidar_pose.inverse() * - sensor_kit_to_lidar_base_pose.inverse(); - - Eigen::Affine3d initial_base_link_to_sensor_kit_pose = initial_base_link_to_lidar_pose * - lidar_base_to_lidar_pose.inverse() * - sensor_kit_to_lidar_base_pose.inverse(); - - geometry_msgs::msg::Pose base_link_to_sensor_kit_msg = tf2::toMsg(base_link_to_sensor_kit_pose); - geometry_msgs::msg::Pose initial_base_link_to_sensor_kit_msg = - tf2::toMsg(initial_base_link_to_sensor_kit_pose); - (void)base_link_to_sensor_kit_msg; - (void)initial_base_link_to_sensor_kit_msg; - - response->success = true; - response->result_pose = base_link_to_sensor_kit_msg; + Eigen::Isometry3d base_link_to_lidar_pose(base_link_to_lidar_transform); + auto base_link_to_lidar_msg = tf2::eigenToTransform(base_link_to_lidar_pose).transform; // Display the initial and calibrated values - const auto & base_to_sensor_kit_rpy = - tier4_autoware_utils::getRPY(base_link_to_sensor_kit_msg.orientation); - const auto & initial_base_to_sensor_kit_rpy = - tier4_autoware_utils::getRPY(initial_base_link_to_sensor_kit_msg.orientation); + const auto & initial_base_to_lidar_rpy = + tier4_autoware_utils::getRPY(initial_base_link_to_lidar_msg.rotation); + const auto & base_to_lidar_rpy = tier4_autoware_utils::getRPY(base_link_to_lidar_msg.rotation); RCLCPP_INFO(this->get_logger(), "base_link: initial and calibrated statistics statistics"); RCLCPP_INFO( this->get_logger(), "\tinitial: x=%.5f y=%.5f z=%.5f roll=%.5f pitch=%.5f yaw=%.5f", - initial_base_link_to_sensor_kit_msg.position.x, initial_base_link_to_sensor_kit_msg.position.y, - initial_base_link_to_sensor_kit_msg.position.z, initial_base_to_sensor_kit_rpy.x, - initial_base_to_sensor_kit_rpy.y, initial_base_to_sensor_kit_rpy.z); + initial_base_link_to_lidar_msg.translation.x, initial_base_link_to_lidar_msg.translation.y, + initial_base_link_to_lidar_msg.translation.z, initial_base_to_lidar_rpy.x, + initial_base_to_lidar_rpy.y, initial_base_to_lidar_rpy.z); RCLCPP_INFO( this->get_logger(), "\tcalibrated: x=%.5f y=%.5f z=%.5f roll=%.5f pitch=%.5f yaw=%.5f", - base_link_to_sensor_kit_msg.position.x, base_link_to_sensor_kit_msg.position.y, - base_link_to_sensor_kit_msg.position.z, base_to_sensor_kit_rpy.x, base_to_sensor_kit_rpy.y, - base_to_sensor_kit_rpy.z); + base_link_to_lidar_msg.translation.x, base_link_to_lidar_msg.translation.y, + base_link_to_lidar_msg.translation.z, base_to_lidar_rpy.x, base_to_lidar_rpy.y, + base_to_lidar_rpy.z); // Display the correction in calibration - Eigen::Affine3d initial_base_link_to_calibrated_base_link_pose = + Eigen::Isometry3d initial_base_link_to_calibrated_base_link_pose = initial_base_link_to_lidar_pose * base_link_to_lidar_pose.inverse(); - Eigen::Matrix3d initial_base_link_to_calibrated_base_link_rot = + Eigen::Matrix3d initial_base_link_to_calibrated_base_link_rotation = initial_base_link_to_calibrated_base_link_pose.rotation(); Eigen::Vector3d initial_base_link_to_calibrated_base_link_translation = initial_base_link_to_calibrated_base_link_pose.translation(); Eigen::Vector3d initial_normal(0.0, 0.0, 1.0); Eigen::Vector3d optimized_norm = - initial_base_link_to_calibrated_base_link_rot * Eigen::Vector3d(0.0, 0.0, 1.0); + initial_base_link_to_calibrated_base_link_rotation * Eigen::Vector3d(0.0, 0.0, 1.0); const double normal_angle_diff = std::acos(initial_normal.dot(optimized_norm)); const double yaw_angle_diff = std::atan2( - initial_base_link_to_calibrated_base_link_rot(1, 0), - initial_base_link_to_calibrated_base_link_rot(0, 0)); + initial_base_link_to_calibrated_base_link_rotation(1, 0), + initial_base_link_to_calibrated_base_link_rotation(0, 0)); RCLCPP_INFO(this->get_logger(), "base_link: initial to calibrated statistics"); RCLCPP_INFO( @@ -575,6 +478,60 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( this->get_logger(), "\t y: %.3f m", initial_base_link_to_calibrated_base_link_translation.y()); RCLCPP_INFO( this->get_logger(), "\t z: %.3f m", initial_base_link_to_calibrated_base_link_translation.z()); + + Eigen::Vector4d initial_ground_model = + tier4_ground_plane_utils::poseToPlaneModel(initial_base_link_to_lidar_pose.inverse()); + RCLCPP_INFO(this->get_logger(), "Initial ground plane model"); + RCLCPP_INFO(this->get_logger(), "\ta=%.4f", initial_ground_model.x()); + RCLCPP_INFO(this->get_logger(), "\tb=%.4f", initial_ground_model.y()); + RCLCPP_INFO(this->get_logger(), "\tc=%.4f", initial_ground_model.z()); + RCLCPP_INFO(this->get_logger(), "\td=%.4f", initial_ground_model.w()); + + Eigen::Vector4d calibrated_ground_model = + tier4_ground_plane_utils::poseToPlaneModel(base_link_to_lidar_pose.inverse()); + RCLCPP_INFO(this->get_logger(), "Calibrated ground plane model"); + RCLCPP_INFO(this->get_logger(), "\ta=%.4f", calibrated_ground_model.x()); + RCLCPP_INFO(this->get_logger(), "\tb=%.4f", calibrated_ground_model.y()); + RCLCPP_INFO(this->get_logger(), "\tc=%.4f", calibrated_ground_model.z()); + RCLCPP_INFO(this->get_logger(), "\td=%.4f", calibrated_ground_model.w()); + + // Format the output + tier4_calibration_msgs::msg::CalibrationResult base_link_result; + base_link_result.message.data = + "Calibration successful. Base calibration does not provide a direct score"; + base_link_result.score = 0.f; + base_link_result.success = true; + base_link_result.transform_stamped = + tf2::eigenToTransform(cvToEigenPose(calibrated_main_sensor_to_base_link_pose_)); + base_link_result.transform_stamped.header.frame_id = main_calibration_sensor_frame_; + base_link_result.transform_stamped.child_frame_id = base_frame_; + response->results.push_back(base_link_result); + + UID main_sensor_uid = getMainSensorUID(); + + for (const auto & [sensor_uid, pose] : data_->optimized_sensor_poses_map) { + tier4_calibration_msgs::msg::CalibrationResult result; + result.message.data = + "Calibration successful. The error corresponds to reprojection error in pixel units"; + result.score = data_->optimized_sensor_residuals_map[sensor_uid]; + result.success = true; + result.transform_stamped = tf2::eigenToTransform(cvToEigenPose(*pose)); + result.transform_stamped.header.frame_id = main_calibration_sensor_frame_; + + if (sensor_uid == main_sensor_uid) { + continue; + } else if (sensor_uid.sensor_type == SensorType::CalibrationLidar) { + result.transform_stamped.child_frame_id = + calibration_lidar_frames_vector_[sensor_uid.calibration_sensor_id]; + } else if (sensor_uid.sensor_type == SensorType::CalibrationCamera) { + result.transform_stamped.child_frame_id = + calibration_camera_frames_vector_[sensor_uid.calibration_sensor_id]; + } else { + continue; + } + + response->results.push_back(result); + } } void ExtrinsicTagBasedBaseCalibrator::calibrationImageCallback( @@ -682,6 +639,7 @@ void ExtrinsicTagBasedBaseCalibrator::lidartagDetectionsCallback( void ExtrinsicTagBasedBaseCalibrator::visualizationTimerCallback() { visualization_msgs::msg::MarkerArray markers; + visualization_msgs::msg::MarkerArray raw_detections_markers; visualization_msgs::msg::Marker base_marker; base_marker.ns = "raw_detections"; @@ -839,30 +797,31 @@ void ExtrinsicTagBasedBaseCalibrator::visualizationTimerCallback() visualization_msgs::msg::Marker optimized_base_link_base_marker = base_marker; optimized_base_link_base_marker.ns = "optimized_base_link"; - cv::Affine3d initial_ground_pose, optimized_ground_pose; - if (computeGroundPlane( - data_->initial_ground_tag_poses_map, ground_tag_parameters_.size, initial_ground_pose)) { - addGrid(markers, initial_ground_pose, 100, 0.2, initial_ground_base_marker); - addAxesMarkers(markers, 0.5, initial_ground_pose, initial_ground_base_marker); + std::optional initial_ground_pose = + computeGroundPlane(data_->initial_ground_tag_poses_map, ground_tag_parameters_.size); + std::optional optimized_ground_pose = + computeGroundPlane(data_->optimized_ground_tag_poses_map, ground_tag_parameters_.size); + + if (initial_ground_pose.has_value()) { + addGrid(markers, initial_ground_pose.value(), 100, 0.2, initial_ground_base_marker); + addAxesMarkers(markers, 0.5, initial_ground_pose.value(), initial_ground_base_marker); if (data_->initial_left_wheel_tag_pose && data_->initial_right_wheel_tag_pose) { cv::Affine3d initial_base_link_pose = computeBaseLink( *data_->initial_left_wheel_tag_pose, *data_->initial_right_wheel_tag_pose, - initial_ground_pose); + initial_ground_pose.value()); addAxesMarkers(markers, 0.5, initial_base_link_pose, initial_base_link_base_marker); } } - if (computeGroundPlane( - data_->optimized_ground_tag_poses_map, ground_tag_parameters_.size, - optimized_ground_pose)) { - addGrid(markers, optimized_ground_pose, 100, 0.2, optimized_ground_base_marker); - addAxesMarkers(markers, 1.0, optimized_ground_pose, optimized_ground_base_marker); + if (optimized_ground_pose.has_value()) { + addGrid(markers, optimized_ground_pose.value(), 100, 0.2, optimized_ground_base_marker); + addAxesMarkers(markers, 1.0, optimized_ground_pose.value(), optimized_ground_base_marker); if (data_->optimized_left_wheel_tag_pose && data_->optimized_right_wheel_tag_pose) { cv::Affine3d optimized_base_link_pose = computeBaseLink( *data_->optimized_left_wheel_tag_pose, *data_->optimized_right_wheel_tag_pose, - optimized_ground_pose); + optimized_ground_pose.value()); addAxesMarkers(markers, 1.0, optimized_base_link_pose, optimized_base_link_base_marker); } } @@ -881,11 +840,12 @@ void ExtrinsicTagBasedBaseCalibrator::visualizationTimerCallback() cv::cv2eigen( calibrated_main_sensor_to_base_link_pose_.matrix, main_sensor_to_base_link_transform); + std::vector transforms_msgs; geometry_msgs::msg::TransformStamped tf_msg = tf2::eigenToTransform(Eigen::Affine3d(main_sensor_to_base_link_transform)); tf_msg.header.frame_id = main_calibration_sensor_frame_; tf_msg.child_frame_id = "estimated_base_link"; - tf_broadcaster_.sendTransform(tf_msg); + transforms_msgs.push_back(tf_msg); // Publish the tf to all external cameras for (const auto & [uid, sensor_pose_cv] : data_->optimized_sensor_poses_map) { @@ -896,9 +856,37 @@ void ExtrinsicTagBasedBaseCalibrator::visualizationTimerCallback() tf2::eigenToTransform(Eigen::Affine3d(sensor_pose_eigen)); tf_msg.header.frame_id = main_calibration_sensor_frame_; tf_msg.child_frame_id = uid.toString(); - tf_broadcaster_.sendTransform(tf_msg); + transforms_msgs.push_back(tf_msg); } + if (publish_tfs_) { + // Publish all the resulting tfs (main sensor to all frames) + // This will probably destroy the current tf tree so proceed with auction + auto main_sensor_uid = getMainSensorUID(); + + for (const auto & [sensor_uid, pose] : data_->optimized_sensor_poses_map) { + geometry_msgs::msg::TransformStamped transform_stamped_msg; + transform_stamped_msg = tf2::eigenToTransform(cvToEigenPose(*pose)); + transform_stamped_msg.header.frame_id = main_calibration_sensor_frame_; + + if (sensor_uid == main_sensor_uid) { + continue; + } else if (sensor_uid.sensor_type == SensorType::CalibrationLidar) { + transform_stamped_msg.child_frame_id = + calibration_lidar_frames_vector_[sensor_uid.calibration_sensor_id]; + } else if (sensor_uid.sensor_type == SensorType::CalibrationCamera) { + transform_stamped_msg.child_frame_id = + calibration_camera_frames_vector_[sensor_uid.calibration_sensor_id]; + } else { + continue; + } + + transforms_msgs.push_back(transform_stamped_msg); + } + } + + tf_broadcaster_.sendTransform(transforms_msgs); + visualization_msgs::msg::Marker detections_base_marker = base_marker; for (std::size_t scene_index = 0; scene_index < data_->scenes.size(); scene_index++) { @@ -925,8 +913,8 @@ void ExtrinsicTagBasedBaseCalibrator::visualizationTimerCallback() for (const auto & grid_detection : grid_detections) { for (const auto & detection : grid_detection.sub_detections) { addTagMarkers( - markers, std::to_string(detection.id), tag_parameters, color, detection.pose, - detections_base_marker); + raw_detections_markers, std::to_string(detection.id), tag_parameters, color, + detection.pose, detections_base_marker); } } } @@ -937,7 +925,13 @@ void ExtrinsicTagBasedBaseCalibrator::visualizationTimerCallback() markers.markers[marker_index].id = marker_index; } + for (std::size_t marker_index = 0; marker_index < raw_detections_markers.markers.size(); + marker_index++) { + raw_detections_markers.markers[marker_index].id = marker_index; + } + markers_pub_->publish(markers); + raw_detections_markers_pub_->publish(raw_detections_markers); } std_msgs::msg::ColorRGBA ExtrinsicTagBasedBaseCalibrator::getNextColor() @@ -958,6 +952,32 @@ std_msgs::msg::ColorRGBA ExtrinsicTagBasedBaseCalibrator::getNextColor() return color; } +UID ExtrinsicTagBasedBaseCalibrator::getMainSensorUID() const +{ + UID main_sensor_uid; + + for (const auto & detections : data_->scenes[0].calibration_cameras_detections) { + if (detections.calibration_frame == main_calibration_sensor_frame_) { + return UID::makeSensorUID(SensorType::CalibrationCamera, detections.calibration_camera_id); + } + } + + for (const auto & detections : data_->scenes[0].calibration_lidars_detections) { + if (detections.calibration_frame == main_calibration_sensor_frame_) { + return UID::makeSensorUID(SensorType::CalibrationLidar, detections.calibration_lidar_id); + } + } + + return main_sensor_uid; +} + +Eigen::Isometry3d ExtrinsicTagBasedBaseCalibrator::cvToEigenPose(const cv::Affine3d & pose) +{ + Eigen::Matrix4d matrix; + cv::cv2eigen(pose.matrix, matrix); + return Eigen::Isometry3d(matrix); +} + bool ExtrinsicTagBasedBaseCalibrator::addExternalCameraImagesCallback( const std::shared_ptr request, std::shared_ptr response) @@ -998,7 +1018,7 @@ bool ExtrinsicTagBasedBaseCalibrator::addExternalCameraImagesCallback( scenes_external_camera_images_[scene_id] = request->files_list[scene_id].files; RCLCPP_INFO( - this->get_logger(), "Addded %lu external images to scene id=%ld (scenes=%lu)", + this->get_logger(), "Added %lu external images to scene id=%ld (scenes=%lu)", request->files_list[scene_id].files.size(), scene_id, num_scenes); } @@ -1007,9 +1027,8 @@ bool ExtrinsicTagBasedBaseCalibrator::addExternalCameraImagesCallback( } bool ExtrinsicTagBasedBaseCalibrator::addCalibrationSensorDetectionsCallback( - __attribute__((unused)) const std::shared_ptr - request, - __attribute__((unused)) std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) { response->success = false; @@ -1151,9 +1170,8 @@ bool ExtrinsicTagBasedBaseCalibrator::calibrateExternalIntrinsicsCallback( } bool ExtrinsicTagBasedBaseCalibrator::preprocessScenesCallback( - __attribute__((unused)) const std::shared_ptr - request, - __attribute__((unused)) std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) { std::size_t num_external_camera_scenes = scenes_external_camera_images_.size(); @@ -1236,8 +1254,7 @@ bool ExtrinsicTagBasedBaseCalibrator::preprocessScenesCallback( CalibrationScene scene = calibration_scene_extractor.processScene( scenes_calibration_camera_images, scene_calibration_lidartag_detections, scene_calibration_apriltag_detections, calibration_lidar_frames_vector_, - calibration_camera_frames_vector_, main_calibration_sensor_frame_, - scenes_external_camera_images_[scene_index]); + calibration_camera_frames_vector_, scenes_external_camera_images_[scene_index]); data_->scenes.push_back(scene); } @@ -1347,25 +1364,10 @@ bool ExtrinsicTagBasedBaseCalibrator::preprocessScenesCallback( } bool ExtrinsicTagBasedBaseCalibrator::calibrationCallback( - __attribute__((unused)) const std::shared_ptr - request, - __attribute__((unused)) std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) { - UID main_sensor_uid; - - for (const auto & detections : data_->scenes[0].calibration_cameras_detections) { - if (detections.calibration_frame == main_calibration_sensor_frame_) { - main_sensor_uid = - UID::makeSensorUID(SensorType::CalibrationCamera, detections.calibration_camera_id); - } - } - - for (const auto & detections : data_->scenes[0].calibration_lidars_detections) { - if (detections.calibration_frame == main_calibration_sensor_frame_) { - main_sensor_uid = - UID::makeSensorUID(SensorType::CalibrationLidar, detections.calibration_lidar_id); - } - } + UID main_sensor_uid = getMainSensorUID(); assert(main_sensor_uid.isValid()); UID left_wheel_uid = UID::makeTagUID(TagType::WheelTag, -1, left_wheel_tag_id); @@ -1411,14 +1413,42 @@ bool ExtrinsicTagBasedBaseCalibrator::calibrationCallback( data_->optimized_sensor_poses_map = data_->initial_sensor_poses_map; data_->optimized_tag_poses_map = data_->initial_tag_poses_map; + if (ba_fixed_ground_plane_model_) { + Eigen::Isometry3d initial_lidar_to_base_link_eigen; + + try { + rclcpp::Time t = rclcpp::Time(0); + rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); + + auto initial_lidar_to_base_link_msg = + tf_buffer_->lookupTransform(main_calibration_sensor_frame_, base_frame_, t, timeout) + .transform; + + initial_lidar_to_base_link_eigen = tf2::transformToEigen(initial_lidar_to_base_link_msg); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "Could not get the necessary tfs for calibration"); + return false; + } + + Eigen::Vector4d ground_model = + tier4_ground_plane_utils::poseToPlaneModel(initial_lidar_to_base_link_eigen); + RCLCPP_INFO( + this->get_logger(), + "Going to use the initial calibration to derive the ground plane used during calibration"); + RCLCPP_INFO(this->get_logger(), "\ta=%.4f", ground_model.x()); + RCLCPP_INFO(this->get_logger(), "\tb=%.4f", ground_model.y()); + RCLCPP_INFO(this->get_logger(), "\tc=%.4f", ground_model.z()); + RCLCPP_INFO(this->get_logger(), "\td=%.4f", ground_model.w()); + + calibration_problem_.setFixedSharedGroundPlane(ba_fixed_ground_plane_model_, ground_model); + } else { + calibration_problem_.setFixedSharedGroundPlane( + ba_fixed_ground_plane_model_, Eigen::Vector4d::Zero()); + } + calibration_problem_.setOptimizeIntrinsics(ba_optimize_intrinsics_); calibration_problem_.setShareIntrinsics(ba_share_intrinsics_); calibration_problem_.setForceSharedGroundPlane(ba_force_shared_ground_plane_); - calibration_problem_.setFixedSharedGroundPlane( - ba_fixed_ground_plane_model_, - Eigen::Vector4d( - ba_fixed_ground_plane_model_a_, ba_fixed_ground_plane_model_b_, - ba_fixed_ground_plane_model_c_, ba_fixed_ground_plane_model_d_)); calibration_problem_.setCalibrationLidarIntrinsics(virtual_lidar_f_); calibration_problem_.setOptimizationWeights( calibration_camera_optimization_weight_, calibration_lidar_optimization_weight_, @@ -1434,24 +1464,28 @@ bool ExtrinsicTagBasedBaseCalibrator::calibrationCallback( calibration_problem_.solve(); calibration_problem_.placeholdersToData(); calibration_problem_.evaluate(); - calibration_problem_.writeDebugImages(); + if (write_debug_images_) { + calibration_problem_.writeDebugImages(); + } + calibration_problem_.printCalibrationResults(); RCLCPP_INFO(this->get_logger(), "Finished optimization"); // Derive the base link pose - cv::Affine3d ground_pose; + std::optional ground_pose = + computeGroundPlane(data_->optimized_ground_tag_poses_map, ground_tag_parameters_.size); if ( - !computeGroundPlane( - data_->optimized_ground_tag_poses_map, ground_tag_parameters_.size, ground_pose) || - !data_->optimized_left_wheel_tag_pose || !data_->optimized_right_wheel_tag_pose) { + !ground_pose.has_value() || !data_->optimized_left_wheel_tag_pose || + !data_->optimized_right_wheel_tag_pose) { RCLCPP_ERROR(this->get_logger(), "Could not compute the base link"); response->success = false; return false; } calibrated_main_sensor_to_base_link_pose_ = computeBaseLink( - *data_->optimized_left_wheel_tag_pose, *data_->optimized_right_wheel_tag_pose, ground_pose); + *data_->optimized_left_wheel_tag_pose, *data_->optimized_right_wheel_tag_pose, + ground_pose.value()); calibration_done_ = true; response->success = true; @@ -1464,7 +1498,7 @@ bool ExtrinsicTagBasedBaseCalibrator::loadDatabaseCallback( { RCLCPP_INFO(this->get_logger(), "Loading database..."); std::ifstream ifs(request->files.files[0]); - boost::archive::text_iarchive ia(ifs); + boost::archive::text_iarchive ia(ifs); // cSpell:ignore iarchive ia >> data_; @@ -1523,7 +1557,7 @@ bool ExtrinsicTagBasedBaseCalibrator::saveDatabaseCallback( { RCLCPP_INFO(this->get_logger(), "Saving database"); std::ofstream ofs(request->files.files[0]); - boost::archive::text_oarchive oa(ofs); + boost::archive::text_oarchive oa(ofs); // cSpell:ignore oarchive oa << data_; @@ -1533,4 +1567,4 @@ bool ExtrinsicTagBasedBaseCalibrator::saveDatabaseCallback( return true; } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/visualization.cpp b/sensor/tag_based_sfm_calibrator/src/visualization.cpp similarity index 97% rename from sensor/extrinsic_tag_based_base_calibrator/src/visualization.cpp rename to sensor/tag_based_sfm_calibrator/src/visualization.cpp index 0dd2181b..fa5be7d5 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/visualization.cpp +++ b/sensor/tag_based_sfm_calibrator/src/visualization.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include #include #include +#include +#include #include -namespace extrinsic_tag_based_base_calibrator +namespace tag_based_sfm_calibrator { void addTextMarker( @@ -302,4 +302,4 @@ void drawAxes( static_cast(std::max(tag_size / 512.0, 1.0)), cv::LINE_AA); } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/__init__.py b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/__init__.py similarity index 100% rename from sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/__init__.py rename to sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/__init__.py diff --git a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/calibrator_ui.py b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/calibrator_ui.py similarity index 98% rename from sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/calibrator_ui.py rename to sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/calibrator_ui.py index 7fbe1232..0b6f30ec 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/calibrator_ui.py +++ b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/calibrator_ui.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -324,13 +324,9 @@ def save_database_status_callback(self, status): self.check_status() def add_external_images_button_callback(self): - # Here simply select folder - # Iterate folder looking for scene{i} - # Call the same service - output_folder = QFileDialog.getExistingDirectory( None, - "Select directory to save the calibration result", + "Select directory to load the external images from", ".", QFileDialog.ShowDirsOnly | QFileDialog.DontResolveSymlinks, ) diff --git a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/ros_interface.py b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/ros_interface.py similarity index 98% rename from sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/ros_interface.py rename to sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/ros_interface.py index 158ac000..a1dbe09e 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/ros_interface.py +++ b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/ros_interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. @@ -88,7 +88,7 @@ def __call__(self, files_list): class RosInterface(Node): def __init__(self): - super().__init__("extrinsic_tag_based_base_calibrator") + super().__init__("tag_based_sfm_calibrator") self.ros_context = None self.ros_executor = None