From c5c3003f81ca326973ca1d845a3889c94ef1f27a Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 5 Oct 2023 16:30:25 +0900 Subject: [PATCH 01/49] Implementing the new calibration manager Signed-off-by: Kenzo Lobos-Tsunekawa --- common/tier4_calibration_msgs/CMakeLists.txt | 2 + .../msg/CalibrationResult.msg | 4 + .../srv/NewExtrinsicCalibrator.srv | 2 + .../tier4_tag_utils/apriltag_hypothesis.hpp | 7 - .../tier4_tag_utils/lidartag_hypothesis.hpp | 9 - .../include/tier4_tag_utils/types.hpp | 34 -- .../tier4_tag_utils/src/apriltag_filter.cpp | 1 - .../src/apriltag_hypothesis.cpp | 64 +-- .../tier4_tag_utils/src/lidartag_filter.cpp | 2 - .../src/lidartag_hypothesis.cpp | 226 ++-------- .../launch/aip_x2/tag_based_lidars.launch.xml | 2 +- .../image_view.py | 17 + .../ros_interface.py | 39 +- .../launch/calibrator.launch.xml | 24 -- .../CMakeLists.txt | 8 +- .../brute_force_matcher.hpp | 6 +- .../calibration_estimator.hpp | 13 +- .../extrinsic_tag_based_pnp_calibrator.hpp} | 31 +- .../tag_calibrator_visualizer.hpp | 8 +- .../launch/apriltag_16h5.launch.py | 0 .../launch/calibrator.launch.xml} | 37 +- .../package.xml | 5 +- .../tag_calib_camera0_pandar_40p_right.rviz | 0 .../rviz/tag_calib_camera0_velodyne_top.rviz | 403 +++--------------- .../tag_calib_camera1_pandar_40p_right.rviz | 0 .../rviz/tag_calib_camera1_velodyne_top.rviz | 0 .../tag_calib_camera2_pandar_40p_right.rviz | 0 .../rviz/tag_calib_camera2_velodyne_top.rviz | 0 .../tag_calib_camera3_pandar_40p_rear.rviz | 0 .../rviz/tag_calib_camera3_velodyne_top.rviz | 0 .../tag_calib_camera4_pandar_40p_left.rviz | 0 .../rviz/tag_calib_camera4_velodyne_top.rviz | 0 .../tag_calib_camera5_pandar_40p_left.rviz | 0 .../rviz/tag_calib_camera5_velodyne_top.rviz | 0 .../tag_calib_camera6_pandar_40p_front.rviz | 0 ...raffic_light_left_camera_velodyne_top.rviz | 0 .../src/brute_force_matcher.cpp | 2 +- .../src/calibration_estimator.cpp | 13 +- .../extrinsic_tag_based_pnp_calibrator.cpp} | 132 +++--- .../src/main.cpp | 6 +- .../src/tag_calibrator_visualizer.cpp | 4 +- .../tag_based_pnp_calibrator.launch.xml | 43 ++ .../base_lidar_calibration.launch.xml | 13 + .../tier4_base_lidar_calibration.launch.xml | 13 + .../tag_based_pnp_calibrator.launch.xml | 61 +++ .../__init__.py | 0 .../calibration_manager_model.py | 56 +++ .../calibrator.py | 155 +++++++ .../calibrator_base.py | 205 +++++++++ .../calibrator_registry.py | 67 +++ .../calibrator_wrapper.py | 185 ++++++++ .../calibrators/__init__.py | 8 + .../calibrators/default_project/__init__.py | 3 + .../tag_based_pnp_calibrator.py | 33 ++ .../calibrators/dummy_project/__init__.py | 11 + .../dummy_base_lidar_calibrator.py | 21 + .../dummy_camera_camera_calibrator.py | 29 ++ .../dummy_camera_lidar_calibrator.py | 29 ++ .../dummy_lidar_lidar_calibrator.py | 32 ++ .../tier4_dummy_project/__init__.py | 3 + .../tier4_dummy_base_lidar_calibrator.py | 39 ++ .../calibrators/xx1_15/__init__.py | 11 + .../xx1_15/dummy_base_lidar_calibrator.py | 21 + .../xx1_15/dummy_camera_camera_calibrator.py | 29 ++ .../xx1_15/dummy_lidar_lidar_calibrator.py | 32 ++ .../xx1_15/tag_based_pnp_calibrator.py | 54 +++ .../new_extrinsic_calibration_manager.py | 300 +++++++++++++ .../ros_interface.py | 138 ++++++ .../types.py | 15 + .../utils.py | 56 +++ .../views/calibrator_selector_view.py | 92 ++++ .../views/launcher_configuration_view.py | 197 +++++++++ .../views/tf_view.py | 367 ++++++++++++++++ .../package.xml | 22 + .../new_extrinsic_calibration_manager | 0 .../setup.cfg | 4 + .../setup.py | 51 +++ .../test/test_pep257.py | 23 + 78 files changed, 2659 insertions(+), 860 deletions(-) create mode 100644 common/tier4_calibration_msgs/msg/CalibrationResult.msg create mode 100644 common/tier4_calibration_msgs/srv/NewExtrinsicCalibrator.srv delete mode 100644 common/tier4_tag_utils/include/tier4_tag_utils/types.hpp delete mode 100644 sensor/extrinsic_tag_based_calibrator/launch/calibrator.launch.xml rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/CMakeLists.txt (70%) rename sensor/{extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator}/brute_force_matcher.hpp (85%) rename sensor/{extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator}/calibration_estimator.hpp (94%) rename sensor/{extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/extrinsic_tag_based_calibrator.hpp => extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp} (79%) rename sensor/{extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator}/tag_calibrator_visualizer.hpp (93%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/launch/apriltag_16h5.launch.py (100%) rename sensor/{extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml => extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml} (67%) mode change 100755 => 100644 rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/package.xml (89%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera0_pandar_40p_right.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera0_velodyne_top.rviz (65%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera1_pandar_40p_right.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera1_velodyne_top.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera2_pandar_40p_right.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera2_velodyne_top.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera3_pandar_40p_rear.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera3_velodyne_top.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera4_pandar_40p_left.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera4_velodyne_top.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera5_pandar_40p_left.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera5_velodyne_top.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_camera6_pandar_40p_front.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz (100%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/src/brute_force_matcher.cpp (99%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/src/calibration_estimator.cpp (98%) rename sensor/{extrinsic_tag_based_calibrator/src/extrinsic_tag_based_calibrator.cpp => extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp} (79%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/src/main.cpp (80%) rename sensor/{extrinsic_tag_based_calibrator => extrinsic_tag_based_pnp_calibrator}/src/tag_calibrator_visualizer.cpp (99%) create mode 100644 sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py create mode 100644 sensor/new_extrinsic_calibration_manager/package.xml create mode 100644 sensor/new_extrinsic_calibration_manager/resource/new_extrinsic_calibration_manager create mode 100644 sensor/new_extrinsic_calibration_manager/setup.cfg create mode 100644 sensor/new_extrinsic_calibration_manager/setup.py create mode 100644 sensor/new_extrinsic_calibration_manager/test/test_pep257.py diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt index 9fbd6245..ed46d69b 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" @@ -29,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ExtrinsicCalibrator.srv" "srv/Frame.srv" "srv/IntrinsicsOptimizer.srv" + "srv/NewExtrinsicCalibrator.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/NewExtrinsicCalibrator.srv b/common/tier4_calibration_msgs/srv/NewExtrinsicCalibrator.srv new file mode 100644 index 00000000..13c3fad2 --- /dev/null +++ b/common/tier4_calibration_msgs/srv/NewExtrinsicCalibrator.srv @@ -0,0 +1,2 @@ +--- +tier4_calibration_msgs/CalibrationResult[] results 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..2342aa68 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 @@ -19,7 +19,6 @@ #include #include #include -#include #include @@ -57,7 +56,6 @@ class ApriltagHypothesis bool converged() const; - void setDynamicsModel(DynamicsModel dynamics_model); void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); void setMaxConvergenceThreshold(double transl); @@ -68,11 +66,7 @@ class ApriltagHypothesis 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_; @@ -89,7 +83,6 @@ class ApriltagHypothesis // 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/lidartag_hypothesis.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp index 785decb3..1eee9295 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 @@ -19,7 +19,6 @@ #include #include #include -#include #include @@ -48,16 +47,13 @@ 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); @@ -70,9 +66,6 @@ class LidartagHypothesis 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,8 +80,6 @@ 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_; 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..27ab4123 100644 --- a/common/tier4_tag_utils/src/apriltag_filter.cpp +++ b/common/tier4_tag_utils/src/apriltag_filter.cpp @@ -14,7 +14,6 @@ #include #include -#include #include diff --git a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp index e8157f70..49416f0e 100644 --- a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp @@ -20,10 +20,7 @@ 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) { } @@ -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; @@ -222,11 +203,6 @@ 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; @@ -248,16 +224,6 @@ void ApriltagHypothesis::setProcessNoise(double transl) { process_noise_transl_ 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]; @@ -287,22 +253,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); + cv::Mat kalman_state(2, 1, CV_64F); - 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; - } 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; - - return kalman_state; - } + return kalman_state; } } // namespace tier4_tag_utils diff --git a/common/tier4_tag_utils/src/lidartag_filter.cpp b/common/tier4_tag_utils/src/lidartag_filter.cpp index 81f44acf..abc10a4e 100644 --- a/common/tier4_tag_utils/src/lidartag_filter.cpp +++ b/common/tier4_tag_utils/src/lidartag_filter.cpp @@ -14,7 +14,6 @@ #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -103,7 +102,6 @@ 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()); diff --git a/common/tier4_tag_utils/src/lidartag_hypothesis.cpp b/common/tier4_tag_utils/src/lidartag_hypothesis.cpp index f51d0315..90a8703e 100644 --- a/common/tier4_tag_utils/src/lidartag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/lidartag_hypothesis.cpp @@ -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; } @@ -200,56 +172,14 @@ double LidartagHypothesis::getTransCov() const 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); -} - 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)}); - + double max_rot_cov = std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); return std::sqrt(max_rot_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 { @@ -266,15 +196,9 @@ bool LidartagHypothesis::converged() const double max_transl_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_transl_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_rot_cov = std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); if ( std::sqrt(max_transl_cov) > convergence_transl_ || @@ -297,11 +221,6 @@ 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; @@ -341,16 +260,6 @@ void LidartagHypothesis::setProcessNoise( 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); @@ -474,30 +383,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 +414,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/extrinsic_calibration_manager/launch/aip_x2/tag_based_lidars.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_lidars.launch.xml index bbfa0e77..9082a727 100644 --- 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 @@ -204,7 +204,7 @@ /--> - + diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py index 51b4bdb1..10819f61 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py @@ -682,6 +682,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 +703,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], diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py index ea2d5e98..0f5e1aea 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py @@ -60,7 +60,6 @@ def __init__(self): 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) @@ -68,9 +67,6 @@ def __init__(self): 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 = ( @@ -255,39 +251,12 @@ def set_paused(self, 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) + camera_optical_lidar_transform ) - self.output_transform_msg.header.frame_id = self.camera_parent_frame - self.output_transform_msg.child_frame_id = self.camera_frame + + self.output_transform_msg.header.frame_id = self.camera_frame + self.output_transform_msg.child_frame_id = self.lidar_frame self.new_output_tf = True def optimize_camera_intrinsics(self, object_points, image_points): 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/CMakeLists.txt b/sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt similarity index 70% rename from sensor/extrinsic_tag_based_calibrator/CMakeLists.txt rename to sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt index 63c39e47..3f81105c 100644 --- a/sensor/extrinsic_tag_based_calibrator/CMakeLists.txt +++ b/sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_tag_based_calibrator) +project(extrinsic_tag_based_pnp_calibrator) find_package(autoware_cmake REQUIRED) find_package(OpenCV REQUIRED) @@ -14,15 +14,15 @@ ament_export_include_directories( # COMPILE THE SOURCE #======================================================================== -ament_auto_add_executable(extrinsic_tag_based_calibrator +ament_auto_add_executable(extrinsic_tag_based_pnp_calibrator src/brute_force_matcher.cpp src/calibration_estimator.cpp - src/extrinsic_tag_based_calibrator.cpp + src/extrinsic_tag_based_pnp_calibrator.cpp src/tag_calibrator_visualizer.cpp src/main.cpp ) -target_link_libraries(extrinsic_tag_based_calibrator +target_link_libraries(extrinsic_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/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp similarity index 85% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/brute_force_matcher.hpp rename to sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp index a5e30958..8f157316 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/brute_force_matcher.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp @@ -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 EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ +#define EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ #include #include @@ -34,4 +34,4 @@ 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 // EXTRINSIC_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/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp similarity index 94% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/calibration_estimator.hpp rename to sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp index 56e617c7..bdb9323b 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/calibration_estimator.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp @@ -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 EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#define EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ #include #include @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -40,13 +39,9 @@ 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( @@ -75,7 +70,6 @@ class CalibrationEstimator void getFilteredPose(cv::Matx31d & trans_vector, cv::Matx33d & rot_matrix) 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); @@ -120,7 +114,6 @@ class CalibrationEstimator const std::vector & object_points, const std::vector & image_points); // Parameters - tier4_tag_utils::DynamicsModel dynamics_model_; // Calibration convergence criteria int convergence_min_pairs_; @@ -174,4 +167,4 @@ class CalibrationEstimator cv::Matx31d hypothesis_translation_vector_, observation_translation_vector_; }; -#endif // EXTRINSIC_TAG_BASED_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp similarity index 79% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/extrinsic_tag_based_calibrator.hpp rename to sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp index 26c23fb6..80d79e99 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/extrinsic_tag_based_calibrator.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp @@ -12,11 +12,11 @@ // 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 EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__EXTRINSIC_TAG_BASED_PNP_CALIBRATOR_HPP_ +#define EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__EXTRINSIC_TAG_BASED_PNP_CALIBRATOR_HPP_ -#include -#include +#include +#include #include #include #include @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -50,10 +50,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( @@ -71,8 +71,8 @@ class ExtrinsicTagBasedCalibrator : public rclcpp::Node void clickedPointCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_msg); void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); void tfTimerCallback(); void manualCalibrationTimerCallback(); @@ -87,10 +87,7 @@ 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_; // Filter parameters @@ -123,7 +120,7 @@ class ExtrinsicTagBasedCalibrator : public rclcpp::Node rclcpp::TimerBase::SharedPtr calib_timer_; rclcpp::TimerBase::SharedPtr tf_timer_; - rclcpp::Service::SharedPtr service_server_; + rclcpp::Service::SharedPtr service_server_; // Threading, sync, and result std::mutex mutex_; @@ -142,15 +139,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 // EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__EXTRINSIC_TAG_BASED_PNP_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/tag_calibrator_visualizer.hpp b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp similarity index 93% rename from sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/tag_calibrator_visualizer.hpp rename to sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp index a0a7734e..6137ea30 100644 --- a/sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/tag_calibrator_visualizer.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp @@ -12,11 +12,11 @@ // 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 EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ +#define EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ #include -#include +#include #include #include #include @@ -123,4 +123,4 @@ class TagCalibratorVisualizer cv::Affine3d camera_base_transform_; }; -#endif // EXTRINSIC_TAG_BASED_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ +#endif // EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ diff --git a/sensor/extrinsic_tag_based_calibrator/launch/apriltag_16h5.launch.py b/sensor/extrinsic_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/extrinsic_tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py diff --git a/sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml old mode 100755 new mode 100644 similarity index 67% rename from sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml rename to sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml index 2875a304..784ebe2b --- a/sensor/extrinsic_tag_based_calibrator/launch/tag_calibrator.launch.xml +++ b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -1,52 +1,42 @@ - - + - - - - - - - - - + + + + - + - + - - - - + - - + - - + + + - - + @@ -69,5 +59,6 @@ + diff --git a/sensor/extrinsic_tag_based_calibrator/package.xml b/sensor/extrinsic_tag_based_pnp_calibrator/package.xml similarity index 89% rename from sensor/extrinsic_tag_based_calibrator/package.xml rename to sensor/extrinsic_tag_based_pnp_calibrator/package.xml index c797b8a5..cd5f2549 100644 --- a/sensor/extrinsic_tag_based_calibrator/package.xml +++ b/sensor/extrinsic_tag_based_pnp_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_tag_based_calibrator + extrinsic_tag_based_pnp_calibrator 0.0.1 - The extrinsic_tag_based_calibrator package + The extrinsic_tag_based_pnp_calibrator package Kenzo Lobos Tsunekawa BSD @@ -13,6 +13,7 @@ autoware_cmake apriltag_msgs + backward_ros cv_bridge geometry_msgs image_geometry diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz similarity index 65% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz index e08c4580..f5b22c71 100644 --- a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz +++ b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz @@ -5,21 +5,12 @@ Panels: 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 + Tree Height: 797 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -84,71 +75,6 @@ Visualization Manager: 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: @@ -198,7 +124,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 78 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 Name: Points of Interest @@ -213,7 +139,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/whole_edged_pc + Value: /lidartag/whole_edged_pc Use Fixed Frame: true Use rainbow: true Value: false @@ -232,9 +158,9 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 49 + Max Intensity: 101 Min Color: 0; 0; 0 - Min Intensity: 1 + Min Intensity: 0 Name: Clusters Position Transformer: XYZ Selectable: true @@ -247,7 +173,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/cluster_edge_pc + Value: /lidartag/cluster_edge_pc Use Fixed Frame: true Use rainbow: true Value: false @@ -266,7 +192,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 89 + Max Intensity: 196 Min Color: 0; 0; 0 Min Intensity: 2 Name: Filled Clusters @@ -281,7 +207,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/detected_pc + Value: /lidartag/detected_pc Use Fixed Frame: true Use rainbow: true Value: false @@ -295,7 +221,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/boundary_marker + Value: /lidartag/boundary_marker Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -307,7 +233,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/cluster_marker + Value: /lidartag/cluster_marker Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -324,9 +250,9 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 77 + Max Intensity: 101 Min Color: 0; 0; 0 - Min Intensity: 27 + Min Intensity: 33 Name: Boundary Points Position Transformer: XYZ Selectable: true @@ -339,7 +265,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/boundary_pts + Value: /lidartag/boundary_pts Use Fixed Frame: true Use rainbow: true Value: false @@ -358,9 +284,9 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 2.369355800876173e-38 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 2.369355800876173e-38 Name: Estimated Corners (PCA) Position Transformer: XYZ Selectable: true @@ -373,7 +299,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/transformed_points_tag + Value: /lidartag/transformed_points_tag Use Fixed Frame: true Use rainbow: true Value: false @@ -407,7 +333,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/transformed_points + Value: /lidartag/transformed_points Use Fixed Frame: true Use rainbow: true Value: false @@ -426,9 +352,9 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 93 + Max Intensity: 101 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 1 Name: Initial Transformed Points Position Transformer: XYZ Selectable: true @@ -441,7 +367,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/initial_template_points + Value: /lidartag/initial_template_points Use Fixed Frame: true Use rainbow: true Value: false @@ -456,19 +382,19 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/tag_frame + Value: /lidartag/tag_frame Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: ID Namespaces: - Text-1: true + Text3: 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: /lidartag/id_markers Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -485,7 +411,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 98 + Max Intensity: 158 Min Color: 0; 0; 0 Min Intensity: 1 Name: (Optimized) Transformed Point @@ -500,7 +426,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/template_points + Value: /lidartag/template_points Use Fixed Frame: true Use rainbow: true Value: false @@ -519,7 +445,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 95 + Max Intensity: 216 Min Color: 0; 0; 0 Min Intensity: 0 Name: (Optimized) Binary Transformed Points @@ -534,7 +460,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/template_points_3d + Value: /lidartag/template_points_3d Use Fixed Frame: true Use rainbow: true Value: false @@ -568,7 +494,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/associated_pattern_3d + Value: /lidartag/associated_pattern_3d Use Fixed Frame: true Use rainbow: true Value: false @@ -582,7 +508,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/ideal_frame + Value: /lidartag/ideal_frame Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -594,7 +520,7 @@ Visualization Manager: 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: /lidartag/detail_valid_marker Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -626,7 +552,7 @@ Visualization Manager: 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 + Value: /lidartag/before_transformed_edge_pc Use Fixed Frame: true Use rainbow: true Value: false @@ -640,75 +566,7 @@ Visualization Manager: 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: /lidartag/intesection_markers Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -720,7 +578,7 @@ Visualization Manager: 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: /lidartag/cluster_buff_index_number_markers Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -732,7 +590,7 @@ Visualization Manager: 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: /lidartag/cluster_buff_points_size_markers Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -764,148 +622,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/colored_cluster_buff + Value: /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) @@ -916,7 +636,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/current_projections + Value: /current_projections Value: false - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -926,25 +646,16 @@ Visualization Manager: 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 + 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: /sensor_kit/sensor_kit_base_link/camera0/camera_link/filtered_projections + Value: /filtered_projections Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -976,7 +687,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_1 + Value: /lidartag/edge_group_1 Use Fixed Frame: true Use rainbow: true Value: false @@ -1010,7 +721,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_2 + Value: /lidartag/edge_group_2 Use Fixed Frame: true Use rainbow: true Value: false @@ -1044,7 +755,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_3 + Value: /lidartag/edge_group_3 Use Fixed Frame: true Use rainbow: true Value: false @@ -1078,7 +789,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/edge_group_4 + Value: /lidartag/edge_group_4 Use Fixed Frame: true Use rainbow: true Value: false @@ -1097,9 +808,9 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 2.369355800876173e-38 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 2.369355800876173e-38 Name: Initial Corners Position Transformer: XYZ Selectable: true @@ -1112,7 +823,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /sensor_kit/sensor_kit_base_link/camera0/camera_link/lidartag/initial_corners + Value: /lidartag/initial_corners Use Fixed Frame: true Use rainbow: true Value: false @@ -1170,26 +881,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.385 + Pitch: 0.38499999046325684 Position: - X: 0.0 - Y: 8.0 - Z: 5.0 + X: -0.02556505985558033 + Y: 5.670583724975586 + Z: 4.358150005340576 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 4.71 + Yaw: 4.710000038146973 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 + QMainWindow State: 000000ff00000000fd0000000400000000000002060000035afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000004ee000000a10000000000000000fb0000000a0049006d00610067006500000002f8000000a10000000000000000fb0000000c00430061006d00650072006100000002d1000000c80000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000038300000016000000000000000000000001000001000000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004240000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -1198,6 +905,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1848 - X: 72 + Width: 1846 + X: 1994 Y: 27 diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz similarity index 100% rename from sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz diff --git a/sensor/extrinsic_tag_based_calibrator/src/brute_force_matcher.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp similarity index 99% rename from sensor/extrinsic_tag_based_calibrator/src/brute_force_matcher.cpp rename to sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp index 12574393..172766ed 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/brute_force_matcher.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include diff --git a/sensor/extrinsic_tag_based_calibrator/src/calibration_estimator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp similarity index 98% rename from sensor/extrinsic_tag_based_calibrator/src/calibration_estimator.cpp rename to sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp index 6fc8f51f..20822974 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/calibration_estimator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include #include @@ -32,8 +32,7 @@ #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), @@ -140,7 +139,6 @@ void CalibrationEstimator::update( // 1) Create a new hypothesis for conevnience 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_); @@ -655,11 +653,6 @@ void CalibrationEstimator::getFilteredPose( rot_matrix = hypothesis_rotation_matrix_; } -void CalibrationEstimator::setDynamicsModel(tier4_tag_utils::DynamicsModel dynamics_mode) -{ - dynamics_model_ = dynamics_mode; -} - void CalibrationEstimator::setCrossvalidationTrainingRatio(double ratio) { crossvalidation_training_ratio_ = ratio; diff --git a/sensor/extrinsic_tag_based_calibrator/src/extrinsic_tag_based_calibrator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp similarity index 79% rename from sensor/extrinsic_tag_based_calibrator/src/extrinsic_tag_based_calibrator.cpp rename to sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp index 51483f4e..272cff6e 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/extrinsic_tag_based_calibrator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp @@ -12,13 +12,13 @@ // 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 @@ -30,8 +30,8 @@ #include #endif -ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptions & options) -: Node("extrinsic_tag_based_calibrator_node", options), +ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::NodeOptions & options) +: Node("extrinsic_tag_based_pnp_calibrator_node", options), tf_broadcaster_(this), got_initial_transform(false) { @@ -39,14 +39,11 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio 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"); - 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 = @@ -95,19 +92,19 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio 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 +112,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); @@ -147,14 +140,14 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio const auto period_ns = std::chrono::duration_cast( std::chrono::duration(1.0 / calib_rate_)); - auto tf_timer_callback = std::bind(&ExtrinsicTagBasedCalibrator::tfTimerCallback, this); + auto tf_timer_callback = std::bind(&ExtrinsicTagBasedPNPCalibrator::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); + std::bind(&ExtrinsicTagBasedPNPCalibrator::automaticCalibrationTimerCallback, this); calib_timer_ = std::make_shared>( this->get_clock(), period_ns, std::move(calib_timer_callback), @@ -166,10 +159,10 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // initialize service server - service_server_ = this->create_service( + 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_); @@ -186,7 +179,7 @@ ExtrinsicTagBasedCalibrator::ExtrinsicTagBasedCalibrator(const rclcpp::NodeOptio visualizer_->setApriltagMaxConvergenceThreshold(apriltag_max_convergence_transl); } -void ExtrinsicTagBasedCalibrator::lidarTagDetectionsCallback( +void ExtrinsicTagBasedPNPCalibrator::lidarTagDetectionsCallback( const lidartag_msgs::msg::LidarTagDetectionArray::SharedPtr detections_msg) { latest_timestamp_ = rclcpp::Time(detections_msg->header.stamp); @@ -198,7 +191,7 @@ void ExtrinsicTagBasedCalibrator::lidarTagDetectionsCallback( visualizer_->setLidarFrame(lidar_frame_); } -void ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback( +void ExtrinsicTagBasedPNPCalibrator::aprilTagDetectionsCallback( const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr detections_msg) { latest_timestamp_ = rclcpp::Time(detections_msg->header.stamp); @@ -239,7 +232,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 +241,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; } @@ -261,27 +254,7 @@ void ExtrinsicTagBasedCalibrator::aprilTagDetectionsCallback( estimator_.update(*apriltag_detections_array_); } -/*void ExtrinsicTagBasedCalibrator::cameraImageCallback(const -sensor_msgs::msg::Image::ConstSharedPtr & msg_img, const -sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg_ci) -{ - 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_); - - if (manual_calibrator_ui) - { - cv::Mat cv_img_ = cv_bridge::toCvShare(msg_img)->image.clone(); - cv::cvtColor(cv_img_, cv_img_, CV_BayerRG2RGB); - - manual_calibrator_ui->setImage(cv_img_); - } -}*/ - -void ExtrinsicTagBasedCalibrator::cameraInfoCallback( +void ExtrinsicTagBasedPNPCalibrator::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) { latest_timestamp_ = rclcpp::Time(camera_info_msg->header.stamp); @@ -296,12 +269,13 @@ void ExtrinsicTagBasedCalibrator::cameraInfoCallback( estimator_.setCameraModel(camera_info_); } -void ExtrinsicTagBasedCalibrator::requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response) +void ExtrinsicTagBasedPNPCalibrator::requestReceivedCallback( + 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"); // Wait for subscription topic while (rclcpp::ok()) { @@ -311,49 +285,38 @@ void ExtrinsicTagBasedCalibrator::requestReceivedCallback( if (estimator_.converged() && got_initial_transform && 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_; 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_.getCrossValidationReprojError(); + 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_); @@ -385,18 +348,16 @@ void ExtrinsicTagBasedCalibrator::tfTimerCallback() } 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_; 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_); @@ -459,7 +420,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 +430,14 @@ 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_reproj_error = reprojection_error(image_points, initial_projected_points); + double current_reproj_error = reprojection_error(image_points, current_projected_points); + double filtered_reproj_error = reprojection_error(image_points, filtered_projected_points); + + RCLCPP_INFO(this->get_logger(), "Partial calibration results:"); + RCLCPP_INFO(this->get_logger(), "\tInitial reprojection error=%.2f", initial_reproj_error); + RCLCPP_INFO(this->get_logger(), "\tCurrent reprojection error=%.2f", current_reproj_error); + RCLCPP_INFO(this->get_logger(), "\tFiltered reprojection error=%.2f", filtered_reproj_error); // Publish calibration points publishCalibrationPoints(object_points, image_points); @@ -484,7 +450,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/main.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/main.cpp similarity index 80% rename from sensor/extrinsic_tag_based_calibrator/src/main.cpp rename to sensor/extrinsic_tag_based_pnp_calibrator/src/main.cpp index 8f698cb0..330a6de0 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/main.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/main.cpp @@ -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/extrinsic_tag_based_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp similarity index 99% rename from sensor/extrinsic_tag_based_calibrator/src/tag_calibrator_visualizer.cpp rename to sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index f4a0132b..0acadcc7 100644 --- a/sensor/extrinsic_tag_based_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -424,7 +424,6 @@ void TagCalibratorVisualizer::drawCalibrationStatusText( const double trans_cov = h->getTransCov(); const double rot_cov = h->getRotCov(); - const double rot_dot_cov = h->getRotDotCov(); const double speed = h->getSpeed(); cv::Matx31d center_base(center.x, center.y, center.z); @@ -445,7 +444,6 @@ void TagCalibratorVisualizer::drawCalibrationStatusText( to_string_with_precision(lidartag_convergence_rot_, 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); text_marker.pose.position.x = center_base(0); diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..e5c8e4b9 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml new file mode 100644 index 00000000..b134a0d1 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml new file mode 100644 index 00000000..b134a0d1 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..1849683c --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py new file mode 100644 index 00000000..b989083f --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py @@ -0,0 +1,56 @@ +from typing import List + +from PySide2.QtCore import QAbstractTableModel +from PySide2.QtCore import Qt +from new_extrinsic_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): + # print(f"CalibratorManagerModel: on_changed", flush=True) + 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()] + # print(f"data: index={index} role={role} value={value}c") + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py new file mode 100644 index 00000000..7c4fe170 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py @@ -0,0 +1,155 @@ +#!/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 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 + + +class Calibrator: + def __init__(self): + # Calibration parameters + self.min_points = None + self.inlier_error = None + self.flags = None + self.use_ransac = None + self.ransac_iters = 200 + + # Camera parameters + self.k = None + self.d = None + pass + + def set_min_points(self, min_points): + self.min_points = min_points + + def set_inlier_error(self, inlier_error): + self.inlier_error = inlier_error + + def set_camera_info(self, k, d): + self.k = np.array(k).reshape(3, 3) + self.d = np.array(d).reshape( + -1, + ) + + def set_method(self, method): + if method == "sqpnp": + self.flags = cv2.SOLVEPNP_SQPNP + else: + self.flags = cv2.SOLVEPNP_ITERATIVE + + def set_ransac(self, use_ransac): + self.use_ransac = use_ransac + + def calibrate(self, object_points, image_points): + if len(object_points) == 0 or len(image_points) == 0: + return None + + object_points = np.array(object_points, dtype=np.float64) + image_points = np.array(image_points, dtype=np.float64) + + num_points, dim = object_points.shape + assert dim == 3 + assert num_points == image_points.shape[0] + + if num_points < self.min_points: + return None + + if self.use_ransac: + return self.calibrate_ransac(object_points, image_points) + + tvec = np.zeros((3,)) + rvec = np.zeros((3, 3)) + + try: + retval, rvec, tvec = cv2.solvePnP( + object_points, image_points, self.k, self.d, flags=self.flags + ) + except Exception as e: + print(e) + + camera_to_lidar_transform = cv_to_transformation_matrix(tvec, rvec) + + return camera_to_lidar_transform + + def calibrate_ransac(self, object_points, image_points): + num_points, _ = object_points.shape + + best_tvec = np.zeros((3,)) + best_rvec = np.zeros((3, 3)) + best_inliers = -1 + best_error = np.inf + + for _ in range(self.ransac_iters): + indexes = np.random.choice(num_points, min(num_points, self.min_points)) + object_points_iter = object_points[indexes, :] + image_points_iter = image_points[indexes, :] + + try: + retval, iter_rvec, iter_tvec = cv2.solvePnP( + object_points_iter, image_points_iter, self.k, self.d, flags=self.flags + ) + except Exception as e: + print(e) + continue + + reproj_error_iter, inliers = self.calculate_reproj_error( + object_points, image_points, tvec=iter_tvec, rvec=iter_rvec + ) + + if ( + inliers.sum() == best_inliers and reproj_error_iter < best_error + ) or inliers.sum() > best_inliers: + best_error = reproj_error_iter + best_inliers = inliers.sum() + best_tvec = iter_tvec + best_rvec = iter_rvec + + camera_to_lidar_transform = cv_to_transformation_matrix(best_tvec, best_rvec) + + return camera_to_lidar_transform + + def calculate_reproj_error( + self, object_points, image_points, tvec=None, rvec=None, tf_msg=None, transform_matrix=None + ): + if isinstance(object_points, list) and isinstance(image_points, list): + if len(object_points) == 0: + return 0.0, 0 + + object_points = np.array(object_points, dtype=np.float64) + image_points = np.array(image_points, dtype=np.float64) + + if tf_msg is not None: + transform_matrix = tf_message_to_transform_matrix(tf_msg) + + if transform_matrix is not None: + tvec, rvec = transform_matrix_to_cv(transform_matrix) + + assert tvec is not None + assert rvec is not None + num_points, dim = object_points.shape + projected_points, _ = cv2.projectPoints(object_points, rvec, tvec, self.k, self.d) + projected_points = projected_points.reshape((num_points, 2)) + reproj_error = np.linalg.norm(projected_points - image_points, axis=1) + + if self.use_ransac: + inliers = reproj_error <= self.inlier_error + else: + inliers = np.ones_like(reproj_error) + + return reproj_error.mean(), inliers diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py new file mode 100644 index 00000000..c6c95f87 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py @@ -0,0 +1,205 @@ +# from abc import ABC +# from abc import abstractmethod +from abc import ABCMeta +from abc import abstractproperty + +# from collections import OrderedDict +from collections import defaultdict +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 new_extrinsic_calibration_manager.calibrator_wrapper import CalibratorServiceWrapper +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import CalibratorState +from new_extrinsic_calibration_manager.types import FramePair +from new_extrinsic_calibration_manager.utils import tf_message_to_transform_matrix +from new_extrinsic_calibration_manager.utils import transform_matrix_to_tf_message +import numpy as np +import transforms3d +import yaml + + +class CalibratorBase(QObject): + __metaclass__ = ABCMeta + + state_changed_signal = Signal(CalibratorState) + calibration_finished_signal = Signal() + + def __init__(self, ros_interface: RosInterface): + print("CalibratorBase: constructor start", flush=True) + 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) + print("CalibratorBase: constructor end", flush=True) + + def init(): + print("CalibratorBase: Calibrator init?", flush=True) + pass + + def on_check_tf_timer(self): + print("CalibratorBase: on_check_tf_timer", flush=True) + 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() + print("CalibratorBase: on_check_tf_timer stop", flush=True) + else: + for frame in self.required_frames[1:]: + if not self.ros_interface.can_transform(self.required_frames[0], frame): + print(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]): + print("CalibratorBase: add_calibrator", flush=True) + + 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): + print("CalibratorBase: on_calibration_result", flush=True) + + 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): + 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() + } + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py new file mode 100644 index 00000000..76cf612f --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py @@ -0,0 +1,67 @@ +from collections import defaultdict +import logging +from typing import Callable +from typing import List + +from new_extrinsic_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: + The Executor class itself. TODO: write correct + """ + + 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 excecutor 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py new file mode 100644 index 00000000..87595094 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py @@ -0,0 +1,185 @@ +from collections import defaultdict +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 new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +from tier4_calibration_msgs.msg import CalibrationResult +from tier4_calibration_msgs.srv import NewExtrinsicCalibrator + +# import debugpy +# debugpy.listen(5678) +# debugpy.wait_for_client() + + +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__() + + # TODO: Consider making timer to display the elapsed time + 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: + print( + f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers", + flush=True, + ) + continue + + i = self.children_to_id[child_frame] + + if self.parents[i] != parent_frame: + print( + f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers", + flush=True, + ) + 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): + # print(f"on_status: self.service_status={self.service_status} new_status={status}", flush=True) + # print(f"{threading.get_ident() }: on_status") + + if self.service_status != status: + self.service_status = status + # print(f"emitting data changed", flush=True) + 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: NewExtrinsicCalibrator.Response): + print(f"{threading.get_ident() }: result_ros_callback", flush=True) + self.result_signal.emit(result.results) + + def status_ros_callback(self, status: bool): + self.status_signal.emit(status) + pass + + def get_data(self, index) -> list: + if not self.service_called: + status_message = "Service ready" if self.service_status else "Service not available" + # print(f"getData: status_message={status_message}") + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py new file mode 100644 index 00000000..963d1070 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py @@ -0,0 +1,8 @@ +# __all__ = ['basemapper', 'lxml'] +# from basemaper import * +# import lxml + +from .default_project import * # noqa: F401, F403 +from .dummy_project import * # noqa: F401, F403 +from .tier4_dummy_project import * # noqa: F401, F403 +from .xx1_15 import * # noqa: F401, F403 diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py new file mode 100644 index 00000000..5942b590 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py @@ -0,0 +1,3 @@ +from .tag_based_pnp_calibrator import TagBasedPNPCalibrator + +__all__ = ["TagBasedPNPCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..5d138803 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py @@ -0,0 +1,33 @@ +# from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_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) + + print("DefaultProject_TagBasedPNPCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py new file mode 100644 index 00000000..c790a4e7 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py @@ -0,0 +1,11 @@ +from .dummy_base_lidar_calibrator import DummyBaseLidarCalibrator +from .dummy_camera_camera_calibrator import DummyCameraCameraCalibrator +from .dummy_camera_lidar_calibrator import DummyCameraLidarCalibrator +from .dummy_lidar_lidar_calibrator import DummyLidarLidarCalibrator + +__all__ = [ + "DummyBaseLidarCalibrator", + "DummyCameraCameraCalibrator", + "DummyCameraLidarCalibrator", + "DummyLidarLidarCalibrator", +] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py new file mode 100644 index 00000000..9591f87c --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py @@ -0,0 +1,21 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair + + +@CalibratorRegistry.register_calibrator( + project_name="dummy_project", calibrator_name="base_lidar_calibration" +) +class DummyBaseLidarCalibrator(CalibratorBase): + required_frames = ["base_link, sensor_kit_base_link", "velodyne_top_base_link", "velodyne_top"] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("DummyBaseLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[FramePair(parent="base_link", child="velodyne_top")], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py new file mode 100644 index 00000000..c7d89451 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py @@ -0,0 +1,29 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair + + +@CalibratorRegistry.register_calibrator( + project_name="dummy_project", calibrator_name="camera_camera_calibration" +) +class DummyCameraCameraCalibrator(CalibratorBase): + required_frames = [ + "sensor_kit_base_link", + "camera0/camera_link", + "camera0/camera_optical_link", + "camera1/camera_optical_link", + "camera1/camera_link", + ] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("DummyCameraCameraCalibrator") + + self.add_calibrator( + service_name="calibrate_camera_camera", + expected_calibration_frames=[ + FramePair(parent="camera0/camera_link", child="camera1/camera_link"), + ], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py new file mode 100644 index 00000000..631e88d9 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py @@ -0,0 +1,29 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair + + +@CalibratorRegistry.register_calibrator( + project_name="dummy_project", calibrator_name="camera_lidar_calibration" +) +class DummyCameraLidarCalibrator(CalibratorBase): + required_frames = [ + "sensor_kit_base_link", + "camera0/camera_link", + "camera0/camera_optical_link", + "velodyne_top_base_link", + "velodyne_top", + ] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("DummyCameraLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_camera_lidar", + expected_calibration_frames=[ + FramePair(parent="velodyne_top", child="camera0/camera_link"), + ], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py new file mode 100644 index 00000000..f2ad035d --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py @@ -0,0 +1,32 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair + + +@CalibratorRegistry.register_calibrator( + project_name="dummy_project", calibrator_name="lidar_lidar_calibration" +) +class DummyLidarLidarCalibrator(CalibratorBase): + required_frames = [ + "sensor_kit_base_link", + "velodyne_top_base_link", + "velodyne_top", + "velodyne_left_base_link", + "velodyne_left", + "velodyne_right_base_link", + "velodyne_right", + ] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("DummyLidarLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_camera_lidar", + expected_calibration_frames=[ + FramePair(parent="velodyne_top", child="velodyne_left"), + FramePair(parent="velodyne_top", child="velodyne_right"), + ], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py new file mode 100644 index 00000000..8fb1a0e9 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py @@ -0,0 +1,3 @@ +from .tier4_dummy_base_lidar_calibrator import DummyBaseLidarCalibrator + +__all__ = ["DummyBaseLidarCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py new file mode 100644 index 00000000..8bc57672 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py @@ -0,0 +1,39 @@ +from collections import defaultdict +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@CalibratorRegistry.register_calibrator( + project_name="tier4_dummy_project", calibrator_name="tier4_base_lidar_calibration" +) +class DummyBaseLidarCalibrator(CalibratorBase): + required_frames = ["base_link", "sensor_kit_base_link", "velodyne_top"] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("Tier4DummyBaseLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[FramePair(parent="base_link", child="velodyne_top")], + ) + + def post_process(self, calibration_transforms) -> Dict[str, Dict[str, np.array]]: + sensor_kit_to_lidar = self.get_transform_matrix( + parent="sensor_kit_base_link", child="velodyne_top" + ) + + base_link_to_sensor_kit = calibration_transforms["base_link"][ + "velodyne_top" + ] @ np.linalg.inv(sensor_kit_to_lidar) + + output_transforms = defaultdict(lambda: defaultdict(np.array)) + output_transforms["base_link"]["sensor_kit_base_link"] = base_link_to_sensor_kit + + return output_transforms diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py new file mode 100644 index 00000000..53212ebe --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py @@ -0,0 +1,11 @@ +from .dummy_base_lidar_calibrator import DummyBaseLidarCalibrator +from .dummy_camera_camera_calibrator import DummyCameraCameraCalibrator +from .dummy_lidar_lidar_calibrator import DummyLidarLidarCalibrator +from .tag_based_pnp_calibrator import TagBasedPNPCalibrator + +__all__ = [ + "DummyBaseLidarCalibrator", + "DummyCameraCameraCalibrator", + "TagBasedPNPCalibrator", + "DummyLidarLidarCalibrator", +] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py new file mode 100644 index 00000000..9591f87c --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py @@ -0,0 +1,21 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair + + +@CalibratorRegistry.register_calibrator( + project_name="dummy_project", calibrator_name="base_lidar_calibration" +) +class DummyBaseLidarCalibrator(CalibratorBase): + required_frames = ["base_link, sensor_kit_base_link", "velodyne_top_base_link", "velodyne_top"] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("DummyBaseLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[FramePair(parent="base_link", child="velodyne_top")], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py new file mode 100644 index 00000000..c7d89451 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py @@ -0,0 +1,29 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair + + +@CalibratorRegistry.register_calibrator( + project_name="dummy_project", calibrator_name="camera_camera_calibration" +) +class DummyCameraCameraCalibrator(CalibratorBase): + required_frames = [ + "sensor_kit_base_link", + "camera0/camera_link", + "camera0/camera_optical_link", + "camera1/camera_optical_link", + "camera1/camera_link", + ] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("DummyCameraCameraCalibrator") + + self.add_calibrator( + service_name="calibrate_camera_camera", + expected_calibration_frames=[ + FramePair(parent="camera0/camera_link", child="camera1/camera_link"), + ], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py new file mode 100644 index 00000000..f2ad035d --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py @@ -0,0 +1,32 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair + + +@CalibratorRegistry.register_calibrator( + project_name="dummy_project", calibrator_name="lidar_lidar_calibration" +) +class DummyLidarLidarCalibrator(CalibratorBase): + required_frames = [ + "sensor_kit_base_link", + "velodyne_top_base_link", + "velodyne_top", + "velodyne_left_base_link", + "velodyne_left", + "velodyne_right_base_link", + "velodyne_right", + ] + + def __init__(self, ros_interface: RosInterface): + super().__init__(ros_interface) + + print("DummyLidarLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_camera_lidar", + expected_calibration_frames=[ + FramePair(parent="velodyne_top", child="velodyne_left"), + FramePair(parent="velodyne_top", child="velodyne_right"), + ], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..24b43449 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py @@ -0,0 +1,54 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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") + + print("TagBasedPNPCalibrator") + print(self.camera_name, flush=True) + + 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]]): + camera_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 + @ camera_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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py new file mode 100644 index 00000000..166e01f1 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py @@ -0,0 +1,300 @@ +#!/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 defaultdict +import copy +from functools import partial +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 new_extrinsic_calibration_manager.calibration_manager_model import CalibratorManagerModel +from new_extrinsic_calibration_manager.calibrator_base import CalibratorState +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +import new_extrinsic_calibration_manager.calibrators # noqa: F401 Force imports +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.views.calibrator_selector_view import CalibrationSelectorView +from new_extrinsic_calibration_manager.views.launcher_configuration_view import ( + LauncherConfigurationView, +) +from new_extrinsic_calibration_manager.views.tf_view import TfView +import rclpy + +# import debugpy +# debugpy.listen(5678) +# debugpy.wait_for_client() + + +class NewExtrinsicCalibrationManager(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 manaer") + + self.ros_interface: RosInterface = 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): + print( + f"on_selected_calibrator: project_name={project_name} calibrator_name={calibrator_name}", + flush=True, + ) + self.laucher_configuration_view = LauncherConfigurationView(project_name, calibrator_name) + self.laucher_configuration_view.launcher_parameters.connect( + partial(self.launch_calibrators, project_name, calibrator_name) + ) + pass + + def launch_calibrators(self, project_name: str, calibrator_name: str, argument_dict: Dict): + # Show the main UI + self.show() + + # Execute the launcher + print(argument_dict, flush=True) + argument_list = [f"{k}:={v}" for k, v in argument_dict.items()] + + package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") + path = ( + package_share_directory + + "/launch/" + + project_name + + "/" + + calibrator_name + + ".launch.xml" + ) + self.process = subprocess.Popen(["ros2", "launch", path] + argument_list) + + # 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, **argument_dict + ) + 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() + pass + + 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): + print("on_calibration_request", flush=True) + 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): + print("on_save_request", flush=True) + + output_path = QFileDialog.getSaveFileName( + None, "Save File", f"{os.getcwd()}/calibration_results.yaml", "YAML files (*.yaml)" + ) + + print(output_path, flush=True) + output_path = output_path[0] + + if output_path is None or output_path == "": + print("Invalid path", flush=True) + return + + print(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 + + self.tfs_dict = tfs_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 + ) + # self.calibration_tfs_view.setTfs(self.tfs_dict) + # self.final_tfs_view.setTfs(self.tfs_dict) + + # self.tf_view.plot.setTfs(self.tfs_msg.transforms) + # print("SECOND", flush=True) + + +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 = NewExtrinsicCalibrationManager() # noqa: F841 + + sys.exit(app.exec_()) + except (KeyboardInterrupt, SystemExit): + print("Received sigint. Quiting...") + rclpy.shutdown() + + +def sigint_handler(*args): + QApplication.quit() + + +if __name__ == "__main__": + main() diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py new file mode 100644 index 00000000..355c733f --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 + +# 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. + +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 NewExtrinsicCalibrator + + +class RosInterface(Node): + def __init__(self): + super().__init__("new_extrinsic_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.buf = Buffer(node=self) + self.listener = TransformListener(self.buf, self, spin_thread=False) + + 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): + # print("TF MSG", flush=True) + + 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( + NewExtrinsicCalibrator, 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 = NewExtrinsicCalibrator.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 + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py new file mode 100644 index 00000000..16de8e97 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py @@ -0,0 +1,15 @@ +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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py new file mode 100644 index 00000000..c232f0db --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py @@ -0,0 +1,56 @@ +#!/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 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 + rot_matrix = transforms3d.quaternions.quat2mat((q.w, q.x, q.y, q.z)) + + transform_matrix[0:3, 0:3] = rot_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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py new file mode 100644 index 00000000..5901b64b --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 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 new_extrinsic_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() + print("Closing calibrator selector view", flush=True) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py new file mode 100644 index 00000000..7b1492cc --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 xml.dom.minidom + +from PySide2.QtCore import Signal +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 + +# import debugpy +# debugpy.listen(5678) +# debugpy.wait_for_client() + + +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("new_extrinsic_calibration_manager") + path = ( + package_share_directory + + "/launch/" + + project_name + + "/" + + calibrator_name + + ".launch.xml" + ) + xml_doc = xml.dom.minidom.parse(path) + + print(f"Reading xml from: {path}") + + arg_nodes = [ + node + for node in xml_doc.getElementsByTagName("arg") + if node.parentNode == xml_doc.firstChild + ] + + for element in arg_nodes: + description = ( + element.getAttribute("description") if element.hasAttribute("description") else " " + ) + if element.hasAttribute("default"): + self.optional_arguments_dict[element.getAttribute("name")] = { + "value": element.getAttribute("default"), + "decription": description, + } + else: + self.required_arguments_dict[element.getAttribute("name")] = { + "value": "", + "decription": description, + } + + 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) + + self.arguments_widgets_dict[argument_name] = QLineEdit(argument_data["value"]) + self.arguments_widgets_dict[argument_name].textChanged.connect( + self.check_argument_status + ) + self.arguments_widgets_dict[argument_name].setMinimumWidth(400) + self.arguments_widgets_dict[argument_name].setMaximumWidth(800) + + description_label = QLabel(argument_data["decription"]) + description_label.setMaximumWidth(400) + description_label.setToolTip(argument_data["decription"]) + description_label.setText(argument_data["decription"]) + + 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) + + self.arguments_widgets_dict[argument_name] = QLineEdit(argument_data["value"]) + self.arguments_widgets_dict[argument_name].textChanged.connect( + self.check_argument_status + ) + self.arguments_widgets_dict[argument_name].setMinimumWidth(400) + self.arguments_widgets_dict[argument_name].setMaximumWidth(800) + + description_label = QLabel(argument_data["decription"]) + description_label.setMaximumWidth(400) + description_label.setToolTip(argument_data["decription"]) + description_label.setText(argument_data["decription"]) + + 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 for widget in self.arguments_widgets_dict.values()], + ) + ) + print("check_argument_status", flush=True) + + def on_click(self): + args_dict = { + arg_name: args_widget.text() + for arg_name, args_widget in self.arguments_widgets_dict.items() + } + + print(args_dict, flush=True) + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py new file mode 100644 index 00000000..29393cdd --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py @@ -0,0 +1,367 @@ +#!/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 defaultdict +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 + +# import debugpy +# debugpy.listen(5678) +# debugpy.wait_for_client() + + +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] + + return ( + (sliced_node, sliced_children_frames) + if len(sliced_children) > 0 + else (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(graph_string) + graphs = pydot.graph_from_dot_data(graph_string) + graph = graphs[0] + + # graph.write_png("frames.png") + # graph.write_pdf("frames.pdf") + + imgdata = graph.create_svg() + + # imgdata = StringIO() + # figure.savefig(imgdata, format='svg') + # imgdata.seek(0) + xmlreader = QXmlStreamReader(imgdata) + self.renderer = QSvgRenderer(xmlreader) + self.renderer.setAspectRatioMode(Qt.AspectRatioMode.KeepAspectRatio) + # print(f"bits post savefig= {len(imgdata.getvalue())}", flush=True) + + def paintEvent(self, event): + if self.renderer is None: + return + + p = QPainter() + p.begin(self) + self.renderer.render(p) + p.end() + # print(self.geometry()) + # print(f"PAINTED", flush=True) + + +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) + # print(f"=======Tf plot size: {self.plot.size()}", flush=True) + + def resizeEvent(self, event): + print( + f"PRE resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}", + flush=True, + ) + super().resizeEvent(event) + print( + f"POST resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}", + flush=True, + ) + + # scaled_pix_size = self.pix.size() + # scaled_pix_size.scale(self.data_renderer.widget_size, Qt.KeepAspectRatio) + + # import debugpy + # debugpy.listen(5678) + # debugpy.wait_for_client() + + 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/new_extrinsic_calibration_manager/package.xml b/sensor/new_extrinsic_calibration_manager/package.xml new file mode 100644 index 00000000..cec81f47 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/package.xml @@ -0,0 +1,22 @@ + + + + new_extrinsic_calibration_manager + 0.0.0 + TODO: Package description + Kenzo Lobos Tsunekawa + TODO: License declaration + + python3-pyside2.qtquick + python3-transforms3d + ros2_numpy + ros2launch + tier4_calibration_msgs + ament_copyright + ament_flake8 + python3-pytest + + + ament_python + + diff --git a/sensor/new_extrinsic_calibration_manager/resource/new_extrinsic_calibration_manager b/sensor/new_extrinsic_calibration_manager/resource/new_extrinsic_calibration_manager new file mode 100644 index 00000000..e69de29b diff --git a/sensor/new_extrinsic_calibration_manager/setup.cfg b/sensor/new_extrinsic_calibration_manager/setup.cfg new file mode 100644 index 00000000..e4a17e51 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/new_extrinsic_calibration_manager +[install] +install_scripts=$base/lib/new_extrinsic_calibration_manager diff --git a/sensor/new_extrinsic_calibration_manager/setup.py b/sensor/new_extrinsic_calibration_manager/setup.py new file mode 100644 index 00000000..a54e0f47 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/setup.py @@ -0,0 +1,51 @@ +import os + +from setuptools import setup + +package_name = "new_extrinsic_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="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "new_extrinsic_calibration_manager = new_extrinsic_calibration_manager.new_extrinsic_calibration_manager:main" + ], + }, +) diff --git a/sensor/new_extrinsic_calibration_manager/test/test_pep257.py b/sensor/new_extrinsic_calibration_manager/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/sensor/new_extrinsic_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" From 3b9734e1ba701c992587c8b8d38e22df77c2d95f Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 5 Oct 2023 20:06:00 +0900 Subject: [PATCH 02/49] Did some heavy refactoring in the tag-based camera-lidar calibration Added an option to use rectified images Deleted the raw sqpnp method since it is included in the standard opencv now Enabled the use of hardcoded system time (receiving time) for when sensors are now synchronized Parameterized rviz profiles to avoid having multiple profiles (for this method one should be enough for all use cases) Exposed the number of pnp pairs for calibration and the distance between calibration data and new detections to ease the work of field engineers Changed timers since the default work now (for years now though) Still have not done the spell checking Signed-off-by: Kenzo Lobos-Tsunekawa --- common/tier4_tag_utils/CMakeLists.txt | 1 - .../include/tier4_tag_utils/cv/LICENSE.txt | 202 --- .../include/tier4_tag_utils/cv/_precomp.hpp | 139 -- .../include/tier4_tag_utils/cv/sqpnp.hpp | 195 --- .../src/apriltag_hypothesis.cpp | 21 +- common/tier4_tag_utils/src/cv/LICENSE.txt | 202 --- common/tier4_tag_utils/src/cv/sqpnp.cpp | 793 ----------- .../package.xml | 2 - .../src/apriltag_detection.cpp | 26 +- .../CMakeLists.txt | 2 - .../extrinsic_tag_based_pnp_calibrator.hpp | 2 + .../launch/calibrator.launch.xml | 13 +- .../package.xml | 2 - ...velodyne_top.rviz => default_profile.rviz} | 4 +- .../tag_calib_camera0_pandar_40p_right.rviz | 1203 ---------------- .../tag_calib_camera1_pandar_40p_right.rviz | 1222 ----------------- .../rviz/tag_calib_camera1_velodyne_top.rviz | 1203 ---------------- .../tag_calib_camera2_pandar_40p_right.rviz | 1222 ----------------- .../rviz/tag_calib_camera2_velodyne_top.rviz | 1203 ---------------- .../tag_calib_camera3_pandar_40p_rear.rviz | 1222 ----------------- .../rviz/tag_calib_camera3_velodyne_top.rviz | 1203 ---------------- .../tag_calib_camera4_pandar_40p_left.rviz | 1222 ----------------- .../rviz/tag_calib_camera4_velodyne_top.rviz | 1203 ---------------- .../tag_calib_camera5_pandar_40p_left.rviz | 1222 ----------------- .../rviz/tag_calib_camera5_velodyne_top.rviz | 1203 ---------------- .../tag_calib_camera6_pandar_40p_front.rviz | 1197 ---------------- ...raffic_light_left_camera_velodyne_top.rviz | 1203 ---------------- .../src/calibration_estimator.cpp | 16 +- .../extrinsic_tag_based_pnp_calibrator.cpp | 73 +- .../src/tag_calibrator_visualizer.cpp | 23 +- .../tag_based_pnp_calibrator.launch.xml | 11 +- .../tag_based_pnp_calibrator.launch.xml | 55 +- .../views/launcher_configuration_view.py | 7 +- 33 files changed, 132 insertions(+), 17385 deletions(-) delete mode 100644 common/tier4_tag_utils/include/tier4_tag_utils/cv/LICENSE.txt delete mode 100644 common/tier4_tag_utils/include/tier4_tag_utils/cv/_precomp.hpp delete mode 100644 common/tier4_tag_utils/include/tier4_tag_utils/cv/sqpnp.hpp delete mode 100644 common/tier4_tag_utils/src/cv/LICENSE.txt delete mode 100644 common/tier4_tag_utils/src/cv/sqpnp.cpp rename sensor/extrinsic_tag_based_pnp_calibrator/rviz/{tag_calib_camera0_velodyne_top.rviz => default_profile.rviz} (99%) delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz delete mode 100644 sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz 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/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/src/apriltag_hypothesis.cpp b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp index 49416f0e..14d7c8aa 100644 --- a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include namespace tier4_tag_utils { @@ -118,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 = { @@ -127,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("teir4_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; 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/sensor/extrinsic_tag_based_base_calibrator/package.xml b/sensor/extrinsic_tag_based_base_calibrator/package.xml index 645dcd89..6a2a0393 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/package.xml +++ b/sensor/extrinsic_tag_based_base_calibrator/package.xml @@ -15,7 +15,6 @@ apriltag apriltag_msgs - backward_ros cv_bridge eigen geometry_msgs @@ -43,7 +42,6 @@ rclpy - ament_cmake diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp b/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp index 098ed480..d3503598 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp +++ b/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp @@ -26,8 +26,6 @@ #include #endif -#include - #include namespace extrinsic_tag_based_base_calibrator @@ -126,24 +124,16 @@ 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); - - if (tvec_vec.size() == 0) { - assert(false); - return std::numeric_limits::infinity(); - } + cv::Mat rvec, tvec; - assert(rvec_vec.size() == 1); - cv::Mat rvec = rvec_vec[0]; - cv::Mat tvec = tvec_vec[0]; + bool success = cv::solvePnP( + template_corners, image_corners, intrinsics.camera_matrix, intrinsics.dist_coeffs, rvec, tvec, + false, cv::SOLVEPNP_SQPNP); - // cv::Matx31d translation_vector = tvec; - // cv::Matx33d rotation_matrix; - - // translation_vector = tvec; - // cv::Rodrigues(rvec, rotation_matrix); + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("teir4_tag_utils"), "PNP failed"); + return false; + } pose = cv::Affine3d(rvec, tvec); computeObjectCorners(); diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt b/sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt index 3f81105c..45aacddf 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt +++ b/sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt @@ -12,8 +12,6 @@ ament_export_include_directories( ${OpenCV_INCLUDE_DIRS} ) - # COMPILE THE SOURCE -#======================================================================== ament_auto_add_executable(extrinsic_tag_based_pnp_calibrator src/brute_force_matcher.cpp src/calibration_estimator.cpp diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp index 80d79e99..835fccc7 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp @@ -89,6 +89,8 @@ class ExtrinsicTagBasedPNPCalibrator : public rclcpp::Node // Parameters std::string base_frame_; float calib_rate_; + bool use_receive_time_; + bool use_rectified_image_; // Filter parameters double min_tag_size_; diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml index 784ebe2b..46ccb6bd 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -4,18 +4,24 @@ + + + + + + @@ -25,16 +31,17 @@ + - + - - + + diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/package.xml b/sensor/extrinsic_tag_based_pnp_calibrator/package.xml index cd5f2549..39d56139 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/package.xml +++ b/sensor/extrinsic_tag_based_pnp_calibrator/package.xml @@ -13,7 +13,6 @@ autoware_cmake apriltag_msgs - backward_ros cv_bridge geometry_msgs image_geometry @@ -37,7 +36,6 @@ apriltag_ros lidartag - ament_cmake diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz similarity index 99% rename from sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz rename to sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz index f5b22c71..15634626 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_velodyne_top.rviz +++ b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz @@ -105,7 +105,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw + Value: /pointcloud_topic_placeholder Use Fixed Frame: true Use rainbow: true Value: true @@ -830,7 +830,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: velodyne_top + Fixed Frame: lidar_frame Frame Rate: 30 Name: root Tools: diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera0_pandar_40p_right.rviz deleted file mode 100644 index 6bab776f..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_pandar_40p_right.rviz deleted file mode 100644 index 192d09da..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera1_velodyne_top.rviz deleted file mode 100644 index 72f7f84a..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_pandar_40p_right.rviz deleted file mode 100644 index d02bfc84..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera2_velodyne_top.rviz deleted file mode 100644 index c0df02dd..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_pandar_40p_rear.rviz deleted file mode 100644 index a910a521..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera3_velodyne_top.rviz deleted file mode 100644 index 69433bbb..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_pandar_40p_left.rviz deleted file mode 100644 index 1714ac3e..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera4_velodyne_top.rviz deleted file mode 100644 index bfa1af16..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_pandar_40p_left.rviz deleted file mode 100644 index c9ac66c6..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera5_velodyne_top.rviz deleted file mode 100644 index 1b124fdb..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_camera6_pandar_40p_front.rviz deleted file mode 100644 index a7326ed1..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/tag_calib_traffic_light_left_camera_velodyne_top.rviz deleted file mode 100644 index 0c9824d9..00000000 --- a/sensor/extrinsic_tag_based_pnp_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_tag_based_pnp_calibrator/src/calibration_estimator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp index 20822974..f1e71e54 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include @@ -478,20 +477,17 @@ bool CalibrationEstimator::calibrate( 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) { + if (!success) { + RCLCPP_ERROR(rclcpp::get_logger("teir4_tag_utils"), "PNP failed"); return false; } - cv::Mat rvec = rvec_vec[0]; - cv::Mat tvec = tvec_vec[0]; - translation_vector = tvec; cv::Rodrigues(rvec, rotation_matrix); diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp index 272cff6e..54218397 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include @@ -43,6 +42,8 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod 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"); double calibration_crossvalidation_training_ratio = this->declare_parameter("calibration_crossvalidation_training_ratio"); @@ -137,24 +138,13 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod estimator_.setApriltagMeasurementNoise(apriltag_measurement_noise_transl); estimator_.setApriltagProcessNoise(apriltag_process_noise_transl); - 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(&ExtrinsicTagBasedPNPCalibrator::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(&ExtrinsicTagBasedPNPCalibrator::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); @@ -180,29 +170,32 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod } void ExtrinsicTagBasedPNPCalibrator::lidarTagDetectionsCallback( - const lidartag_msgs::msg::LidarTagDetectionArray::SharedPtr detections_msg) + 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 ExtrinsicTagBasedPNPCalibrator::aprilTagDetectionsCallback( - const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr detections_msg) + 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; @@ -222,7 +215,6 @@ void ExtrinsicTagBasedPNPCalibrator::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); @@ -251,19 +243,36 @@ void ExtrinsicTagBasedPNPCalibrator::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 ExtrinsicTagBasedPNPCalibrator::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; - visualizer_->setCameraFrame(optical_frame_); + if (use_receive_time_) { + camera_info_.header.stamp = this->now(); + } + 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); + } + + 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_); diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index 0acadcc7..56ab05f6 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include #include @@ -837,17 +836,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; } @@ -860,19 +855,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("teir4_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; diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml index e5c8e4b9..b9c7cf3d 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml @@ -8,7 +8,7 @@ - + @@ -39,5 +39,12 @@ - + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml index 1849683c..21ddcca9 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml @@ -1,6 +1,9 @@ + + + @@ -20,30 +23,31 @@ - - - - + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + @@ -56,6 +60,13 @@ - + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 7b1492cc..9b5c9d71 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -70,10 +70,15 @@ def __init__(self, project_name, calibrator_name): + calibrator_name + ".launch.xml" ) - xml_doc = xml.dom.minidom.parse(path) print(f"Reading xml from: {path}") + try: + xml_doc = xml.dom.minidom.parse(path) + except Exception as e: + print("Failed reading xml file. Either not-existent or invalid") + raise e + arg_nodes = [ node for node in xml_doc.getElementsByTagName("arg") From 563ea1984bc219be64285a15dcb845bdd98f2918 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 5 Oct 2023 20:49:20 +0900 Subject: [PATCH 03/49] Fixed spellings in the tag based camera lidar calibrator Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 25 +++++++++++- .../apriltag_detection.hpp | 4 +- .../ceres/camera_residual.hpp | 40 +++++++++---------- .../ceres/lidar_residual.hpp | 22 +++++----- .../types.hpp | 2 +- .../launch/calibrator.launch.xml | 2 +- .../src/apriltag_detection.cpp | 10 ++--- .../src/apriltag_detector.cpp | 10 ++--- .../extrinsic_tag_based_base_calibrator.cpp | 4 +- .../intrinsics_calibrator.cpp | 5 ++- .../calibration_estimator.hpp | 10 ++--- .../tag_calibrator_visualizer.hpp | 2 +- .../rviz/default_profile.rviz | 6 +-- .../src/brute_force_matcher.cpp | 4 +- .../src/calibration_estimator.cpp | 30 +++++++------- .../extrinsic_tag_based_pnp_calibrator.cpp | 18 +++++---- .../src/tag_calibrator_visualizer.cpp | 6 +-- 17 files changed, 113 insertions(+), 87 deletions(-) diff --git a/.cspell.json b/.cspell.json index 385ed9e3..b62e6b13 100644 --- a/.cspell.json +++ b/.cspell.json @@ -1,6 +1,5 @@ { "words": [ - "apriltags", "Rodrigues", "subsampled", "undistortion", @@ -9,6 +8,28 @@ "rvec", "tvec", "rvecs", - "tvecs" + "tvecs", + "rclcpp", + "ransac", + "lidartag", + "apriltag", + "lidartags", + "apriltags", + "pnp", + "sqpnp", + "solvepnp", + "eigen", + "homography", + "reprojection", + "permutate", + "distro", + "matx", + "idless", + "crossvalidation", + "prerejective", + "3dpoints", + "calib", + "coeffs", + "rviz" ] } diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp index ad9680ba..9229e637 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp +++ b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp @@ -51,8 +51,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; diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp index b667bd60..25271db4 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp +++ b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp @@ -195,28 +195,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; diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp index 1d229a09..67cacf74 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp +++ b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp @@ -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; diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp index 5ddf6198..3463a16c 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp +++ b/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp @@ -33,7 +33,7 @@ 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; diff --git a/sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml index f2f222f8..9d7b4332 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml @@ -85,7 +85,7 @@ - + diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp b/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp index d3503598..9438bab7 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp +++ b/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp @@ -131,24 +131,24 @@ double ApriltagDetection::computePose(const IntrinsicParameters & intrinsics) false, cv::SOLVEPNP_SQPNP); if (!success) { - RCLCPP_ERROR(rclcpp::get_logger("teir4_tag_utils"), "PNP failed"); + 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()); diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp b/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp index 637bf25c..4c63dd48 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp +++ b/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp @@ -234,16 +234,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, + tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reprojection_error, rotation_angle); continue; } @@ -255,7 +255,7 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c 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, + tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reprojection_error, rotation_angle); continue; } @@ -263,7 +263,7 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c 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, + tag_family_and_id.c_str(), det->decision_margin, max_homography_error, reprojection_error, rotation_angle); individual_detections_map[tag_type].emplace_back(result); diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp b/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp index 85a69e8c..19d346f9 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp +++ b/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp @@ -285,8 +285,8 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( this->declare_parameter("apriltag_min_margin", 20.0); 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); + apriltag_detector_parameters_.max_reprojection_error = + this->declare_parameter("apriltag_max_reprojection_error", 10.0); apriltag_detector_parameters_.max_homography_error = this->declare_parameter("apriltag_max_homography_error", 0.5); apriltag_detector_parameters_.quad_decimate = diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp b/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp index f261c1c1..07f43f44 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp +++ b/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp @@ -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); diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp index bdb9323b..68f1d70a 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp @@ -45,7 +45,7 @@ class CalibrationEstimator bool update(const rclcpp::Time & timestamp); void getCalibrationPoints( - std::vector & object_points, std::vector & image_pointsbool, + std::vector & object_points, std::vector & image_points, bool use_estimated); bool calibrate(); @@ -79,7 +79,7 @@ class CalibrationEstimator void setTagSizes(std::vector & tag_id, std::vector & tag_sizes); void setLidartagMaxConvergenceThreshold( - double transl, double tansl_dot, double angle, double angle_dot); + double transl, double transl_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); @@ -92,7 +92,7 @@ class CalibrationEstimator double getNewHypothesisDistance() const; double getCalibrationCoveragePercentage() const; int getCurrentCalibrationPairsNumber() const; - double getCrossValidationReprojError() const; + double getCrossValidationReprojectionError() const; private: void getCalibrationPointsIdBased( @@ -110,7 +110,7 @@ class CalibrationEstimator 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 @@ -161,7 +161,7 @@ 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_; diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp index 6137ea30..c4d06ecb 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp @@ -73,7 +73,7 @@ class TagCalibratorVisualizer void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); void setLidartagMaxConvergenceThreshold( - double transl, double tansl_dot, double angle, double angle_dot); + double transl, double transl_dot, double angle, double angle_dot); void setApriltagMaxConvergenceThreshold(double transl); private: diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz index 15634626..fcaa0b8f 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz +++ b/sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz @@ -8,7 +8,7 @@ Panels: - /(Optimized) Binary Transformed Points1/Topic1 - "/Cluster info: detail code1/Topic1" - "/Cluster info: detail code1/Namespaces1" - - /Tag calib markers (filtered)1/Namespaces1 + - /Tag calibration markers (filtered)1/Namespaces1 Splitter Ratio: 0.6812933087348938 Tree Height: 797 - Class: rviz_common/Selection @@ -628,7 +628,7 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: Tag calib markers (unfiltered) + Name: Tag calibration markers (unfiltered) Namespaces: {} Topic: @@ -640,7 +640,7 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: Tag calib markers (filtered) + Name: Tag calibration markers (filtered) Namespaces: active_center: true active_lidartag_frame: true diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp index 172766ed..c62cbe67 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp @@ -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_pnp_calibrator/src/calibration_estimator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp index f1e71e54..ce24ac59 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -50,7 +50,7 @@ CalibrationEstimator::CalibrationEstimator() 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()), + crossvalidation_reprojection_error_(std::numeric_limits::infinity()), valid_(false) { } @@ -82,7 +82,7 @@ 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_); @@ -116,7 +116,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; } @@ -135,7 +135,7 @@ 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->setMaxNoObservationTime(max_no_observation_time_); @@ -459,7 +459,7 @@ bool CalibrationEstimator::calibrate() bool status = observation_status && estimation_status; valid_ |= status; - computeCrossValidationReprojError(estimated_object_points, estimated_image_points); + computeCrossValidationReprojectionError(estimated_object_points, estimated_image_points); return status; } @@ -484,7 +484,7 @@ bool CalibrationEstimator::calibrate( cv::SOLVEPNP_SQPNP); if (!success) { - RCLCPP_ERROR(rclcpp::get_logger("teir4_tag_utils"), "PNP failed"); + RCLCPP_ERROR(rclcpp::get_logger("tier4_tag_utils"), "PNP failed"); return false; } @@ -509,11 +509,11 @@ tf2::Transform CalibrationEstimator::toTf2( return tf2::Transform(tf2_rot_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; @@ -561,17 +561,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 @@ -763,7 +763,7 @@ 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_; } diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp index 54218397..af905af0 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp @@ -303,7 +303,7 @@ void ExtrinsicTagBasedPNPCalibrator::requestReceivedCallback( tier4_calibration_msgs::msg::CalibrationResult result; result.success = true; - result.score = estimator_.getCrossValidationReprojError(); + 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); @@ -439,14 +439,18 @@ void ExtrinsicTagBasedPNPCalibrator::automaticCalibrationTimerCallback() return error / points1.size(); }; - double initial_reproj_error = reprojection_error(image_points, initial_projected_points); - double current_reproj_error = reprojection_error(image_points, current_projected_points); - double 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:"); - RCLCPP_INFO(this->get_logger(), "\tInitial reprojection error=%.2f", initial_reproj_error); - RCLCPP_INFO(this->get_logger(), "\tCurrent reprojection error=%.2f", current_reproj_error); - RCLCPP_INFO(this->get_logger(), "\tFiltered reprojection error=%.2f", filtered_reproj_error); + 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); diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index 56ab05f6..26df7327 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -404,8 +404,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); @@ -862,7 +862,7 @@ std::vector TagCalibratorVisualizer::get3dpoints( pinhole_camera_model_.distortionCoeffs(), rvec, tvec, false, cv::SOLVEPNP_SQPNP); if (!success) { - RCLCPP_ERROR(rclcpp::get_logger("teir4_tag_utils"), "PNP failed"); + RCLCPP_ERROR(rclcpp::get_logger("tier4_tag_utils"), "PNP failed"); return object_points; } From 6fb5de1b588d513c0457dea2f05bfd9df547b86b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 28 Dec 2023 18:50:11 +0900 Subject: [PATCH 04/49] feat: adapter the lidar-lidar 2d to the new API Signed-off-by: Kenzo Lobos-Tsunekawa --- ...extrinsic_lidar_to_lidar_2d_calibrator.hpp | 135 ++++--- .../launch/calibrator.launch.xml | 44 ++- .../rviz/{velodyne_top.rviz => default.rviz} | 206 +++------- ...extrinsic_lidar_to_lidar_2d_calibrator.cpp | 355 ++++++++++-------- .../src/main.cpp | 5 +- .../lidar_lidar_2d_calibrator.launch.xml | 43 +++ .../calibrators/default_project/__init__.py | 3 +- .../lidar_lidar_2d_calibrator.py | 31 ++ 8 files changed, 466 insertions(+), 356 deletions(-) mode change 100755 => 100644 sensor/extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_lidar_to_lidar_2d_calibrator.hpp rename sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/{velodyne_top.rviz => default.rviz} (67%) mode change 100755 => 100644 sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp mode change 100755 => 100644 sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp create mode 100644 sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py 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 old mode 100755 new mode 100644 index c20cc355..34ce93ba --- 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 @@ -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. @@ -22,7 +22,9 @@ #include #include -#include +#include +#include +#include #include #include @@ -31,11 +33,6 @@ #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 @@ -44,44 +41,102 @@ #include #include #include +#include #include +namespace extrinsic_lidar_to_lidar_2d_calibrator +{ + using PointType = pcl::PointXYZ; +/** + * A 2D lidar-lidar calibrator. + * This tools assumes the existance 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 ladars have different + * resolutions and FOV, this metod 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); - - 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); - + 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); - void findBestTransform( + /*! + * 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 containin the best aligned pointcloud, trasform, and score + */ + std::tuple::Ptr, Eigen::Matrix4f, float> 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> & registratators); // 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_; @@ -105,54 +160,44 @@ class LidarToLidar2DCalibrator : public rclcpp::Node std::shared_ptr tf_buffer_; std::shared_ptr transform_listener_; - rclcpp::TimerBase::SharedPtr calib_timer_; + rclcpp::TimerBase::SharedPtr calibration_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::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_; + rclcpp::Service::SharedPtr service_server_; - // Threading, sync, and result + // Threading 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_; + 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 - 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_; + // Calibration output + geometry_msgs::msg::TransformStamped output_calibration_msg_; bool got_initial_transform_; bool received_request_; @@ -164,4 +209,6 @@ class LidarToLidar2DCalibrator : public rclcpp::Node bool first_observation_; }; +} // namespace extrinsic_lidar_to_lidar_2d_calibrator + #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 index 9079f8c0..7bcec88a 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml @@ -1,25 +1,39 @@ - + - - + + + + + + + + + + + + + + + + + + - - - - + + - - - + + + @@ -38,11 +52,17 @@ - - + + + + + + + + diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/velodyne_top.rviz b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/default.rviz similarity index 67% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/velodyne_top.rviz rename to sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/default.rviz index 6d424c49..59baffa9 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/velodyne_top.rviz +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/default.rviz @@ -6,8 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - Splitter Ratio: 0.500627338886261 - Tree Height: 725 + - /target pointcloud (raw)1 + Splitter Ratio: 0.46037736535072327 + Tree Height: 1106 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -25,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: center_source + SyncSource: source pointcloud (raw) Visualization Manager: Class: "" Displays: @@ -54,26 +55,7 @@ Visualization Manager: 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 + - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -83,15 +65,15 @@ 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 Max Color: 255; 255; 255 - Max Intensity: 140 + Max Intensity: 255 Min Color: 0; 0; 0 - Min Intensity: 1 - Name: top + Min Intensity: 0 + Name: source pointcloud (raw) Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -103,7 +85,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw + Value: /source_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -117,15 +99,15 @@ 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 Max Color: 255; 255; 255 - Max Intensity: 164 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: livox_left + Name: target pointcloud (raw) Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -137,7 +119,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_left/livox/lidar + Value: /target_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -150,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: 150 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: livox_center + Name: initial source pointcloud (filtered) Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -171,7 +153,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_center/livox/lidar + Value: /filtered_source_initial_points Use Fixed Frame: true Use rainbow: true Value: true @@ -184,16 +166,16 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity + Color: 0; 255; 0 + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 150 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: livox_right + Name: aligned source pointcloud (filtered) Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -205,7 +187,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_right/livox/lidar + Value: /filtered_source_aligned_points Use Fixed Frame: true Use rainbow: true Value: true @@ -218,62 +200,28 @@ 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: 4096 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: left_source + Name: target pointcloud (filtered) Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares + 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/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 + Value: /filtered_target_points Use Fixed Frame: true Use rainbow: true Value: true @@ -286,28 +234,28 @@ 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: 4096 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: center_source + Name: initial source pointcloud (flattened) Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares + 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/source_points_2d + Value: /flat_source_initial_points Use Fixed Frame: true Use rainbow: true Value: true @@ -326,56 +274,22 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: center_target + Name: aligned source pointcloud (flattened) 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 + 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/source_points_2d + Value: /flat_source_aligned_points Use Fixed Frame: true Use rainbow: true Value: true @@ -388,28 +302,28 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 0 + Color: 255; 0; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: right_target + Name: target pointcloud (flattened) Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares + 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/livox_front_right_base_link/target_points_2d + Reliability Policy: Best Effort + Value: /flat_target_points Use Fixed Frame: true Use rainbow: true Value: true @@ -458,8 +372,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.5749987363815308 - Class: rviz_default_plugins/TopDownOrtho + Class: rviz_default_plugins/FPS Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -468,19 +381,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 20.338735580444336 + Pitch: 1.5697963237762451 + Position: + X: 13.73879337310791 + Y: 25.009069442749023 + Z: 56.92116928100586 Target Frame: - Value: TopDownOrtho (rviz_default_plugins) - X: 4.058230876922607 - Y: 0.42603132128715515 + Value: FPS (rviz_default_plugins) + Yaw: 6.2720112800598145 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001990000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004930000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000031d000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000004ddfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004dd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000058d000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -489,6 +405,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1848 - X: 72 - Y: 27 + Width: 2486 + X: 1994 + Y: 0 diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp old mode 100755 new mode 100644 index 1900f81e..9d36b4d7 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_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. @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -26,15 +27,11 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - +#include #include -#define UNUSED(x) (void)x; +namespace extrinsic_lidar_to_lidar_2d_calibrator +{ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & options) : Node("extrinsic_lidar_to_lidar_2d_calibrator", options), @@ -50,8 +47,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 +80,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,13 +101,13 @@ 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( + service_server_ = this->create_service( "extrinsic_calibration", std::bind( &LidarToLidar2DCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -140,17 +144,16 @@ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & o } void LidarToLidar2DCalibrator::requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response) + [[maybe_unused]] const std::shared_ptr< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + 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 +165,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() @@ -198,11 +207,13 @@ bool LidarToLidar2DCalibrator::checkInitialTransforms() try { rclcpp::Time t = rclcpp::Time(0); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); + tf2::Transform initial_base_to_source_tf2_; + tf2::Transform initial_base_to_target_tf2_; - 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_); @@ -210,22 +221,9 @@ bool LidarToLidar2DCalibrator::checkInitialTransforms() 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 +233,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(source_pointcloud_kcs, target_pointcloud_kcs); - double final_error = getAlignmentError(source_pointcloud_aligned_kcs, target_pointcloud_kcs); + 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); RCLCPP_INFO(this->get_logger(), "Initial avrage 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_) { @@ -341,41 +353,84 @@ void LidarToLidar2DCalibrator::calibrationTimerCallback() } 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 +440,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 +449,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 +464,23 @@ double LidarToLidar2DCalibrator::getAlignmentError( correspondence_estimator.determineCorrespondences(correspondences); double error = 0.0; - int n = 0; + std::size_t accepted_correspondances = 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_correspondances += 1; } } - return error / n; + return error / accepted_correspondances; } -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> & registratators) { pcl::Correspondences correspondences; pcl::registration::CorrespondenceEstimation estimator; @@ -441,13 +488,13 @@ 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 & correspondance) { return correspondance.distance; }) / + correspondences.size(); std::vector transforms = {best_transform}; @@ -481,4 +528,8 @@ void LidarToLidar2DCalibrator::findBestTransform( transforms.push_back(best_registrator_transform); } -} + + return std::make_tuple<>(best_aligned_pointcloud_ptr, best_transform, best_score); +} // extrinsic_lidar_to_lidar_2d_calibrator + +} // namespace extrinsic_lidar_to_lidar_2d_calibrator diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp old mode 100755 new mode 100644 index 6c66c3e6..d956e1f3 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp @@ -23,8 +23,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/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml new file mode 100644 index 00000000..b6786df0 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py index 5942b590..df511e74 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py @@ -1,3 +1,4 @@ +from .lidar_lidar_2d_calibrator import LidarLidar2DCalibrator from .tag_based_pnp_calibrator import TagBasedPNPCalibrator -__all__ = ["TagBasedPNPCalibrator"] +__all__ = ["TagBasedPNPCalibrator", "LidarLidar2DCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py new file mode 100644 index 00000000..363fd96e --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py @@ -0,0 +1,31 @@ +# from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_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]) + + print("DefaultProject_LidarLidar2DCalibrator") + + self.add_calibrator( + service_name="calibrate_lidar_lidar", + expected_calibration_frames=[ + FramePair(parent=self.target_frame, child=self.source_frame), + ], + ) From b9a71daff2b63a0b07523af3e9480802deb45331 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 28 Dec 2023 19:05:48 +0900 Subject: [PATCH 05/49] chore: fixed spellings Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 11 +++++++++++ .../extrinsic_lidar_to_lidar_2d_calibrator.hpp | 10 +++++----- .../extrinsic_lidar_to_lidar_2d_calibrator.cpp | 15 ++++++++------- .../lidar_lidar_2d_calibrator.launch.xml | 2 +- 4 files changed, 25 insertions(+), 13 deletions(-) diff --git a/.cspell.json b/.cspell.json index b62e6b13..1fedb7ce 100644 --- a/.cspell.json +++ b/.cspell.json @@ -1,5 +1,16 @@ { "words": [ + "autoware", + "kalman", + "pointcloud", + "pointclouds", + "lidars", + "registrator", + "registrators", + "gicp", + "icp", + "vectord", + "quaterniond", "Rodrigues", "subsampled", "undistortion", 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 index 34ce93ba..2baf94f0 100644 --- 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 @@ -51,10 +51,10 @@ using PointType = pcl::PointXYZ; /** * A 2D lidar-lidar calibrator. - * This tools assumes the existance of two lidars that have been independently calibrated to the + * 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 ladars have different - * resolutions and FOV, this metod selects a range of z values (measured from the base frame), + * 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. * @@ -128,12 +128,12 @@ class LidarToLidar2DCalibrator : public rclcpp::Node * @param target_pointcloud_ptr The target pointcloud * @param target_pointcloud_ptr A vector of pointcloud registrators to solve the calibration * problem - * @return A tuple containin the best aligned pointcloud, trasform, and score + * @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> & registratators); + std::vector::Ptr> & registrators); // Parameters std::string base_frame_; diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp index 9d36b4d7..99e0331d 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp @@ -331,7 +331,7 @@ void LidarToLidar2DCalibrator::calibrationTimerCallback() double final_error = getAlignmentError(best_source_aligned_pointcloud_bcs, flat_target_pointcloud_bcs); - 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()); @@ -352,6 +352,7 @@ void LidarToLidar2DCalibrator::calibrationTimerCallback() kalman_filter_.update(x); } + // cSpell:ignore getXelement estimated_rpy.z = kalman_filter_.getXelement(0); filtered_affine.translation().x() = kalman_filter_.getXelement(1); filtered_affine.translation().y() = kalman_filter_.getXelement(2); @@ -464,23 +465,23 @@ double LidarToLidar2DCalibrator::getAlignmentError( correspondence_estimator.determineCorrespondences(correspondences); double error = 0.0; - std::size_t accepted_correspondances = 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); - accepted_correspondances += 1; + accepted_correspondences += 1; } } - return error / accepted_correspondances; + return error / accepted_correspondences; } std::tuple::Ptr, Eigen::Matrix4f, float> LidarToLidar2DCalibrator::findBestTransform( pcl::PointCloud::Ptr & source_pointcloud_ptr, pcl::PointCloud::Ptr & target_pointcloud_ptr, - std::vector::Ptr> & registratators) + std::vector::Ptr> & registrators) { pcl::Correspondences correspondences; pcl::registration::CorrespondenceEstimation estimator; @@ -493,12 +494,12 @@ LidarToLidar2DCalibrator::findBestTransform( float best_score = std::transform_reduce( correspondences.cbegin(), correspondences.cend(), 0.f, std::plus{}, - [](const pcl::Correspondence & correspondance) { return correspondance.distance; }) / + [](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( diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml index b6786df0..f29ea9e5 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml @@ -11,7 +11,7 @@ - + Date: Tue, 2 Jan 2024 22:31:43 +0900 Subject: [PATCH 06/49] feat: adapted the ground plane calibrator to the new api Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 2 + .../extrinsic_ground_plane_calibrator.hpp | 127 +++-- .../launch/calibrator.launch.xml | 65 ++- .../rviz/{xx1.rviz => default.rviz} | 215 ++------ .../rviz/velodyne_top.rviz | 351 ------------- .../rviz/x2.rviz | 399 -------------- .../src/extrinsic_ground_plane_calibrator.cpp | 489 +++++++++--------- .../src/main.cpp | 5 +- .../interactive_calibrator.py | 16 +- ...extrinsic_lidar_to_lidar_2d_calibrator.hpp | 8 +- ...extrinsic_lidar_to_lidar_2d_calibrator.cpp | 11 +- .../scripts/calibrator_ui_node.py | 2 +- .../scripts/calibrator_ui_node.py | 2 +- .../ground_plane_calibrator.launch.xml | 55 ++ .../x1/ground_plane_calibrator.launch.xml | 54 ++ .../x2/ground_plane_calibrator.launch.xml | 54 ++ .../xx1/ground_plane_calibrator.launch.xml | 54 ++ .../calibrator.py | 155 ------ .../calibrator_registry.py | 2 +- .../calibrator_wrapper.py | 4 - .../calibrators/__init__.py | 7 +- .../calibrators/default_project/__init__.py | 3 +- .../ground_plane_calibrator.py | 30 ++ .../calibrators/x1/__init__.py | 3 + .../calibrators/x1/ground_plane_calibrator.py | 47 ++ .../calibrators/x2/__init__.py | 3 + .../calibrators/x2/ground_plane_calibrator.py | 47 ++ .../calibrators/xx1/__init__.py | 3 + .../xx1/ground_plane_calibrator.py | 47 ++ .../new_extrinsic_calibration_manager.py | 12 +- .../views/launcher_configuration_view.py | 20 +- .../views/tf_view.py | 9 +- 32 files changed, 863 insertions(+), 1438 deletions(-) rename sensor/extrinsic_ground_plane_calibrator/rviz/{xx1.rviz => default.rviz} (64%) delete mode 100644 sensor/extrinsic_ground_plane_calibrator/rviz/velodyne_top.rviz delete mode 100644 sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz create mode 100644 sensor/new_extrinsic_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py diff --git a/.cspell.json b/.cspell.json index 1fedb7ce..3ebc261c 100644 --- a/.cspell.json +++ b/.cspell.json @@ -7,6 +7,7 @@ "lidars", "registrator", "registrators", + "representer", "gicp", "icp", "vectord", @@ -27,6 +28,7 @@ "lidartags", "apriltags", "pnp", + "pydot", "sqpnp", "solvepnp", "eigen", diff --git a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp b/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp index fde4a276..96dfbe4d 100644 --- a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp +++ b/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_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. @@ -15,7 +15,6 @@ #ifndef EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ #define EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ -#define PCL_NO_PRECOMPILE #include #include #include @@ -23,7 +22,9 @@ #include #include -#include +#include +#include +#include #include #include @@ -34,29 +35,53 @@ #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 extrinsic_ground_plane_calibrator +{ + using PointType = pcl::PointXYZ; +/** + * 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 { public: 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); + const std::shared_ptr request, + const std::shared_ptr response); /*! * ROS pointcloud callback @@ -66,20 +91,18 @@ 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 + * @return A tuple containing wether or not th calibration plane was found, the estimated ground + * plane model, and the inliers of the respective model */ - bool extractGroundPlane( - pcl::PointCloud::Ptr & pointcloud, Eigen::Vector4d & model, - pcl::PointCloud::Ptr & inliers); + std::tuple::Ptr> extractGroundPlane( + pcl::PointCloud::Ptr & pointcloud); /*! * Computes the fitting error of an estimated model and the initial one @@ -95,7 +118,7 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node * @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); /*! @@ -134,7 +157,7 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node * 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 + * @return the plane model */ Eigen::Vector4d poseToPlaneModel(const Eigen::Isometry3d & pose) const; @@ -142,37 +165,45 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node * 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 + * @return 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 + * 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 - * @retval the refined base lidar pose + * @return the refined base lidar pose */ - Eigen::Isometry3d refineBaseLidarPose( - const Eigen::Isometry3d & base_lidar_pose, const Eigen::Vector4d & model) const; + Eigen::Isometry3d estimateBaseLidarTransform( + const Eigen::Isometry3d & initial_base_lidar_transform, 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 + * @return the refined base lidar pose */ pcl::PointCloud::Ptr removeOutliers( pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, double outlier_tolerance) const; + /*! + * 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) 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_; double marker_size_; bool use_crop_box_filter_; @@ -191,7 +222,7 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node 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_; @@ -215,41 +246,31 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr pointcloud_sub_; - rclcpp::Service::SharedPtr service_server_; + rclcpp::Service::SharedPtr service_server_; // Threading, sync, and result std::mutex mutex_; // 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_; }; +} // namespace extrinsic_ground_plane_calibrator + #endif // EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml index 06b74ee7..efaddcce 100644 --- a/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml @@ -1,10 +1,22 @@ - + + - - + + + + + + + + + + + + + @@ -15,23 +27,35 @@ - + + + + + - - + - - + + - - - - - + + + + + + + + + + + + + + @@ -49,17 +73,10 @@ - - - - - - - - - - - + + + + diff --git a/sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz b/sensor/extrinsic_ground_plane_calibrator/rviz/default.rviz similarity index 64% rename from sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz rename to sensor/extrinsic_ground_plane_calibrator/rviz/default.rviz index e7ad85a9..69a218db 100644 --- a/sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz +++ b/sensor/extrinsic_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/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/rviz/x2.rviz b/sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz deleted file mode 100644 index 2a2e109a..00000000 --- a/sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz +++ /dev/null @@ -1,399 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /left_upper1/Topic1 - - /initial_base_link1 - - /pandar_40p_left_inliers1/Topic1 - Splitter Ratio: 0.500627338886261 - Tree Height: 719 - - 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: 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: 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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: left_upper - 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: 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.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: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 191 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: rear_lower - 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 - - 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 - Enabled: true - Length: 1 - Name: pandar_40p_front - Radius: 0.10000000149011612 - Reference Frame: pandar_40p_front - Value: true - - Class: rviz_default_plugins/Axes - 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: - {} - 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: pandar_40p_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: /top_unit/base_link/top_unit_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: pandar_40p_front_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: /front_unit/base_link/front_unit_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: pandar_40p_rear_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: /rear_unit/base_link/rear_unit_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.5850001573562622 - Position: - X: -30.511119842529297 - Y: 2.103529930114746 - Z: 24.23221778869629 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 6.260024547576904 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002b40000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003670000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1846 - X: 1994 - 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 index 118f4eef..edbaed89 100644 --- a/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp +++ b/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_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,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -26,27 +27,19 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include +namespace extrinsic_ground_plane_calibrator +{ + 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) +: Node("extrinsic_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"); - parent_frame_ = this->declare_parameter("parent_frame"); - child_frame_ = this->declare_parameter("child_frame"); + lidar_frame_ = this->declare_parameter("lidar_frame"); marker_size_ = this->declare_parameter("marker_size", 20.0); @@ -68,7 +61,7 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod 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); + 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); @@ -108,7 +101,7 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod // The service server runs in a dedicated thread srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - service_server_ = this->create_service( + service_server_ = this->create_service( "extrinsic_calibration", std::bind( &ExtrinsicGroundPlaneCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -128,48 +121,81 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod } void ExtrinsicGroundPlaneCalibrator::requestReceivedCallback( - __attribute__((unused)) - const std::shared_ptr + [[maybe_unused]] const std::shared_ptr< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> request, - const std::shared_ptr response) + 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(10s); + rclcpp::sleep_for(1s); std::unique_lock lock(mutex_); if (calibration_done_) { 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_parent_to_child_msg_; + 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 = + 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) { - lidar_frame_ = msg->header.frame_id; + 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 (!checkInitialTransforms() || calibration_done_) { + if (!received_request || !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 @@ -185,8 +211,10 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( } // Extract the ground plane model - Eigen::Vector4d ground_plane_model; - if (!extractGroundPlane(pointcloud, ground_plane_model, inliers_pointcloud)) { + auto [ground_plane_result, ground_plane_model, inliers_pointcloud] = + extractGroundPlane(pointcloud); + + if (!ground_plane_result) { return; } @@ -198,10 +226,11 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( 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); + 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); @@ -210,6 +239,7 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( bool ExtrinsicGroundPlaneCalibrator::checkInitialTransforms() { if (lidar_frame_ == "") { + RCLCPP_ERROR(this->get_logger(), "The lidar frame can not be empty !"); return false; } @@ -221,37 +251,26 @@ bool ExtrinsicGroundPlaneCalibrator::checkInitialTransforms() rclcpp::Time t = rclcpp::Time(0); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - initial_base_to_lidar_msg_ = + geometry_msgs::msg::Transform initial_base_to_lidar_transform_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_); + 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()); + 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) +std::tuple::Ptr> +ExtrinsicGroundPlaneCalibrator::extractGroundPlane(pcl::PointCloud::Ptr & pointcloud) { + Eigen::Vector4d model; + pcl::PointCloud::Ptr inliers_pointcloud(new pcl::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)); @@ -272,7 +291,7 @@ bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( rough_normal = vectors.col(2); } else { rough_normal = - (initial_base_to_lidar_eigen_.inverse().rotation() * Eigen::Vector3d(0.0, 0.0, 1.0)) + (initial_base_to_lidar_transform_.inverse().rotation() * Eigen::Vector3d(0.0, 0.0, 1.0)) .cast(); } @@ -339,7 +358,8 @@ bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( extract.setIndices(inliers); extract.setNegative(false); extract.filter(*inliers_pointcloud); - return true; + + return std::make_tuple(true, model, inliers_pointcloud); } else { if (remove_outliers_) { bool accept = true; @@ -384,13 +404,13 @@ bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( iteration_cloud->swap(next_cloud); iteration_size = iteration_cloud->height * iteration_cloud->width; } - return false; + return std::make_tuple(false, model, inliers_pointcloud); } void ExtrinsicGroundPlaneCalibrator::evaluateModels( const Eigen::Vector4d & estimated_model, pcl::PointCloud::Ptr inliers) const { - auto modelError = + 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; @@ -400,172 +420,153 @@ void ExtrinsicGroundPlaneCalibrator::evaluateModels( return sum / (pc->height * pc->width); }; - Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_eigen_.inverse(); + Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_transform_.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); + model_error(initial_model(0), initial_model(1), initial_model(2), initial_model(3), inliers); - float estimated_model_error = modelError( + float estimated_model_error = model_error( 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( +void ExtrinsicGroundPlaneCalibrator::filterGroundModelEstimation( 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::Isometry3d estimated_base_to_lidar_transform = + estimateBaseLidarTransform(initial_base_to_lidar_transform_, ground_plane_model); Eigen::Vector3d estimated_translation; auto estimated_rpy = - tier4_autoware_utils::getRPY(tf2::toMsg(estimated_parent_to_child_eigen).orientation); + tier4_autoware_utils::getRPY(tf2::toMsg(estimated_base_to_lidar_transform).orientation); auto initial_rpy = - tier4_autoware_utils::getRPY(tf2::toMsg(initial_parent_to_child_eigen).orientation); + tier4_autoware_utils::getRPY(tf2::toMsg(initial_base_to_lidar_transform_).orientation); if (verbose_) { RCLCPP_INFO( - this->get_logger(), "Initial parent->child euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", + 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 parent->child euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", + 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 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()); + 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 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()); + 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()); } - // 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); + // 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; } - // 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; + // 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); + } - tf2::fromMsg(estimated_orientation_msg, estimated_orientation_eigen); + // 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]; + } - output_parent_to_child_eigen_.linear() = estimated_orientation_eigen.toRotationMatrix(); - output_parent_to_child_eigen_.translation() = estimated_translation; + 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)); - // 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(), "Convergence thresh: angle=%.2e, translation=%.2e", + angle_convergence_threshold_, translation_convergence_threshold_); - 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)); + // Save the inliers for later refinement + inlier_observations_.add(inliers); - RCLCPP_INFO( - this->get_logger(), "Convergence thresh: angle=%.2e, translation=%.2e", - angle_convergence_threshold_, translation_convergence_threshold_); + if (!converged) { + return; + } - if (!converged) { - return; - } + // Integrate all the inliers so far and refine the estimation + pcl::PointCloud::Ptr augmented_inliers(new pcl::PointCloud); - pcl::PointCloud::Ptr augmented_inliers(new pcl::PointCloud); + for (const auto & inliers : inlier_observations_.get()) { + *augmented_inliers += *inliers; + } - for (const auto & inliers : inlier_observations_.get()) { - *augmented_inliers += *inliers; - } + // 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(10 * max_inlier_distance_); + seg.setMaxIterations(max_iterations_); - 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; - } - } + 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()); + + std::unique_lock lock(mutex_); + calibrated_base_to_lidar_transform_ = + estimateBaseLidarTransform(initial_base_to_lidar_transform_, output_model); + calibration_done_ = true; } void ExtrinsicGroundPlaneCalibrator::visualizeCalibration( @@ -573,7 +574,7 @@ void ExtrinsicGroundPlaneCalibrator::visualizeCalibration( { visualization_msgs::msg::MarkerArray markers; - Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_eigen_.inverse(); + Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_transform_.inverse(); visualizePlaneModel("initial_calibration_pose", initial_lidar_base_transform, markers); @@ -674,7 +675,7 @@ void ExtrinsicGroundPlaneCalibrator::visualizePlaneModel( 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()); + 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"; @@ -682,40 +683,32 @@ void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_pl 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"; + raw_lidar_to_base_msg.child_frame_id = "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 calibrated_base_to_lidar_transform = + calibration_done_ + ? calibrated_base_to_lidar_transform_ + : 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 = overwriteXYYawValues( + initial_base_to_lidar_transform_msg_, calibrated_base_to_lidar_transform_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); + 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); } Eigen::Vector4d ExtrinsicGroundPlaneCalibrator::poseToPlaneModel( @@ -773,32 +766,37 @@ Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::modelPlaneToPose( return pose; } -Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::refineBaseLidarPose( - const Eigen::Isometry3d & base_lidar_pose, const Eigen::Vector4d & model) const +Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::estimateBaseLidarTransform( + const Eigen::Isometry3d & initial_base_to_lidar_transform, 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(); + 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 ExtrinsicGroundPlaneCalibrator::removeOutliers( @@ -828,3 +826,30 @@ pcl::PointCloud::Ptr ExtrinsicGroundPlaneCalibrator::removeOutliers( return inliers; } + +geometry_msgs::msg::TransformStamped ExtrinsicGroundPlaneCalibrator::overwriteXYYawValues( + const geometry_msgs::msg::TransformStamped & initial_base_lidar_transform_msg, + const geometry_msgs::msg::TransformStamped & calibrated_base_lidar_transform_msg) const +{ + 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 extrinsic_ground_plane_calibrator diff --git a/sensor/extrinsic_ground_plane_calibrator/src/main.cpp b/sensor/extrinsic_ground_plane_calibrator/src/main.cpp index 547ba88a..8a16e5da 100644 --- a/sensor/extrinsic_ground_plane_calibrator/src/main.cpp +++ b/sensor/extrinsic_ground_plane_calibrator/src/main.cpp @@ -23,8 +23,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_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py index 6dff89d4..b9938d22 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py @@ -230,7 +230,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" ) @@ -272,7 +272,7 @@ def calibration_intrinsics_callback(): self.calibration2_button.setEnabled(False) self.calibration2_button.setText("Optimizing...") self.optimize_camera_intrinsics_waiting = True - assert self.optimize_camera_intrinsics_status == True + assert self.optimize_camera_intrinsics_status is True self.calibration2_button = QPushButton("Calibrate intrinsics\n(experimental)") self.calibration2_button.clicked.connect(calibration_intrinsics_callback) @@ -648,8 +648,8 @@ def load_calibration_callback(self): object_calibration_points = np.loadtxt(os.path.join(input_dir, "object_points.txt")) 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) @@ -946,8 +946,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( @@ -1002,13 +1002,13 @@ def main(args=None): signal.signal(signal.SIGINT, sigint_handler) ros_interface = RosInterface() - ex = InteractiveCalibratorUI(ros_interface) + ex = InteractiveCalibratorUI(ros_interface) # noqa: F841 ros_interface.spin() sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quiting...") + print("Received sigint. Quitting...") rclpy.shutdown() 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 index 2baf94f0..737340dc 100644 --- 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 @@ -199,14 +199,14 @@ class LidarToLidar2DCalibrator : public rclcpp::Node // Calibration output geometry_msgs::msg::TransformStamped output_calibration_msg_; - bool got_initial_transform_; - bool received_request_; + bool got_initial_transform_{false}; + bool received_request_{false}; bool broadcast_tf_; - bool calibration_done_; + bool calibration_done_{false}; // Filtering KalmanFilter kalman_filter_; - bool first_observation_; + bool first_observation_{true}; }; } // namespace extrinsic_lidar_to_lidar_2d_calibrator diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp index 99e0331d..4c00c1f7 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp @@ -34,12 +34,7 @@ namespace extrinsic_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("extrinsic_lidar_to_lidar_2d_calibrator", options), tf_broadcaster_(this) { using std::chrono_literals::operator""ms; @@ -207,8 +202,6 @@ bool LidarToLidar2DCalibrator::checkInitialTransforms() try { rclcpp::Time t = rclcpp::Time(0); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); - tf2::Transform initial_base_to_source_tf2_; - tf2::Transform initial_base_to_target_tf2_; geometry_msgs::msg::Transform initial_base_to_source_msg_ = tf_buffer_->lookupTransform(base_frame_, source_pointcloud_frame_, t, timeout).transform; @@ -216,8 +209,6 @@ bool LidarToLidar2DCalibrator::checkInitialTransforms() 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_); diff --git a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py b/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py index d65ec068..4360b43e 100755 --- a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py @@ -38,7 +38,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quiting...", flush=True) + print("Received sigint. Quitting...", flush=True) rclpy.shutdown() diff --git a/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py b/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py index 9fbb257c..0abdfe88 100755 --- a/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py @@ -38,7 +38,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quiting...", flush=True) + print("Received sigint. Quitting...", flush=True) rclpy.shutdown() diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..a46f4b69 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..de209df0 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..ee479fda --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml new file mode 100644 index 00000000..de209df0 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py deleted file mode 100644 index 7c4fe170..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator.py +++ /dev/null @@ -1,155 +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 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 - - -class Calibrator: - def __init__(self): - # Calibration parameters - self.min_points = None - self.inlier_error = None - self.flags = None - self.use_ransac = None - self.ransac_iters = 200 - - # Camera parameters - self.k = None - self.d = None - pass - - def set_min_points(self, min_points): - self.min_points = min_points - - def set_inlier_error(self, inlier_error): - self.inlier_error = inlier_error - - def set_camera_info(self, k, d): - self.k = np.array(k).reshape(3, 3) - self.d = np.array(d).reshape( - -1, - ) - - def set_method(self, method): - if method == "sqpnp": - self.flags = cv2.SOLVEPNP_SQPNP - else: - self.flags = cv2.SOLVEPNP_ITERATIVE - - def set_ransac(self, use_ransac): - self.use_ransac = use_ransac - - def calibrate(self, object_points, image_points): - if len(object_points) == 0 or len(image_points) == 0: - return None - - object_points = np.array(object_points, dtype=np.float64) - image_points = np.array(image_points, dtype=np.float64) - - num_points, dim = object_points.shape - assert dim == 3 - assert num_points == image_points.shape[0] - - if num_points < self.min_points: - return None - - if self.use_ransac: - return self.calibrate_ransac(object_points, image_points) - - tvec = np.zeros((3,)) - rvec = np.zeros((3, 3)) - - try: - retval, rvec, tvec = cv2.solvePnP( - object_points, image_points, self.k, self.d, flags=self.flags - ) - except Exception as e: - print(e) - - camera_to_lidar_transform = cv_to_transformation_matrix(tvec, rvec) - - return camera_to_lidar_transform - - def calibrate_ransac(self, object_points, image_points): - num_points, _ = object_points.shape - - best_tvec = np.zeros((3,)) - best_rvec = np.zeros((3, 3)) - best_inliers = -1 - best_error = np.inf - - for _ in range(self.ransac_iters): - indexes = np.random.choice(num_points, min(num_points, self.min_points)) - object_points_iter = object_points[indexes, :] - image_points_iter = image_points[indexes, :] - - try: - retval, iter_rvec, iter_tvec = cv2.solvePnP( - object_points_iter, image_points_iter, self.k, self.d, flags=self.flags - ) - except Exception as e: - print(e) - continue - - reproj_error_iter, inliers = self.calculate_reproj_error( - object_points, image_points, tvec=iter_tvec, rvec=iter_rvec - ) - - if ( - inliers.sum() == best_inliers and reproj_error_iter < best_error - ) or inliers.sum() > best_inliers: - best_error = reproj_error_iter - best_inliers = inliers.sum() - best_tvec = iter_tvec - best_rvec = iter_rvec - - camera_to_lidar_transform = cv_to_transformation_matrix(best_tvec, best_rvec) - - return camera_to_lidar_transform - - def calculate_reproj_error( - self, object_points, image_points, tvec=None, rvec=None, tf_msg=None, transform_matrix=None - ): - if isinstance(object_points, list) and isinstance(image_points, list): - if len(object_points) == 0: - return 0.0, 0 - - object_points = np.array(object_points, dtype=np.float64) - image_points = np.array(image_points, dtype=np.float64) - - if tf_msg is not None: - transform_matrix = tf_message_to_transform_matrix(tf_msg) - - if transform_matrix is not None: - tvec, rvec = transform_matrix_to_cv(transform_matrix) - - assert tvec is not None - assert rvec is not None - num_points, dim = object_points.shape - projected_points, _ = cv2.projectPoints(object_points, rvec, tvec, self.k, self.d) - projected_points = projected_points.reshape((num_points, 2)) - reproj_error = np.linalg.norm(projected_points - image_points, axis=1) - - if self.use_ransac: - inliers = reproj_error <= self.inlier_error - else: - inliers = np.ones_like(reproj_error) - - return reproj_error.mean(), inliers diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py index 76cf612f..715c300b 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py @@ -45,7 +45,7 @@ def inner_wrapper(wrapped_class: CalibratorBase) -> CalibratorBase: @classmethod def create_calibrator(cls, project_name: str, calibrator_name: str, **kwargs) -> CalibratorBase: - """Create the excecutor using a factory pattern. + """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 diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py index 87595094..3d13d716 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py @@ -13,10 +13,6 @@ from tier4_calibration_msgs.msg import CalibrationResult from tier4_calibration_msgs.srv import NewExtrinsicCalibrator -# import debugpy -# debugpy.listen(5678) -# debugpy.wait_for_client() - class CalibratorServiceWrapper(QObject): data_changed = Signal() diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py index 963d1070..81c5dd00 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py @@ -1,8 +1,7 @@ -# __all__ = ['basemapper', 'lxml'] -# from basemaper import * -# import lxml - from .default_project import * # noqa: F401, F403 from .dummy_project import * # noqa: F401, F403 from .tier4_dummy_project 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py index df511e74..838fe6b6 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py @@ -1,4 +1,5 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator from .lidar_lidar_2d_calibrator import LidarLidar2DCalibrator from .tag_based_pnp_calibrator import TagBasedPNPCalibrator -__all__ = ["TagBasedPNPCalibrator", "LidarLidar2DCalibrator"] +__all__ = ["GroundPlaneCalibrator", "TagBasedPNPCalibrator", "LidarLidar2DCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py new file mode 100644 index 00000000..636fd791 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py @@ -0,0 +1,30 @@ +# from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_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]) + + print("DefaultProject_GroundPlane2DCalibrator") + + self.add_calibrator( + service_name="calibrate_base_lidar", + expected_calibration_frames=[ + FramePair(parent=self.base_frame, child=self.lidar_frame), + ], + ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/__init__.py new file mode 100644 index 00000000..0db7adc9 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/__init__.py @@ -0,0 +1,3 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator + +__all__ = ["GroundPlaneCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py new file mode 100644 index 00000000..0063c128 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py @@ -0,0 +1,47 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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]) + + print("X1_GroundPlane2DCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py new file mode 100644 index 00000000..0db7adc9 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py @@ -0,0 +1,3 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator + +__all__ = ["GroundPlaneCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py new file mode 100644 index 00000000..14af1df7 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py @@ -0,0 +1,47 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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]) + + print("X2_GroundPlane2DCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py new file mode 100644 index 00000000..0db7adc9 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py @@ -0,0 +1,3 @@ +from .ground_plane_calibrator import GroundPlaneCalibrator + +__all__ = ["GroundPlaneCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py new file mode 100644 index 00000000..94e4fae2 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py @@ -0,0 +1,47 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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]) + + print("XX1_GroundPlane2DCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py index 166e01f1..e1770557 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py @@ -48,10 +48,6 @@ from new_extrinsic_calibration_manager.views.tf_view import TfView import rclpy -# import debugpy -# debugpy.listen(5678) -# debugpy.wait_for_client() - class NewExtrinsicCalibrationManager(QMainWindow): tfs_graph_signal = Signal(object) @@ -63,7 +59,7 @@ def __init__(self): # self.central_widget.resize(1000,1000) self.setCentralWidget(self.central_widget) - # self.setWindowTitle("New extrinsic calibration manaer") + # self.setWindowTitle("New extrinsic calibration manager") self.ros_interface: RosInterface = None @@ -130,8 +126,8 @@ def on_selected_calibrator(self, project_name, calibrator_name): f"on_selected_calibrator: project_name={project_name} calibrator_name={calibrator_name}", flush=True, ) - self.laucher_configuration_view = LauncherConfigurationView(project_name, calibrator_name) - self.laucher_configuration_view.launcher_parameters.connect( + self.launcher_configuration_view = LauncherConfigurationView(project_name, calibrator_name) + self.launcher_configuration_view.launcher_parameters.connect( partial(self.launch_calibrators, project_name, calibrator_name) ) pass @@ -288,7 +284,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quiting...") + print("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 9b5c9d71..4598167b 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -29,10 +29,6 @@ from PySide2.QtWidgets import QWidget from ament_index_python.packages import get_package_share_directory -# import debugpy -# debugpy.listen(5678) -# debugpy.wait_for_client() - class LauncherConfigurationView(QWidget): """A simple widget to visualize and edit a ParameterizedClass's parameters.""" @@ -92,12 +88,12 @@ def __init__(self, project_name, calibrator_name): if element.hasAttribute("default"): self.optional_arguments_dict[element.getAttribute("name")] = { "value": element.getAttribute("default"), - "decription": description, + "description": description, } else: self.required_arguments_dict[element.getAttribute("name")] = { "value": "", - "decription": description, + "description": description, } self.required_argument_layout.addWidget(QLabel("Name"), 0, 0) @@ -115,10 +111,10 @@ def __init__(self, project_name, calibrator_name): self.arguments_widgets_dict[argument_name].setMinimumWidth(400) self.arguments_widgets_dict[argument_name].setMaximumWidth(800) - description_label = QLabel(argument_data["decription"]) + description_label = QLabel(argument_data["description"]) description_label.setMaximumWidth(400) - description_label.setToolTip(argument_data["decription"]) - description_label.setText(argument_data["decription"]) + 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( @@ -141,10 +137,10 @@ def __init__(self, project_name, calibrator_name): self.arguments_widgets_dict[argument_name].setMinimumWidth(400) self.arguments_widgets_dict[argument_name].setMaximumWidth(800) - description_label = QLabel(argument_data["decription"]) + description_label = QLabel(argument_data["description"]) description_label.setMaximumWidth(400) - description_label.setToolTip(argument_data["decription"]) - description_label.setText(argument_data["decription"]) + 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py index 29393cdd..a6fba7a7 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py @@ -30,10 +30,6 @@ import pydot import transforms3d -# import debugpy -# debugpy.listen(5678) -# debugpy.wait_for_client() - class TfNode: def __init__(self, frame, transform): @@ -209,6 +205,7 @@ def setTfs( # graph.write_png("frames.png") # graph.write_pdf("frames.pdf") + # cspell: ignore imgdata imgdata = graph.create_svg() # imgdata = StringIO() @@ -274,10 +271,6 @@ def resizeEvent(self, event): # scaled_pix_size = self.pix.size() # scaled_pix_size.scale(self.data_renderer.widget_size, Qt.KeepAspectRatio) - # import debugpy - # debugpy.listen(5678) - # debugpy.wait_for_client() - for item in self.scene.items(): item.prepareGeometryChange() item.update() From 905943764c2e983b085908ec84509f14aecea961 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 10 Jan 2024 13:49:56 +0900 Subject: [PATCH 07/49] feat: applied the changes required for the mapping-based lidar-lidar & base-lidar calibrators Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 75 +- .../tier4_ground_plane_utils/CMakeLists.txt | 24 + .../ground_plane_utils.hpp | 133 +++ common/tier4_ground_plane_utils/package.xml | 32 + .../src/ground_plane_utils.cpp | 375 ++++++++ .../extrinsic_ground_plane_calibrator.hpp | 86 +- .../package.xml | 1 + .../src/extrinsic_ground_plane_calibrator.cpp | 417 +------- .../launch/calibrator.launch.xml | 8 +- .../CMakeLists.txt | 5 - .../base_lidar_calibrator.hpp | 45 +- .../calibration_mapper.hpp | 24 +- .../camera_calibrator.hpp | 15 +- .../extrinsic_mapping_based_calibrator.hpp | 39 +- .../filters/best_frames_filter.hpp | 2 +- .../filters/dynamics_filter.hpp | 2 +- .../filters/filter.hpp | 2 +- .../filters/lost_state_filter.hpp | 2 +- .../filters/object_detection_filter.hpp | 2 +- .../filters/sequential_filter.hpp | 2 +- .../lidar_calibrator.hpp | 20 +- .../sensor_calibrator.hpp | 15 +- .../serialization.hpp | 6 +- .../types.hpp | 15 +- .../utils.hpp | 20 +- .../voxel_grid_filter_wrapper.hpp | 2 +- .../launch/calibrator.launch.xml | 32 +- .../package.xml | 1 + .../rviz/{x2.rviz => default.rviz} | 478 ++++++++-- .../rviz/x1.rviz | 887 ------------------ .../rviz/xx1.rviz | 886 ----------------- .../src/base_lidar_calibrator.cpp | 240 ++--- .../src/calibration_mapper.cpp | 216 +++-- .../src/camera_calibrator.cpp | 62 +- .../extrinsic_mapping_based_calibrator.cpp | 444 +++------ .../src/filters/dynamics_filter.cpp | 2 +- .../src/filters/object_detection_filter.cpp | 3 +- .../src/lidar_calibrator.cpp | 219 +---- .../src/sensor_calibrator.cpp | 2 +- .../src/utils.cpp | 27 +- ...ng_based_lidar_lidar_calibrator.launch.xml | 51 + ...ing_based_base_lidar_calibrator.launch.xml | 108 +++ ...ng_based_lidar_lidar_calibrator.launch.xml | 100 ++ ...ing_based_base_lidar_calibrator.launch.xml | 77 ++ ...ng_based_lidar_lidar_calibrator.launch.xml | 51 + .../calibrators/default_project/__init__.py | 8 +- .../mapping_based_lidar_lidar_calibrator.py | 29 + .../calibrators/x2/__init__.py | 8 +- .../x2/mapping_based_base_lidar_calibrator.py | 48 + .../mapping_based_lidar_lidar_calibrator.py | 139 +++ .../calibrators/xx1/__init__.py | 8 +- .../mapping_based_base_lidar_calibrator.py | 48 + .../mapping_based_lidar_lidar_calibrator.py | 75 ++ .../views/launcher_configuration_view.py | 18 +- 54 files changed, 2351 insertions(+), 3285 deletions(-) create mode 100755 common/tier4_ground_plane_utils/CMakeLists.txt create mode 100644 common/tier4_ground_plane_utils/include/tier4_ground_plane_utils/ground_plane_utils.hpp create mode 100755 common/tier4_ground_plane_utils/package.xml create mode 100644 common/tier4_ground_plane_utils/src/ground_plane_utils.cpp rename sensor/extrinsic_mapping_based_calibrator/rviz/{x2.rviz => default.rviz} (66%) delete mode 100644 sensor/extrinsic_mapping_based_calibrator/rviz/x1.rviz delete mode 100644 sensor/extrinsic_mapping_based_calibrator/rviz/xx1.rviz create mode 100644 sensor/new_extrinsic_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py diff --git a/.cspell.json b/.cspell.json index 3ebc261c..95184399 100644 --- a/.cspell.json +++ b/.cspell.json @@ -1,48 +1,55 @@ { "words": [ + "3dpoints", + "Rodrigues", + "apriltag", + "apriltags", "autoware", + "auxiliar", + "calib", + "coeffs", + "crossvalidation", + "discretization", + "distro", + "eigen", + "gicp", + "homography", + "hsize", + "icp", + "idless", "kalman", + "keyframes", + "lidars", + "lidartag", + "lidartags", + "matx", + "permutate", + "pnp", "pointcloud", "pointclouds", - "lidars", + "prerejective", + "pydot", + "quaterniond", + "ransac", + "rclcpp", "registrator", "registrators", "representer", - "gicp", - "icp", - "vectord", - "quaterniond", - "Rodrigues", - "subsampled", - "undistortion", - "uniformingly", - "hsize", + "reprojection", + "rosbag", "rvec", - "tvec", "rvecs", - "tvecs", - "rclcpp", - "ransac", - "lidartag", - "apriltag", - "lidartags", - "apriltags", - "pnp", - "pydot", - "sqpnp", + "rviz", + "slerp", "solvepnp", - "eigen", - "homography", - "reprojection", - "permutate", - "distro", - "matx", - "idless", - "crossvalidation", - "prerejective", - "3dpoints", - "calib", - "coeffs", - "rviz" + "sqpnp", + "srvs", + "subsampled", + "tvec", + "tvecs", + "undistortion", + "uniformingly", + "vectord", + "voxel" ] } 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/common/tier4_ground_plane_utils/package.xml b/common/tier4_ground_plane_utils/package.xml new file mode 100755 index 00000000..89e99232 --- /dev/null +++ b/common/tier4_ground_plane_utils/package.xml @@ -0,0 +1,32 @@ + + + + tier4_ground_plane_utils + 0.0.1 + The tier4_ground_plane_utils package + Kenzo Lobos Tsunekawa + + BSD + + ament_cmake_auto + + autoware_cmake + + eigen + geometry_msgs + pcl_conversions + pcl_ros + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + 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/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp b/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp index 96dfbe4d..f5d51711 100644 --- a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp +++ b/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -27,9 +28,9 @@ #include #include -#include #include #include +#include #include #include #include @@ -46,6 +47,7 @@ namespace extrinsic_ground_plane_calibrator { using PointType = pcl::PointXYZ; +using tier4_ground_plane_utils::GroundPlaneExtractorParameters; /** * A base-lidar calibrator. @@ -95,23 +97,6 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node */ bool checkInitialTransforms(); - /*! - * 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); - - /*! - * 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 @@ -152,75 +137,12 @@ 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 - * @return 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 - * @return the plane pose - */ - Eigen::Isometry3d modelPlaneToPose(const Eigen::Vector4d & model) const; - - /*! - * 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) 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 - * @return the refined base lidar pose - */ - pcl::PointCloud::Ptr removeOutliers( - pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, - double outlier_tolerance) const; - - /*! - * 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) const; - // Parameters std::string base_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 overwrite_xy_yaw_; bool filter_estimations_; diff --git a/sensor/extrinsic_ground_plane_calibrator/package.xml b/sensor/extrinsic_ground_plane_calibrator/package.xml index 5c64ad95..2ce7a073 100644 --- a/sensor/extrinsic_ground_plane_calibrator/package.xml +++ b/sensor/extrinsic_ground_plane_calibrator/package.xml @@ -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/src/extrinsic_ground_plane_calibrator.cpp b/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp index edbaed89..d4b8a305 100644 --- a/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp +++ b/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp @@ -16,15 +16,6 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -43,24 +34,27 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod 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_ = + 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); - max_cos_distance_ = this->declare_parameter("max_cos_distance", 0.2); - max_iterations_ = this->declare_parameter("max_iterations", 500); + 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); @@ -158,8 +152,8 @@ void ExtrinsicGroundPlaneCalibrator::requestReceivedCallback( "tool"; if (overwrite_xy_yaw_) { - result.transform_stamped = - overwriteXYYawValues(initial_base_to_lidar_transform_msg_, result.transform_stamped); + result.transform_stamped = tier4_ground_plane_utils::overwriteXYYawValues( + initial_base_to_lidar_transform_msg_, result.transform_stamped); } response->results.emplace_back(result); @@ -199,10 +193,11 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( pcl::fromROSMsg(*msg, *pointcloud); // Filter the pointcloud using previous outlier models - if (remove_outliers_) { + if (ground_plane_extractor_parameters_.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_); + pointcloud = tier4_ground_plane_utils::removeOutliers( + pointcloud, outlier_model, ground_plane_extractor_parameters_.remove_outlier_tolerance_); } RCLCPP_INFO( @@ -212,14 +207,20 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( // Extract the ground plane model auto [ground_plane_result, ground_plane_model, inliers_pointcloud] = - extractGroundPlane(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 - evaluateModels(ground_plane_model, inliers_pointcloud); + 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; @@ -265,179 +266,12 @@ bool ExtrinsicGroundPlaneCalibrator::checkInitialTransforms() return true; } -std::tuple::Ptr> -ExtrinsicGroundPlaneCalibrator::extractGroundPlane(pcl::PointCloud::Ptr & pointcloud) -{ - Eigen::Vector4d model; - pcl::PointCloud::Ptr inliers_pointcloud(new pcl::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_transform_.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 std::make_tuple(true, model, inliers_pointcloud); - } 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 std::make_tuple(false, model, inliers_pointcloud); -} - -void ExtrinsicGroundPlaneCalibrator::evaluateModels( - const Eigen::Vector4d & estimated_model, pcl::PointCloud::Ptr inliers) const -{ - 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); - }; - - Eigen::Isometry3d initial_lidar_base_transform = initial_base_to_lidar_transform_.inverse(); - Eigen::Vector4d initial_model = poseToPlaneModel(initial_lidar_base_transform); - - 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(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::filterGroundModelEstimation( const Eigen::Vector4d & ground_plane_model, pcl::PointCloud::Ptr inliers) { Eigen::Isometry3d estimated_base_to_lidar_transform = - estimateBaseLidarTransform(initial_base_to_lidar_transform_, ground_plane_model); + tier4_ground_plane_utils::estimateBaseLidarTransform( + initial_base_to_lidar_transform_, ground_plane_model); Eigen::Vector3d estimated_translation; auto estimated_rpy = @@ -539,33 +373,19 @@ void ExtrinsicGroundPlaneCalibrator::filterGroundModelEstimation( *augmented_inliers += *inliers; } - // 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(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]); + 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", - output_model(0), output_model(1), output_model(2), output_model(3), - final_inliers->indices.size(), + 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_ = - estimateBaseLidarTransform(initial_base_to_lidar_transform_, output_model); + calibrated_base_to_lidar_transform_ = tier4_ground_plane_utils::estimateBaseLidarTransform( + initial_base_to_lidar_transform_, final_model); calibration_done_ = true; } @@ -578,7 +398,8 @@ void ExtrinsicGroundPlaneCalibrator::visualizeCalibration( visualizePlaneModel("initial_calibration_pose", initial_lidar_base_transform, markers); - Eigen::Vector4d fake_model = poseToPlaneModel(initial_lidar_base_transform); + Eigen::Vector4d fake_model = + tier4_ground_plane_utils::poseToPlaneModel(initial_lidar_base_transform); visualizePlaneModel("initial_calibration_model", fake_model, markers); @@ -590,7 +411,7 @@ void ExtrinsicGroundPlaneCalibrator::visualizeCalibration( void ExtrinsicGroundPlaneCalibrator::visualizePlaneModel( const std::string & name, Eigen::Vector4d model, visualization_msgs::msg::MarkerArray & markers) { - visualizePlaneModel(name, modelPlaneToPose(model), markers); + visualizePlaneModel(name, tier4_ground_plane_utils::modelPlaneToPose(model), markers); } void ExtrinsicGroundPlaneCalibrator::visualizePlaneModel( @@ -681,7 +502,8 @@ void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_pl 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); + 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); @@ -692,14 +514,14 @@ void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_pl tf_broadcaster_.sendTransform(raw_lidar_to_base_msg); Eigen::Isometry3d calibrated_base_to_lidar_transform = - calibration_done_ - ? calibrated_base_to_lidar_transform_ - : estimateBaseLidarTransform(initial_base_to_lidar_transform_, ground_plane_model); + 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 = overwriteXYYawValues( + calibrated_base_to_lidar_transform_msg = tier4_ground_plane_utils::overwriteXYYawValues( initial_base_to_lidar_transform_msg_, calibrated_base_to_lidar_transform_msg); } @@ -711,145 +533,4 @@ void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_pl tf_broadcaster_.sendTransform(lidar_to_calibrated_base_transform_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::estimateBaseLidarTransform( - const Eigen::Isometry3d & initial_base_to_lidar_transform, const Eigen::Vector4d & model) const -{ - 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 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; -} - -geometry_msgs::msg::TransformStamped ExtrinsicGroundPlaneCalibrator::overwriteXYYawValues( - const geometry_msgs::msg::TransformStamped & initial_base_lidar_transform_msg, - const geometry_msgs::msg::TransformStamped & calibrated_base_lidar_transform_msg) const -{ - 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 extrinsic_ground_plane_calibrator 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 index 7bcec88a..fad1332c 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml @@ -26,6 +26,10 @@ + + + + @@ -54,10 +58,6 @@ - - - - diff --git a/sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt b/sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt index 09f4c9fc..3ce411e4 100644 --- a/sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt +++ b/sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt @@ -18,11 +18,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} 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 index 10caddee..d66cf2e2 100644 --- 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 @@ -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. @@ -28,6 +28,7 @@ #include #include +#include #include class BaseLidarCalibrator : public SensorCalibrator @@ -44,55 +45,23 @@ class BaseLidarCalibrator : public SensorCalibrator 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 + * 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; - - virtual void singleSensorCalibrationCallback( - __attribute__((unused)) const std::shared_ptr - request, - __attribute__((unused)) const std::shared_ptr - response) - { - } + std::tuple calibrate() override; - virtual void multipleSensorCalibrationCallback( - __attribute__((unused)) const std::shared_ptr - request, - __attribute__((unused)) const std::shared_ptr - response) - { - } - - virtual void configureCalibrators() {} + void configureCalibrators() override {} 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 Eigen::Vector4d & ground_model, const Eigen::Matrix4f & pose, 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_; diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/calibration_mapper.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/calibration_mapper.hpp index b428dff8..58a84acf 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/calibration_mapper.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. @@ -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_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/camera_calibrator.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/camera_calibrator.hpp index bb17e783..397e195d 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/camera_calibrator.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. @@ -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 diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_mapping_based_calibrator.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_mapping_based_calibrator.hpp index 30518943..832877f9 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_mapping_based_calibrator.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_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. @@ -32,8 +32,8 @@ #include #include #include -#include #include +#include #include #include @@ -59,17 +59,16 @@ 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, - const std::shared_ptr request, - const std::shared_ptr response); + /*! + * 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); /*! * Message callback for detected objects @@ -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,6 +143,7 @@ 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_; diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/best_frames_filter.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/best_frames_filter.hpp index 0847e9d4..f82a7c16 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/best_frames_filter.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/dynamics_filter.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/dynamics_filter.hpp index f0dc67c9..ca373855 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/dynamics_filter.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/filter.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/filter.hpp index ff5797f4..e7cca00e 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/filter.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/lost_state_filter.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/lost_state_filter.hpp index 7ebea281..1c35a087 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/lost_state_filter.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/object_detection_filter.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/object_detection_filter.hpp index 6666c69d..adcb0164 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/object_detection_filter.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/sequential_filter.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/sequential_filter.hpp index 9645cae2..dfa45f3a 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/filters/sequential_filter.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/lidar_calibrator.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/lidar_calibrator.hpp index e6090be7..d9f27277 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/lidar_calibrator.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. @@ -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_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/sensor_calibrator.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/sensor_calibrator.hpp index f8552a84..463a6a32 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/sensor_calibrator.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. @@ -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, diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/serialization.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/serialization.hpp index 08f4f7d4..3f865187 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/serialization.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. @@ -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_; diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/types.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/types.hpp index 36b79ef3..9e8de59d 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/types.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. @@ -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_ diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/utils.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/utils.hpp index 9d2630f2..0bfea953 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/utils.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. @@ -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 diff --git a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/voxel_grid_filter_wrapper.hpp b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/voxel_grid_filter_wrapper.hpp index cf4040fd..e0645cae 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/voxel_grid_filter_wrapper.hpp +++ b/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml index 244d7d2e..57dee3df 100644 --- a/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml @@ -1,23 +1,23 @@ - + + + + + + + - - - - - - - + @@ -58,21 +58,21 @@ + + + + + + - - - - - - @@ -82,6 +82,7 @@ + @@ -123,10 +124,13 @@ + + + diff --git a/sensor/extrinsic_mapping_based_calibrator/package.xml b/sensor/extrinsic_mapping_based_calibrator/package.xml index 14564cc8..8ae3cf4e 100644 --- a/sensor/extrinsic_mapping_based_calibrator/package.xml +++ b/sensor/extrinsic_mapping_based_calibrator/package.xml @@ -39,6 +39,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/extrinsic_mapping_based_calibrator/rviz/default.rviz similarity index 66% rename from sensor/extrinsic_mapping_based_calibrator/rviz/x2.rviz rename to sensor/extrinsic_mapping_based_calibrator/rviz/default.rviz index 4fd7d98a..15ddf925 100644 --- a/sensor/extrinsic_mapping_based_calibrator/rviz/x2.rviz +++ b/sensor/extrinsic_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/extrinsic_mapping_based_calibrator/rviz/x1.rviz b/sensor/extrinsic_mapping_based_calibrator/rviz/x1.rviz deleted file mode 100644 index 16b45eb0..00000000 --- a/sensor/extrinsic_mapping_based_calibrator/rviz/x1.rviz +++ /dev/null @@ -1,887 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /mapping velodyne pointcloud1/Topic1 - - /Frame Path1/Topic1 - 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: output map -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: /sensor_kit/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: /sensor_kit/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: /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 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensor_kit/keyframe_path - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: KeyFrame Markers - 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 - - Alpha: 1 - Autocompute Intensity Bounds: 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: livox_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: /sensor_kit/livox_front_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: livox_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: /sensor_kit/livox_front_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: livox_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: /sensor_kit/livox_front_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: livox_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: /sensor_kit/livox_front_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: livox_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: /sensor_kit/livox_front_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: livox_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: /sensor_kit/livox_front_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: livox_center_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: /sensor_kit/livox_front_center/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: livox_center_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: /sensor_kit/livox_front_center/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: livox_center_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: /sensor_kit/livox_front_center/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.6499999165534973 - Position: - X: -3.9148199558258057 - Y: 1.8611059188842773 - Z: 6.485511302947998 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 5.768178939819336 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002df00000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004510000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1846 - X: 74 - Y: 0 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 index f64968e5..4d076448 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/base_lidar_calibrator.cpp +++ b/sensor/extrinsic_mapping_based_calibrator/src/base_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. @@ -15,6 +15,8 @@ #include #include #include +#include +#include #include #include @@ -23,14 +25,6 @@ #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, @@ -46,50 +40,32 @@ BaseLidarCalibrator::BaseLidarCalibrator( { } -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) +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::Matrix4f initial_lidar_to_base_transform; - Eigen::Isometry3d initial_lidar_to_base_affine; + Eigen::Isometry3d initial_base_to_lidar_transform; 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(); + 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 false; + return std::make_tuple<>(false, Eigen::Matrix4d::Identity(), 0.f); } - 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()); + initial_base_to_lidar_transform.cast()); pcl::CropBox box_filter; box_filter.setMin(Eigen::Vector4f( @@ -102,166 +78,90 @@ bool BaseLidarCalibrator::calibrate( 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(); + *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); + } - 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_); + publishResults( + ground_plane_model, last_keyframe->pose_, ground_plane_inliers_ptr, augmented_pointcloud_ptr); - // Extract the ground plane inliers - extract.setInputCloud(iteration_cloud); - extract.setIndices(inliers); - extract.setNegative(false); - extract.filter(*inliers_pointcloud); - return true; - } + Eigen::Isometry3d calibrated_base_to_lidar_transform = + tier4_ground_plane_utils::estimateBaseLidarTransform( + initial_base_to_lidar_transform, ground_plane_model); - // Extract the inliers from the pointcloud (the detected plane was not the ground plane) - extract.setInputCloud(iteration_cloud); - extract.setIndices(inliers); - extract.setNegative(true); + 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); - pcl::PointCloud next_cloud; - extract.filter(next_cloud); + calibrated_base_to_lidar_transform_msg = tier4_ground_plane_utils::overwriteXYYawValues( + initial_base_to_lidar_transform_msg_, calibrated_base_to_lidar_transform_msg); - iteration_cloud->swap(next_cloud); - iteration_size = iteration_cloud->height * iteration_cloud->width; + calibrated_base_to_lidar_transform = + tf2::transformToEigen(calibrated_base_to_lidar_transform_msg); } - return false; + // 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 pcl::PointCloud::Ptr & ground_plane_inliers_ptr, - const pcl::PointCloud::Ptr & augmented_pointcloud_ptr) + 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) { + 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_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; + 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_ptr, ground_plane_inliers_msg); - pcl::toROSMsg(*augmented_pointcloud_ptr, augmented_pointcloud_msg); + 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_->mapping_lidar_frame_; - augmented_pointcloud_msg.header.frame_id = data_->mapping_lidar_frame_; + 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 = modelPlaneToPose(ground_model); + 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); } - -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_mapping_based_calibrator/src/calibration_mapper.cpp b/sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp index 6618c128..f68580fd 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp +++ b/sensor/extrinsic_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. @@ -15,17 +15,12 @@ #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,22 +434,24 @@ 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; + trans_angle_diff = delta_pose2.translation().norm() > parameters_->new_keyframe_min_distance_ + ? trans_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) + (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_ || @@ -428,7 +461,7 @@ void CalibrationMapper::checkKeyframeLost(Frame::Ptr keyframe) RCLCPP_WARN( rclcpp::get_logger("calibration_mapper"), "Mapping failed. Angle between keyframes is too high. a1=%.2f (deg) a2=%.2f (deg) " - "theshold=%.2f (deg)", + "threshold=%.2f (deg)", trans_angle_diff, rot_angle_diff, parameters_->lost_frame_max_angle_diff_); return; @@ -579,15 +612,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 +756,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 +769,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 +778,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 +797,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 +825,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 +914,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 +960,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/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp index 675fc96d..405a520c 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp +++ b/sensor/extrinsic_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. @@ -19,6 +19,7 @@ #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_ * @@ -280,25 +268,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/extrinsic_mapping_based_calibrator.cpp b/sensor/extrinsic_mapping_based_calibrator/src/extrinsic_mapping_based_calibrator.cpp index e53798d7..e164217a 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/extrinsic_mapping_based_calibrator.cpp +++ b/sensor/extrinsic_mapping_based_calibrator/src/extrinsic_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. @@ -17,6 +17,7 @@ #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 @@ -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); @@ -301,9 +280,14 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( 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); + 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,13 +296,16 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( target_markers_pub); } - for (const auto & frame_name : mapping_data_->calibration_lidar_frame_names_) { + 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_, @@ -334,6 +321,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 +342,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 +363,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 +387,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 +403,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 +412,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 +425,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 +506,125 @@ 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, - const std::shared_ptr response) +void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( + [[maybe_unused]] const std::shared_ptr< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + 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 +648,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 +673,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 +681,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 +730,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/filters/dynamics_filter.cpp b/sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp index d32a9d1f..3ca31cac 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp +++ b/sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp @@ -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/object_detection_filter.cpp b/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp index 25cee1d4..ff16b060 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp +++ b/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp @@ -99,7 +99,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/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp index 05449bd0..065c4c46 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp +++ b/sensor/extrinsic_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. @@ -20,16 +20,9 @@ #include #include #include - -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else #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/sensor_calibrator.cpp b/sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp index 1465949a..fb8705ef 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp +++ b/sensor/extrinsic_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. diff --git a/sensor/extrinsic_mapping_based_calibrator/src/utils.cpp b/sensor/extrinsic_mapping_based_calibrator/src/utils.cpp index 07856e8b..5d94a0ee 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/utils.cpp +++ b/sensor/extrinsic_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. @@ -15,18 +15,13 @@ #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()); @@ -124,9 +120,9 @@ Eigen::Matrix4f poseInterpolation( te -= dt; } - auto asd = poseInterpolationBase(te, 0, dt, Eigen::Matrix4f::Identity(), dm); + auto rem = poseInterpolationBase(te, 0, dt, Eigen::Matrix4f::Identity(), dm); - return m * asd; + return m * rem; } template @@ -137,15 +133,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/new_extrinsic_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..639c8cb6 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..de73e9dc --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..9cbb96f2 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..52bb4af3 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..b055959b --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py index 838fe6b6..bb5c08e3 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py @@ -1,5 +1,11 @@ from .ground_plane_calibrator import GroundPlaneCalibrator from .lidar_lidar_2d_calibrator import LidarLidar2DCalibrator +from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator from .tag_based_pnp_calibrator import TagBasedPNPCalibrator -__all__ = ["GroundPlaneCalibrator", "TagBasedPNPCalibrator", "LidarLidar2DCalibrator"] +__all__ = [ + "GroundPlaneCalibrator", + "LidarLidar2DCalibrator", + "MappingBasedLidarLidarCalibrator", + "TagBasedPNPCalibrator", +] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..622a79de --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,29 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_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]) + + print("default_MappingBasedLidarLidarCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py index 0db7adc9..4d724c46 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py @@ -1,3 +1,9 @@ from .ground_plane_calibrator import GroundPlaneCalibrator +from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator +from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator -__all__ = ["GroundPlaneCalibrator"] +__all__ = [ + "GroundPlaneCalibrator", + "MappingBasedBaseLidarCalibrator", + "MappingBasedLidarLidarCalibrator", +] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py new file mode 100644 index 00000000..8cebe83e --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py @@ -0,0 +1,48 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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] + ) + + print("X2_MappingBasedBaseLidarCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..80a7403d --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,139 @@ +from collections import defaultdict +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("X2_MappingBasedLidarLidarCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py index 0db7adc9..4d724c46 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py @@ -1,3 +1,9 @@ from .ground_plane_calibrator import GroundPlaneCalibrator +from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator +from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator -__all__ = ["GroundPlaneCalibrator"] +__all__ = [ + "GroundPlaneCalibrator", + "MappingBasedBaseLidarCalibrator", + "MappingBasedLidarLidarCalibrator", +] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py new file mode 100644 index 00000000..6ed48f3c --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py @@ -0,0 +1,48 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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] + ) + + print("XX1_MappingBasedBaseLidarCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..16c24e46 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,75 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("XX1_MappingBasedLidarLidarCalibrator") + + 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]]): + print(f"post_process\n{calibration_transforms}") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 4598167b..dbc47c88 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -16,6 +16,7 @@ from functools import reduce +from typing import Dict import xml.dom.minidom from PySide2.QtCore import Signal @@ -86,8 +87,10 @@ def __init__(self, project_name, calibrator_name): element.getAttribute("description") if element.hasAttribute("description") else " " ) if element.hasAttribute("default"): + default_value = element.getAttribute("default").replace(" ", "") + self.optional_arguments_dict[element.getAttribute("name")] = { - "value": element.getAttribute("default"), + "value": default_value, "description": description, } else: @@ -104,7 +107,9 @@ def __init__(self, project_name, calibrator_name): name_label = QLabel(argument_name) name_label.setMaximumWidth(400) - self.arguments_widgets_dict[argument_name] = QLineEdit(argument_data["value"]) + default_value = argument_data["value"].getAttribute("default").replace(" ", "") + + self.arguments_widgets_dict[argument_name] = QLineEdit(default_value) self.arguments_widgets_dict[argument_name].textChanged.connect( self.check_argument_status ) @@ -181,11 +186,18 @@ def check_argument_status(self, text=None): print("check_argument_status", flush=True) def on_click(self): - args_dict = { + args_dict: Dict[str, str] = { arg_name: args_widget.text() 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] = [item.strip() for item in value.strip("[]").split(",")] + print(args_dict, flush=True) self.launcher_parameters.emit(args_dict) From a6c6bdfc22811452fc8ab57a65fdcc4037f99ff4 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 11 Jan 2024 13:12:39 +0900 Subject: [PATCH 08/49] fix: fixed non-optional parameters in the launcher configuration ui Signed-off-by: Kenzo Lobos-Tsunekawa --- .../views/launcher_configuration_view.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index dbc47c88..18fb2177 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -107,7 +107,7 @@ def __init__(self, project_name, calibrator_name): name_label = QLabel(argument_name) name_label.setMaximumWidth(400) - default_value = argument_data["value"].getAttribute("default").replace(" ", "") + default_value = argument_data["value"].replace(" ", "") self.arguments_widgets_dict[argument_name] = QLineEdit(default_value) self.arguments_widgets_dict[argument_name].textChanged.connect( From 3f0f35cbf403ccc8a479ae13aa5805ff6dbf1da4 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 11 Jan 2024 14:41:41 +0900 Subject: [PATCH 09/49] chore: removed unused dummy calibrator Signed-off-by: Kenzo Lobos-Tsunekawa --- .../extrinsic_dummy_calibrator/CMakeLists.txt | 29 --------- .../extrinsic_dummy_calibrator.hpp | 63 ------------------- .../launch/calibrator.launch.xml | 14 ----- sensor/extrinsic_dummy_calibrator/package.xml | 30 --------- .../src/extrinsic_dummy_calibrator.cpp | 60 ------------------ 5 files changed, 196 deletions(-) delete mode 100644 sensor/extrinsic_dummy_calibrator/CMakeLists.txt delete mode 100644 sensor/extrinsic_dummy_calibrator/include/extrinsic_dummy_calibrator/extrinsic_dummy_calibrator.hpp delete mode 100644 sensor/extrinsic_dummy_calibrator/launch/calibrator.launch.xml delete mode 100644 sensor/extrinsic_dummy_calibrator/package.xml delete mode 100644 sensor/extrinsic_dummy_calibrator/src/extrinsic_dummy_calibrator.cpp 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/package.xml b/sensor/extrinsic_dummy_calibrator/package.xml deleted file mode 100644 index 3b86ddaa..00000000 --- a/sensor/extrinsic_dummy_calibrator/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - extrinsic_dummy_calibrator - 0.1.0 - The extrinsic_dummy_calibrator package - Kenzo Lobos Tsunekawa - Apache License 2.0 - - ament_cmake_auto - - eigen - geometry_msgs - message_filters - pcl_conversions - pcl_ros - rclcpp - sensor_msgs - std_msgs - tf2 - tf2_geometry_msgs - tier4_calibration_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - 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; -} From 84e364fd70a1e63b94efa68fcc83be4c6a041e20 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 12 Jan 2024 18:42:34 +0900 Subject: [PATCH 10/49] feat: partial implementation of the changes for radar-lidar and implementation of a native way to edit the launchers Signed-off-by: Kenzo Lobos-Tsunekawa --- .../CMakeLists.txt | 8 +- .../__init__.py | 0 .../calibrator_ui.py | 4 +- .../ros_interface.py | 2 +- ...trinsic_marker_radar_lidar_calibrator.hpp} | 60 +-- .../track.hpp | 15 +- .../types.hpp | 13 +- .../launch/calibrator.launch.xml | 17 +- .../package.xml | 4 +- .../rviz/default.rviz} | 39 +- .../scripts/calibrator_ui_node.py | 6 +- ...trinsic_marker_radar_lidar_calibrator.cpp} | 115 ++--- .../src/main.cpp | 9 +- .../src/track.cpp | 11 +- .../rviz/x2_front_left.rviz | 405 ------------------ .../rviz/x2_front_right.rviz | 405 ------------------ .../rviz/x2_rear_center.rviz | 405 ------------------ .../rviz/x2_rear_left.rviz | 405 ------------------ .../rviz/x2_rear_right.rviz | 405 ------------------ .../rviz/xx1.rviz | 387 ----------------- .../marker_radar_lidar_calibrator.launch.xml | 49 +++ .../calibrators/x2/__init__.py | 2 + .../x2/marker_radar_lidar_calibrator.py | 46 ++ .../new_extrinsic_calibration_manager.py | 42 +- .../utils.py | 2 +- .../views/launcher_configuration_view.py | 85 ++-- .../views/tf_view.py | 2 +- 27 files changed, 369 insertions(+), 2574 deletions(-) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/CMakeLists.txt (67%) rename sensor/{extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator}/__init__.py (100%) rename sensor/{extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator}/calibrator_ui.py (97%) rename sensor/{extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator}/ros_interface.py (99%) rename sensor/{extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp => extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp} (84%) rename sensor/{extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator}/track.hpp (87%) rename sensor/{extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator}/types.hpp (82%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/launch/calibrator.launch.xml (74%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/package.xml (89%) rename sensor/{extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz => extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz} (92%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/scripts/calibrator_ui_node.py (87%) rename sensor/{extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp => extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp} (93%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/src/main.cpp (70%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/src/track.cpp (95%) delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_left.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_right.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_center.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_right.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/xx1.rviz create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py diff --git a/sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt b/sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt similarity index 67% rename from sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt rename to sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt index efaeb51f..9959501e 100644 --- a/sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_reflector_based_calibrator) +project(extrinsic_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(extrinsic_marker_radar_lidar_calibrator + src/extrinsic_marker_radar_lidar_calibrator.cpp src/track.cpp src/main.cpp ) -target_link_libraries(extrinsic_reflector_based_calibrator +target_link_libraries(extrinsic_marker_radar_lidar_calibrator ${OpenCV_LIBS} ) diff --git a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/__init__.py b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_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/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/__init__.py diff --git a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/calibrator_ui.py similarity index 97% rename from sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py rename to sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/calibrator_ui.py index cc76acf8..8c18bd3a 100644 --- a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_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/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py similarity index 99% rename from sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py rename to sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py index 82fb99a6..a5f75b92 100644 --- a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_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. diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp similarity index 84% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp index b063c4ba..8d169f37 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_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 EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ +#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ #include -#include -#include +#include +#include #include #include #include @@ -25,7 +25,9 @@ #include #include -#include +#include +#include +#include #include #include @@ -35,20 +37,18 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include +#include #include #include #include #include #include +namespace extrinsic_marker_radar_lidar_calibrator +{ + class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node { public: @@ -59,8 +59,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node protected: void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); void timerCallback(); @@ -121,7 +121,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node struct Parameters { - std::string parent_frame; + std::string radar_parallel_frame; bool use_lidar_initial_crop_box_filter; double lidar_initial_crop_box_min_x; double lidar_initial_crop_box_min_y; @@ -183,7 +183,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr lidar_sub_; rclcpp::Subscription::SharedPtr radar_sub_; - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr calibration_request_server_; rclcpp::Service::SharedPtr background_model_service_server_; rclcpp::Service::SharedPtr tracking_service_server_; @@ -198,22 +198,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_; @@ -225,11 +225,13 @@ 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_; }; -#endif // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__EXTRINSIC_REFLECTOR_BASED_CALIBRATOR_HPP_ +} // namespace extrinsic_marker_radar_lidar_calibrator + +#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/track.hpp similarity index 87% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/track.hpp index ee0f6349..c87cf962 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_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 EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ +#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ #include #include -#include +#include #include #include #include +namespace extrinsic_marker_radar_lidar_calibrator +{ + class Track { public: @@ -81,4 +84,6 @@ class TrackFactory double max_matching_distance_; }; -#endif // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TRACK_HPP_ +} // namespace extrinsic_marker_radar_lidar_calibrator + +#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/types.hpp similarity index 82% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/types.hpp index e05c2fbf..987ea08e 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_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 EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ #include @@ -25,6 +25,9 @@ #include #include +namespace extrinsic_marker_radar_lidar_calibrator +{ + struct BackgroundModel { public: @@ -54,4 +57,6 @@ struct BackgroundModel TreeType tree_; }; -#endif // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__TYPES_HPP_ +} // namespace extrinsic_marker_radar_lidar_calibrator + +#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml similarity index 74% rename from sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml rename to sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml index b61fba19..cdec8468 100644 --- a/sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml @@ -1,7 +1,10 @@ - + + + + @@ -25,8 +28,10 @@ - - + + + + @@ -49,6 +54,10 @@ - + + + + + diff --git a/sensor/extrinsic_reflector_based_calibrator/package.xml b/sensor/extrinsic_marker_radar_lidar_calibrator/package.xml similarity index 89% rename from sensor/extrinsic_reflector_based_calibrator/package.xml rename to sensor/extrinsic_marker_radar_lidar_calibrator/package.xml index 8932c4fa..84e838f5 100644 --- a/sensor/extrinsic_reflector_based_calibrator/package.xml +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_reflector_based_calibrator + extrinsic_marker_radar_lidar_calibrator 0.0.1 - The extrinsic_reflector_based_calibrator package + The extrinsic_marker_radar_lidar_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz b/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz similarity index 92% rename from sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz rename to sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz index f90051b6..1d1360ff 100644 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz @@ -28,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: lidar Visualization Manager: Class: "" Displays: @@ -69,7 +69,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw + Value: /pointcloud_topic Use Fixed Frame: true Use rainbow: true Value: true @@ -103,7 +103,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_background_pointcloud + Value: /lidar_background_pointcloud Use Fixed Frame: true Use rainbow: true Value: false @@ -137,7 +137,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_foreground_pointcloud + Value: /lidar_foreground_pointcloud Use Fixed Frame: true Use rainbow: true Value: false @@ -171,7 +171,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_colored_clusters + Value: /lidar_colored_clusters Use Fixed Frame: true Use rainbow: true Value: true @@ -185,7 +185,7 @@ Visualization Manager: 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: /lidar_detection_markers Value: true - BUS: Alpha: 0.9990000128746033 @@ -212,7 +212,10 @@ Visualization Manager: Color: 119; 11; 32 Name: DetectedObjects Namespaces: - {} + label: true + shape: true + twist: true + velocity: true PEDESTRIAN: Alpha: 0.9990000128746033 Color: 255; 192; 203 @@ -244,7 +247,7 @@ Visualization Manager: 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: /radar_detection_markers Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -258,7 +261,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,10 +279,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/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 @@ -290,7 +293,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/tracking_markers + Value: /tracking_markers Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -325,7 +328,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_front + Fixed Frame: radar_frame Frame Rate: 30 Name: root Tools: @@ -376,14 +379,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.2697972357273102 + Pitch: 0.3347971737384796 Position: - X: 0.19182920455932617 - Y: 5.7288312911987305 - Z: 3.2564549446105957 + X: -5.563827037811279 + Y: 0.02594861388206482 + Z: 3.217146873474121 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 4.681333541870117 + Yaw: 6.261345386505127 Saved: ~ Window Geometry: Displays: diff --git a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py b/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py similarity index 87% rename from sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py rename to sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py index 4360b43e..103f1c56 100755 --- a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/extrinsic_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. @@ -18,8 +18,8 @@ import sys from PySide2.QtWidgets import QApplication -from extrinsic_reflector_based_calibrator import CalibratorUI -from extrinsic_reflector_based_calibrator import RosInterface +from extrinsic_marker_radar_lidar_calibrator import CalibratorUI +from extrinsic_marker_radar_lidar_calibrator import RosInterface import rclpy diff --git a/sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp b/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp similarity index 93% rename from sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp index 329e787e..28b722bf 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_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,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include #include #include @@ -29,15 +30,10 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include +#include #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(parameters, #NAME, PARAM_STRUCT.NAME) @@ -59,6 +55,9 @@ void update_param( } } // namespace +namespace extrinsic_marker_radar_lidar_calibrator +{ + rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::paramCallback( const std::vector & parameters) { @@ -69,7 +68,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); @@ -118,19 +117,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("extrinsic_reflector_based_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); @@ -264,7 +255,7 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( std::bind(&ExtrinsicReflectorBasedCalibrator::paramCallback, this, std::placeholders::_1)); calibration_request_server_ = - this->create_service( + this->create_service( "extrinsic_calibration", std::bind( &ExtrinsicReflectorBasedCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -284,10 +275,10 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( } void ExtrinsicReflectorBasedCalibrator::requestReceivedCallback( - __attribute__((unused)) - const std::shared_ptr + [[maybe_unused]] const std::shared_ptr< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> request, - const std::shared_ptr response) + const std::shared_ptr response) { using std::chrono_literals::operator""s; @@ -305,12 +296,15 @@ 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; + tier4_calibration_msgs::msg::CalibrationResult result; + result.message.data = "Calibration successful"; + result.score = 0.f; + 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() @@ -336,8 +330,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; @@ -363,8 +357,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; @@ -392,8 +386,8 @@ void ExtrinsicReflectorBasedCalibrator::trackingRequestCallback( } 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; @@ -938,15 +932,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) { @@ -1135,7 +1128,7 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) { auto track = converged_tracks_[track_index]; const auto & lidar_estimation = track.getLidarEstimation(); - const auto & lidar_estimation_pcs = parent_to_lidar_eigen_ * lidar_estimation; + const auto & lidar_estimation_pcs = radar_parallel_to_lidar_eigen_ * lidar_estimation; const auto & lidar_transformed_estimation = initial_radar_to_lidar_eigen_ * lidar_estimation; const auto & radar_estimation_rcs = track.getRadarEstimation(); lidar_points_pcs->emplace_back(eigen_to_pcl_2d(lidar_estimation_pcs)); @@ -1161,16 +1154,36 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() // 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 is 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(); @@ -1265,6 +1278,8 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() 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 && @@ -1275,6 +1290,8 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() "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(), @@ -1475,3 +1492,5 @@ void ExtrinsicReflectorBasedCalibrator::visualizationMarkers( tracking_markers_pub_->publish(tracking_marker_array); } + +} // namespace extrinsic_marker_radar_lidar_calibrator diff --git a/sensor/extrinsic_reflector_based_calibrator/src/main.cpp b/sensor/extrinsic_marker_radar_lidar_calibrator/src/main.cpp similarity index 70% rename from sensor/extrinsic_reflector_based_calibrator/src/main.cpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/src/main.cpp index 374ecae8..254cd810 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/main.cpp +++ b/sensor/extrinsic_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/track.cpp b/sensor/extrinsic_marker_radar_lidar_calibrator/src/track.cpp similarity index 95% rename from sensor/extrinsic_reflector_based_calibrator/src/track.cpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/src/track.cpp index c331f524..c7cc8b7e 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/track.cpp +++ b/sensor/extrinsic_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,10 +13,13 @@ // limitations under the License. #include -#include -#include +#include +#include #include +namespace extrinsic_marker_radar_lidar_calibrator +{ + Track::Track( builtin_interfaces::msg::Time & t0, const KalmanFilter & initial_lidar_filter, const KalmanFilter & initial_radar_filter, double lidar_convergence_thresh, @@ -151,3 +154,5 @@ Track TrackFactory::makeTrack( t0, lidar_filter, radar_filter, lidar_convergence_thresh_, radar_convergence_thresh_, timeout_thresh_, max_matching_distance_); } + +} // namespace extrinsic_marker_radar_lidar_calibrator 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 054a899f..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_forground_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 7adf1073..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_forground_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 abe01b41..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_forground_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_left.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz deleted file mode 100644 index d42fde54..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_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/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_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_forground_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_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: /rear_unit/rear_unit_base_link/rear_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: /rear_unit/rear_unit_base_link/rear_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/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: - {} - 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: 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_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: /rear_unit/rear_unit_base_link/rear_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: /rear_unit/rear_unit_base_link/rear_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: 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.2947973608970642 - Position: - X: 4.067726135253906 - Y: 0.976508617401123 - Z: 2.6349170207977295 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 3.4413163661956787 - 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 8e9f180a..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_forground_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 8fc65e00..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_forground_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/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..a70c9abb --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py index 4d724c46..c70b97f4 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py @@ -1,9 +1,11 @@ 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 __all__ = [ "GroundPlaneCalibrator", "MappingBasedBaseLidarCalibrator", "MappingBasedLidarLidarCalibrator", + "MarkerRadarLidarCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py new file mode 100644 index 00000000..2eae86ce --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py @@ -0,0 +1,46 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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]) + + print("X2_MarkerRadarLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_radar_lidar", + expected_calibration_frames=[ + FramePair(parent=self.radar_parallel_frame, child=self.radar_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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py index e1770557..47113fce 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.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. @@ -36,6 +36,11 @@ 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 from new_extrinsic_calibration_manager.calibration_manager_model import CalibratorManagerModel from new_extrinsic_calibration_manager.calibrator_base import CalibratorState from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry @@ -132,16 +137,18 @@ def on_selected_calibrator(self, project_name, calibrator_name): ) pass - def launch_calibrators(self, project_name: str, calibrator_name: str, argument_dict: Dict): + def launch_calibrators( + self, project_name: str, calibrator_name: str, launch_argument_dict: Dict + ): # Show the main UI self.show() # Execute the launcher - print(argument_dict, flush=True) - argument_list = [f"{k}:={v}" for k, v in argument_dict.items()] + print(launch_argument_dict, flush=True) + argument_list = [f"{k}:={v}" for k, v in launch_argument_dict.items()] package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") - path = ( + launcher_path = ( package_share_directory + "/launch/" + project_name @@ -149,7 +156,25 @@ def launch_calibrators(self, project_name: str, calibrator_name: str, argument_d + calibrator_name + ".launch.xml" ) - self.process = subprocess.Popen(["ros2", "launch", path] + argument_list) + self.process = subprocess.Popen(["ros2", "launch", launcher_path] + argument_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: + print("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) + + print(context.launch_configurations) # Start the ROS interface self.ros_interface = RosInterface() @@ -157,7 +182,10 @@ def launch_calibrators(self, project_name: str, calibrator_name: str, argument_d # Create the calibrator wrapper self.calibrator = CalibratorRegistry.create_calibrator( - project_name, calibrator_name, ros_interface=self.ros_interface, **argument_dict + 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) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py index c232f0db..53121c62 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/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. diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 18fb2177..55aca765 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -17,9 +17,9 @@ from functools import reduce from typing import Dict -import xml.dom.minidom 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 @@ -29,6 +29,9 @@ 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): @@ -59,7 +62,7 @@ def __init__(self, project_name, calibrator_name): self.arguments_widgets_dict = {} package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") - path = ( + launcher_path = ( package_share_directory + "/launch/" + project_name @@ -68,35 +71,38 @@ def __init__(self, project_name, calibrator_name): + ".launch.xml" ) - print(f"Reading xml from: {path}") + print(f"Reading xml from: {launcher_path}") try: - xml_doc = xml.dom.minidom.parse(path) + with open(launcher_path) as f: + root_entity, parser = Parser.load(f) except Exception as e: print("Failed reading xml file. Either not-existent or invalid") raise e - arg_nodes = [ - node - for node in xml_doc.getElementsByTagName("arg") - if node.parentNode == xml_doc.firstChild - ] + ld: LaunchDescription = parser.parse_description(root_entity) - for element in arg_nodes: - description = ( - element.getAttribute("description") if element.hasAttribute("description") else " " - ) - if element.hasAttribute("default"): - default_value = element.getAttribute("default").replace(" ", "") + for e in ld.entities: + if not isinstance(e, DeclareLaunchArgument): + continue + + description = e.description if e.description != "no description given" else "" - self.optional_arguments_dict[element.getAttribute("name")] = { + if 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[element.getAttribute("name")] = { + self.required_arguments_dict[e.name] = { "value": "", "description": description, + "choices": e.choices, } self.required_argument_layout.addWidget(QLabel("Name"), 0, 0) @@ -109,10 +115,21 @@ def __init__(self, project_name, calibrator_name): default_value = argument_data["value"].replace(" ", "") - self.arguments_widgets_dict[argument_name] = QLineEdit(default_value) - self.arguments_widgets_dict[argument_name].textChanged.connect( - self.check_argument_status - ) + 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) @@ -135,10 +152,21 @@ def __init__(self, project_name, calibrator_name): name_label = QLabel(argument_name) name_label.setMaximumWidth(400) - self.arguments_widgets_dict[argument_name] = QLineEdit(argument_data["value"]) - self.arguments_widgets_dict[argument_name].textChanged.connect( - self.check_argument_status - ) + 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) @@ -180,7 +208,10 @@ def check_argument_status(self, text=None): self.launch_button.setEnabled( reduce( lambda a, b: a and b, - [len(widget.text()) > 0 for widget in self.arguments_widgets_dict.values()], + [ + len(widget.text()) > 0 if hasattr(widget, "text") else widget.currentText() + for widget in self.arguments_widgets_dict.values() + ], ) ) print("check_argument_status", flush=True) @@ -188,6 +219,8 @@ def check_argument_status(self, text=None): 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() } diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py index a6fba7a7..d40cca51 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_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. From f7bab99f69e35231ee622767319bab6b05470a66 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 12 Jan 2024 18:42:34 +0900 Subject: [PATCH 11/49] feat: partial implementation of the changes for radar-lidar and implementation of a native way to edit the launchers Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 1 + .../CMakeLists.txt | 8 +- .../__init__.py | 0 .../calibrator_ui.py | 4 +- .../ros_interface.py | 2 +- ...trinsic_marker_radar_lidar_calibrator.hpp} | 60 +-- .../track.hpp | 17 +- .../types.hpp | 15 +- .../launch/calibrator.launch.xml | 17 +- .../package.xml | 4 +- .../rviz/default.rviz} | 39 +- .../scripts/calibrator_ui_node.py | 6 +- ...trinsic_marker_radar_lidar_calibrator.cpp} | 129 +++--- .../src/main.cpp | 9 +- .../src/track.cpp | 11 +- .../rviz/x2_front_left.rviz | 405 ------------------ .../rviz/x2_front_right.rviz | 405 ------------------ .../rviz/x2_rear_center.rviz | 405 ------------------ .../rviz/x2_rear_left.rviz | 405 ------------------ .../rviz/x2_rear_right.rviz | 405 ------------------ .../rviz/xx1.rviz | 387 ----------------- .../marker_radar_lidar_calibrator.launch.xml | 49 +++ .../calibrators/x2/__init__.py | 2 + .../x2/marker_radar_lidar_calibrator.py | 46 ++ .../new_extrinsic_calibration_manager.py | 42 +- .../utils.py | 2 +- .../views/launcher_configuration_view.py | 85 ++-- .../views/tf_view.py | 2 +- 28 files changed, 379 insertions(+), 2583 deletions(-) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/CMakeLists.txt (67%) rename sensor/{extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator}/__init__.py (100%) rename sensor/{extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator}/calibrator_ui.py (97%) rename sensor/{extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator}/ros_interface.py (99%) rename sensor/{extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp => extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp} (84%) rename sensor/{extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator}/track.hpp (84%) rename sensor/{extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator}/types.hpp (78%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/launch/calibrator.launch.xml (74%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/package.xml (89%) rename sensor/{extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz => extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz} (92%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/scripts/calibrator_ui_node.py (87%) rename sensor/{extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp => extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp} (93%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/src/main.cpp (70%) rename sensor/{extrinsic_reflector_based_calibrator => extrinsic_marker_radar_lidar_calibrator}/src/track.cpp (95%) delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_left.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_right.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_center.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_right.rviz delete mode 100644 sensor/extrinsic_reflector_based_calibrator/rviz/xx1.rviz create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py diff --git a/.cspell.json b/.cspell.json index 95184399..03c78c93 100644 --- a/.cspell.json +++ b/.cspell.json @@ -32,6 +32,7 @@ "quaterniond", "ransac", "rclcpp", + "rclpy", "registrator", "registrators", "representer", diff --git a/sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt b/sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt similarity index 67% rename from sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt rename to sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt index efaeb51f..9959501e 100644 --- a/sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_reflector_based_calibrator) +project(extrinsic_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(extrinsic_marker_radar_lidar_calibrator + src/extrinsic_marker_radar_lidar_calibrator.cpp src/track.cpp src/main.cpp ) -target_link_libraries(extrinsic_reflector_based_calibrator +target_link_libraries(extrinsic_marker_radar_lidar_calibrator ${OpenCV_LIBS} ) diff --git a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/__init__.py b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_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/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/__init__.py diff --git a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/calibrator_ui.py similarity index 97% rename from sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py rename to sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/calibrator_ui.py index cc76acf8..8c18bd3a 100644 --- a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_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/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py similarity index 99% rename from sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py rename to sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py index 82fb99a6..a5f75b92 100644 --- a/sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_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. diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp similarity index 84% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp index b063c4ba..8d169f37 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_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 EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ +#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ #include -#include -#include +#include +#include #include #include #include @@ -25,7 +25,9 @@ #include #include -#include +#include +#include +#include #include #include @@ -35,20 +37,18 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include +#include #include #include #include #include #include +namespace extrinsic_marker_radar_lidar_calibrator +{ + class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node { public: @@ -59,8 +59,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node protected: void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); void timerCallback(); @@ -121,7 +121,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node struct Parameters { - std::string parent_frame; + std::string radar_parallel_frame; bool use_lidar_initial_crop_box_filter; double lidar_initial_crop_box_min_x; double lidar_initial_crop_box_min_y; @@ -183,7 +183,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr lidar_sub_; rclcpp::Subscription::SharedPtr radar_sub_; - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr calibration_request_server_; rclcpp::Service::SharedPtr background_model_service_server_; rclcpp::Service::SharedPtr tracking_service_server_; @@ -198,22 +198,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_; @@ -225,11 +225,13 @@ 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_; }; -#endif // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__EXTRINSIC_REFLECTOR_BASED_CALIBRATOR_HPP_ +} // namespace extrinsic_marker_radar_lidar_calibrator + +#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/track.hpp similarity index 84% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/track.hpp index ee0f6349..18c66b01 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/track.hpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_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 EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ +#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ #include #include -#include +#include #include #include #include +namespace extrinsic_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 extrinsic_marker_radar_lidar_calibrator + +#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/types.hpp similarity index 78% rename from sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/types.hpp index e05c2fbf..b8d755ad 100644 --- a/sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/types.hpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_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 EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ #include @@ -25,11 +25,14 @@ #include #include +namespace extrinsic_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 extrinsic_marker_radar_lidar_calibrator + +#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ diff --git a/sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml similarity index 74% rename from sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml rename to sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml index b61fba19..cdec8468 100644 --- a/sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml @@ -1,7 +1,10 @@ - + + + + @@ -25,8 +28,10 @@ - - + + + + @@ -49,6 +54,10 @@ - + + + + + diff --git a/sensor/extrinsic_reflector_based_calibrator/package.xml b/sensor/extrinsic_marker_radar_lidar_calibrator/package.xml similarity index 89% rename from sensor/extrinsic_reflector_based_calibrator/package.xml rename to sensor/extrinsic_marker_radar_lidar_calibrator/package.xml index 8932c4fa..84e838f5 100644 --- a/sensor/extrinsic_reflector_based_calibrator/package.xml +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_reflector_based_calibrator + extrinsic_marker_radar_lidar_calibrator 0.0.1 - The extrinsic_reflector_based_calibrator package + The extrinsic_marker_radar_lidar_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz b/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz similarity index 92% rename from sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz rename to sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz index f90051b6..1d1360ff 100644 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz @@ -28,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: lidar Visualization Manager: Class: "" Displays: @@ -69,7 +69,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/front_lower/pointcloud_raw + Value: /pointcloud_topic Use Fixed Frame: true Use rainbow: true Value: true @@ -103,7 +103,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_background_pointcloud + Value: /lidar_background_pointcloud Use Fixed Frame: true Use rainbow: true Value: false @@ -137,7 +137,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_foreground_pointcloud + Value: /lidar_foreground_pointcloud Use Fixed Frame: true Use rainbow: true Value: false @@ -171,7 +171,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/lidar_colored_clusters + Value: /lidar_colored_clusters Use Fixed Frame: true Use rainbow: true Value: true @@ -185,7 +185,7 @@ Visualization Manager: 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: /lidar_detection_markers Value: true - BUS: Alpha: 0.9990000128746033 @@ -212,7 +212,10 @@ Visualization Manager: Color: 119; 11; 32 Name: DetectedObjects Namespaces: - {} + label: true + shape: true + twist: true + velocity: true PEDESTRIAN: Alpha: 0.9990000128746033 Color: 255; 192; 203 @@ -244,7 +247,7 @@ Visualization Manager: 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: /radar_detection_markers Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -258,7 +261,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,10 +279,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/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 @@ -290,7 +293,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_unit/front_unit_base_link/front_center/radar_link/tracking_markers + Value: /tracking_markers Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -325,7 +328,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: pandar_40p_front + Fixed Frame: radar_frame Frame Rate: 30 Name: root Tools: @@ -376,14 +379,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.2697972357273102 + Pitch: 0.3347971737384796 Position: - X: 0.19182920455932617 - Y: 5.7288312911987305 - Z: 3.2564549446105957 + X: -5.563827037811279 + Y: 0.02594861388206482 + Z: 3.217146873474121 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 4.681333541870117 + Yaw: 6.261345386505127 Saved: ~ Window Geometry: Displays: diff --git a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py b/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py similarity index 87% rename from sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py rename to sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py index 4360b43e..103f1c56 100755 --- a/sensor/extrinsic_reflector_based_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/extrinsic_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. @@ -18,8 +18,8 @@ import sys from PySide2.QtWidgets import QApplication -from extrinsic_reflector_based_calibrator import CalibratorUI -from extrinsic_reflector_based_calibrator import RosInterface +from extrinsic_marker_radar_lidar_calibrator import CalibratorUI +from extrinsic_marker_radar_lidar_calibrator import RosInterface import rclpy diff --git a/sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp b/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp similarity index 93% rename from sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp index 329e787e..103f6e65 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/extrinsic_reflector_based_calibrator.cpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_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,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include #include #include @@ -29,15 +30,10 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include +#include #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(parameters, #NAME, PARAM_STRUCT.NAME) @@ -59,6 +55,9 @@ void update_param( } } // namespace +namespace extrinsic_marker_radar_lidar_calibrator +{ + rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::paramCallback( const std::vector & parameters) { @@ -69,7 +68,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); @@ -118,19 +117,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("extrinsic_reflector_based_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); @@ -264,7 +255,7 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( std::bind(&ExtrinsicReflectorBasedCalibrator::paramCallback, this, std::placeholders::_1)); calibration_request_server_ = - this->create_service( + this->create_service( "extrinsic_calibration", std::bind( &ExtrinsicReflectorBasedCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -284,10 +275,10 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( } void ExtrinsicReflectorBasedCalibrator::requestReceivedCallback( - __attribute__((unused)) - const std::shared_ptr + [[maybe_unused]] const std::shared_ptr< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> request, - const std::shared_ptr response) + const std::shared_ptr response) { using std::chrono_literals::operator""s; @@ -305,12 +296,15 @@ 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; + tier4_calibration_msgs::msg::CalibrationResult result; + result.message.data = "Calibration successful"; + result.score = 0.f; + 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() @@ -336,8 +330,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; @@ -363,8 +357,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; @@ -392,8 +386,8 @@ void ExtrinsicReflectorBasedCalibrator::trackingRequestCallback( } 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; @@ -460,11 +454,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( @@ -517,7 +511,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) { @@ -801,7 +795,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); @@ -938,15 +932,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) { @@ -970,13 +963,13 @@ ExtrinsicReflectorBasedCalibrator::matchDetections( // Lidar transformed detections std::vector lidar_detections_transformed; - const auto radar_to_lidar_transorm = initial_radar_to_lidar_eigen_; + const auto radar_to_lidar_transform = initial_radar_to_lidar_eigen_; std::transform( lidar_detections.cbegin(), lidar_detections.cend(), std::back_inserter(lidar_detections_transformed), - [&radar_to_lidar_transorm](const auto & lidar_detection) { - auto transformed_point = radar_to_lidar_transorm * lidar_detection; + [&radar_to_lidar_transform](const auto & lidar_detection) { + auto transformed_point = radar_to_lidar_transform * lidar_detection; transformed_point.z() = 0.f; return transformed_point; }); @@ -1135,7 +1128,7 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) { auto track = converged_tracks_[track_index]; const auto & lidar_estimation = track.getLidarEstimation(); - const auto & lidar_estimation_pcs = parent_to_lidar_eigen_ * lidar_estimation; + const auto & lidar_estimation_pcs = radar_parallel_to_lidar_eigen_ * lidar_estimation; const auto & lidar_transformed_estimation = initial_radar_to_lidar_eigen_ * lidar_estimation; const auto & radar_estimation_rcs = track.getRadarEstimation(); lidar_points_pcs->emplace_back(eigen_to_pcl_2d(lidar_estimation_pcs)); @@ -1161,16 +1154,36 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() // 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 is 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(); @@ -1265,6 +1278,8 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() 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 && @@ -1275,6 +1290,8 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() "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(), @@ -1475,3 +1492,5 @@ void ExtrinsicReflectorBasedCalibrator::visualizationMarkers( tracking_markers_pub_->publish(tracking_marker_array); } + +} // namespace extrinsic_marker_radar_lidar_calibrator diff --git a/sensor/extrinsic_reflector_based_calibrator/src/main.cpp b/sensor/extrinsic_marker_radar_lidar_calibrator/src/main.cpp similarity index 70% rename from sensor/extrinsic_reflector_based_calibrator/src/main.cpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/src/main.cpp index 374ecae8..254cd810 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/main.cpp +++ b/sensor/extrinsic_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/track.cpp b/sensor/extrinsic_marker_radar_lidar_calibrator/src/track.cpp similarity index 95% rename from sensor/extrinsic_reflector_based_calibrator/src/track.cpp rename to sensor/extrinsic_marker_radar_lidar_calibrator/src/track.cpp index c331f524..c7cc8b7e 100644 --- a/sensor/extrinsic_reflector_based_calibrator/src/track.cpp +++ b/sensor/extrinsic_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,10 +13,13 @@ // limitations under the License. #include -#include -#include +#include +#include #include +namespace extrinsic_marker_radar_lidar_calibrator +{ + Track::Track( builtin_interfaces::msg::Time & t0, const KalmanFilter & initial_lidar_filter, const KalmanFilter & initial_radar_filter, double lidar_convergence_thresh, @@ -151,3 +154,5 @@ Track TrackFactory::makeTrack( t0, lidar_filter, radar_filter, lidar_convergence_thresh_, radar_convergence_thresh_, timeout_thresh_, max_matching_distance_); } + +} // namespace extrinsic_marker_radar_lidar_calibrator 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 054a899f..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_forground_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 7adf1073..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_forground_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 abe01b41..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_forground_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_left.rviz b/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz deleted file mode 100644 index d42fde54..00000000 --- a/sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_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/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_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_forground_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_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: /rear_unit/rear_unit_base_link/rear_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: /rear_unit/rear_unit_base_link/rear_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/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: - {} - 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: 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_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: /rear_unit/rear_unit_base_link/rear_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: /rear_unit/rear_unit_base_link/rear_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: 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.2947973608970642 - Position: - X: 4.067726135253906 - Y: 0.976508617401123 - Z: 2.6349170207977295 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 3.4413163661956787 - 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 8e9f180a..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_forground_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 8fc65e00..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_forground_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/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..a70c9abb --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py index 4d724c46..c70b97f4 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py @@ -1,9 +1,11 @@ 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 __all__ = [ "GroundPlaneCalibrator", "MappingBasedBaseLidarCalibrator", "MappingBasedLidarLidarCalibrator", + "MarkerRadarLidarCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py new file mode 100644 index 00000000..2eae86ce --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py @@ -0,0 +1,46 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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]) + + print("X2_MarkerRadarLidarCalibrator") + + self.add_calibrator( + service_name="calibrate_radar_lidar", + expected_calibration_frames=[ + FramePair(parent=self.radar_parallel_frame, child=self.radar_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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py index e1770557..47113fce 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.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. @@ -36,6 +36,11 @@ 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 from new_extrinsic_calibration_manager.calibration_manager_model import CalibratorManagerModel from new_extrinsic_calibration_manager.calibrator_base import CalibratorState from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry @@ -132,16 +137,18 @@ def on_selected_calibrator(self, project_name, calibrator_name): ) pass - def launch_calibrators(self, project_name: str, calibrator_name: str, argument_dict: Dict): + def launch_calibrators( + self, project_name: str, calibrator_name: str, launch_argument_dict: Dict + ): # Show the main UI self.show() # Execute the launcher - print(argument_dict, flush=True) - argument_list = [f"{k}:={v}" for k, v in argument_dict.items()] + print(launch_argument_dict, flush=True) + argument_list = [f"{k}:={v}" for k, v in launch_argument_dict.items()] package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") - path = ( + launcher_path = ( package_share_directory + "/launch/" + project_name @@ -149,7 +156,25 @@ def launch_calibrators(self, project_name: str, calibrator_name: str, argument_d + calibrator_name + ".launch.xml" ) - self.process = subprocess.Popen(["ros2", "launch", path] + argument_list) + self.process = subprocess.Popen(["ros2", "launch", launcher_path] + argument_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: + print("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) + + print(context.launch_configurations) # Start the ROS interface self.ros_interface = RosInterface() @@ -157,7 +182,10 @@ def launch_calibrators(self, project_name: str, calibrator_name: str, argument_d # Create the calibrator wrapper self.calibrator = CalibratorRegistry.create_calibrator( - project_name, calibrator_name, ros_interface=self.ros_interface, **argument_dict + 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) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py index c232f0db..53121c62 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/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. diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 18fb2177..55aca765 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -17,9 +17,9 @@ from functools import reduce from typing import Dict -import xml.dom.minidom 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 @@ -29,6 +29,9 @@ 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): @@ -59,7 +62,7 @@ def __init__(self, project_name, calibrator_name): self.arguments_widgets_dict = {} package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") - path = ( + launcher_path = ( package_share_directory + "/launch/" + project_name @@ -68,35 +71,38 @@ def __init__(self, project_name, calibrator_name): + ".launch.xml" ) - print(f"Reading xml from: {path}") + print(f"Reading xml from: {launcher_path}") try: - xml_doc = xml.dom.minidom.parse(path) + with open(launcher_path) as f: + root_entity, parser = Parser.load(f) except Exception as e: print("Failed reading xml file. Either not-existent or invalid") raise e - arg_nodes = [ - node - for node in xml_doc.getElementsByTagName("arg") - if node.parentNode == xml_doc.firstChild - ] + ld: LaunchDescription = parser.parse_description(root_entity) - for element in arg_nodes: - description = ( - element.getAttribute("description") if element.hasAttribute("description") else " " - ) - if element.hasAttribute("default"): - default_value = element.getAttribute("default").replace(" ", "") + for e in ld.entities: + if not isinstance(e, DeclareLaunchArgument): + continue + + description = e.description if e.description != "no description given" else "" - self.optional_arguments_dict[element.getAttribute("name")] = { + if 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[element.getAttribute("name")] = { + self.required_arguments_dict[e.name] = { "value": "", "description": description, + "choices": e.choices, } self.required_argument_layout.addWidget(QLabel("Name"), 0, 0) @@ -109,10 +115,21 @@ def __init__(self, project_name, calibrator_name): default_value = argument_data["value"].replace(" ", "") - self.arguments_widgets_dict[argument_name] = QLineEdit(default_value) - self.arguments_widgets_dict[argument_name].textChanged.connect( - self.check_argument_status - ) + 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) @@ -135,10 +152,21 @@ def __init__(self, project_name, calibrator_name): name_label = QLabel(argument_name) name_label.setMaximumWidth(400) - self.arguments_widgets_dict[argument_name] = QLineEdit(argument_data["value"]) - self.arguments_widgets_dict[argument_name].textChanged.connect( - self.check_argument_status - ) + 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) @@ -180,7 +208,10 @@ def check_argument_status(self, text=None): self.launch_button.setEnabled( reduce( lambda a, b: a and b, - [len(widget.text()) > 0 for widget in self.arguments_widgets_dict.values()], + [ + len(widget.text()) > 0 if hasattr(widget, "text") else widget.currentText() + for widget in self.arguments_widgets_dict.values() + ], ) ) print("check_argument_status", flush=True) @@ -188,6 +219,8 @@ def check_argument_status(self, text=None): 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() } diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py index a6fba7a7..d40cca51 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_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. From 285ec36fd7255d45f789e243cc799b5527f17683 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 12 Jan 2024 19:42:39 +0900 Subject: [PATCH 12/49] fix: fixed the camera-lidar calibrator that was broken due to some new parameters ant the new launcher configuration schmeme Signed-off-by: Kenzo Lobos-Tsunekawa --- .../launch/calibrator.launch.xml | 2 ++ .../launch/default_project/tag_based_pnp_calibrator.launch.xml | 2 ++ .../views/launcher_configuration_view.py | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml index dc46ada1..ed4f0430 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -5,6 +5,7 @@ + @@ -32,6 +33,7 @@ + diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml index b9c7cf3d..93cae250 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml @@ -6,6 +6,7 @@ + @@ -23,6 +24,7 @@ + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 55aca765..6b2e1c90 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -88,7 +88,7 @@ def __init__(self, project_name, calibrator_name): description = e.description if e.description != "no description given" else "" - if len(e.default_value) > 0: + 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 From 0b845c31d9d7430da047bc1f3965688e92a9d810 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 15 Jan 2024 08:46:09 +0900 Subject: [PATCH 13/49] feat: complete implementation for the radar-lidar, partial implementation for the rdv, and others Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 12 +- .../point_cloud_accumulator/CMakeLists.txt | 0 common/point_cloud_accumulator/COLCON_IGNORE | 0 .../point_cloud_accumulator.hpp | 0 .../launch/point_cloud_accumulator.launch.xml | 0 .../point_cloud_accumulator/package.xml | 0 .../src/point_cloud_accumulator.cpp | 0 .../extrinsic_manual_calibrator/COLCON_IGNORE | 0 .../COLCON_IGNORE | 0 .../launch/calibrator.launch.xml | 2 + .../ros_interface.py | 2 +- .../launch/calibrator.launch.xml | 2 +- .../rviz/default.rviz | 69 +- ...xtrinsic_marker_radar_lidar_calibrator.cpp | 26 +- .../tag_calib_camera7_pandar_40p_left.rviz | 1222 ----------------- .../brute_force_matcher.hpp | 5 +- .../calibration_estimator.hpp | 24 +- .../extrinsic_tag_based_pnp_calibrator.hpp | 8 +- .../launch/calibrator.launch.xml | 2 +- .../src/calibration_estimator.cpp | 109 +- .../extrinsic_tag_based_pnp_calibrator.cpp | 37 +- .../src/tag_calibrator_visualizer.cpp | 1 - .../base_lidar_calibration.launch.xml | 13 - ...ing_based_base_lidar_calibrator.launch.xml | 76 + ...ng_based_lidar_lidar_calibrator.launch.xml | 63 + .../marker_radar_lidar_calibrator.launch.xml | 31 + .../rdv/tag_based_pnp_calibrator.launch.xml | 70 + .../tier4_base_lidar_calibration.launch.xml | 13 - ...ing_based_base_lidar_calibrator.launch.xml | 1 - .../calibrator_base.py | 10 + .../calibrators/__init__.py | 3 +- .../calibrators/dummy_project/__init__.py | 11 - .../dummy_base_lidar_calibrator.py | 21 - .../dummy_camera_camera_calibrator.py | 29 - .../dummy_camera_lidar_calibrator.py | 29 - .../dummy_lidar_lidar_calibrator.py | 32 - .../calibrators/rdv/__init__.py | 11 + .../mapping_based_base_lidar_calibrator.py | 48 + .../mapping_based_lidar_lidar_calibrator.py | 79 ++ .../rdv/marker_radar_lidar_calibrator.py | 46 + .../rdv/tag_based_pnp_calibrator.py | 65 + .../tier4_dummy_project/__init__.py | 3 - .../tier4_dummy_base_lidar_calibrator.py | 39 - .../x2/marker_radar_lidar_calibrator.py | 2 +- .../calibrators/xx1_15/__init__.py | 6 - .../xx1_15/dummy_base_lidar_calibrator.py | 21 - .../xx1_15/dummy_camera_camera_calibrator.py | 29 - .../xx1_15/dummy_lidar_lidar_calibrator.py | 32 - .../ros_interface.py | 6 +- 49 files changed, 644 insertions(+), 1666 deletions(-) rename {sensor => common}/point_cloud_accumulator/CMakeLists.txt (100%) create mode 100644 common/point_cloud_accumulator/COLCON_IGNORE rename {sensor => common}/point_cloud_accumulator/include/point_cloud_accumulator/point_cloud_accumulator.hpp (100%) rename {sensor => common}/point_cloud_accumulator/launch/point_cloud_accumulator.launch.xml (100%) rename {sensor => common}/point_cloud_accumulator/package.xml (100%) rename {sensor => common}/point_cloud_accumulator/src/point_cloud_accumulator.cpp (100%) create mode 100644 sensor/extrinsic_manual_calibrator/COLCON_IGNORE create mode 100644 sensor/extrinsic_map_based_calibrator/COLCON_IGNORE delete mode 100644 sensor/extrinsic_tag_based_calibrator/rviz/tag_calib_camera7_pandar_40p_left.rviz delete mode 100644 sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml delete mode 100644 sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py delete mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py diff --git a/.cspell.json b/.cspell.json index a5c5c7b6..9321f6aa 100644 --- a/.cspell.json +++ b/.cspell.json @@ -13,6 +13,7 @@ "discretization", "distro", "eigen", + "figsize", "gicp", "homography", "hsize", @@ -23,6 +24,7 @@ "lidars", "lidartag", "lidartags", + "matplotlib", "matx", "ncols", "nrows", @@ -32,12 +34,14 @@ "pointclouds", "prerejective", "pydot", + "pyplot", "quaterniond", "ransac", "rclcpp", "rclpy", "registrator", "registrators", + "remappings", "representer", "reprojection", "rosbag", @@ -54,6 +58,12 @@ "undistortion", "uniformingly", "vectord", - "voxel" + "voxel", + "xaxis", + "xlabel", + "xlim", + "yaxis", + "ylabel", + "ylim" ] } 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/common/point_cloud_accumulator/COLCON_IGNORE b/common/point_cloud_accumulator/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b 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/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/launch/calibrator.launch.xml b/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml index 57dee3df..feef894e 100644 --- a/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml @@ -21,6 +21,7 @@ + @@ -86,6 +87,7 @@ + diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py index e3cdf246..57982519 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py @@ -59,7 +59,7 @@ def __call__(self): class RosInterface(Node): def __init__(self): - super().__init__("extrinsic_reflector_based_calibrator_ui") + super().__init__("extrinsic_marker_radar_lidar_calibrator_ui") self.ros_context = None self.ros_executor = None diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml index 0070a228..30f050d2 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml @@ -55,7 +55,7 @@ - + diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz b/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz index 1d1360ff..da6a00ed 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz +++ b/sensor/extrinsic_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 @@ -179,7 +178,7 @@ Visualization Manager: Enabled: true Name: lidar_detections Namespaces: - {} + "": true Topic: Depth: 5 Durability Policy: Volatile @@ -187,61 +186,12 @@ Visualization Manager: Reliability Policy: Reliable 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: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - label: true - shape: true - twist: true - velocity: true - 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: - {} + center: true + line: true Topic: Depth: 5 Durability Policy: Volatile @@ -287,7 +237,8 @@ Visualization Manager: Enabled: true Name: tracking_markers Namespaces: - {} + calibrated: true + initial: true Topic: Depth: 5 Durability Policy: Volatile @@ -379,14 +330,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3347971737384796 + Pitch: 0.23479722440242767 Position: - X: -5.563827037811279 - Y: 0.02594861388206482 - Z: 3.217146873474121 + X: -6.539778709411621 + Y: 0.01612010970711708 + Z: 3.799168348312378 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 6.261345386505127 + Yaw: 0.0031585693359375 Saved: ~ Window Geometry: Displays: diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp b/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp index 26f84884..9490c45f 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(parameters, #NAME, PARAM_STRUCT.NAME) @@ -49,7 +50,7 @@ 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("extrinsic_marker_radar_lidar_calibrator"), "Setting parameter [" << name << "] to " << value); } } @@ -117,7 +118,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( const rclcpp::NodeOptions & options) -: Node("extrinsic_reflector_based_calibrator_node", options), tf_broadcaster_(this) +: Node("extrinsic_marker_radar_lidar_calibrator_node", options), tf_broadcaster_(this) { tf_buffer_ = std::make_shared(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); @@ -301,9 +302,13 @@ void ExtrinsicReflectorBasedCalibrator::requestReceivedCallback( std::unique_lock lock(mutex_); + 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 = "Calibration successful"; - result.score = 0.f; + 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_; @@ -1013,6 +1018,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()); @@ -1754,7 +1770,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()); 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_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp index 8f157316..ff9384f9 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp @@ -26,8 +26,9 @@ 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( diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp index 68f1d70a..93befcce 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include @@ -44,8 +45,7 @@ class CalibrationEstimator 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_points, + std::tuple, std::vector> getCalibrationPoints( bool use_estimated); bool calibrate(); @@ -64,10 +64,10 @@ 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 setCrossvalidationTrainingRatio(double ratio); @@ -93,19 +93,17 @@ class CalibrationEstimator double getCalibrationCoveragePercentage() const; int getCurrentCalibrationPairsNumber() 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; diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp index 835fccc7..057869e9 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp @@ -21,6 +21,7 @@ #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 @@ -125,6 +120,7 @@ class ExtrinsicTagBasedPNPCalibrator : public rclcpp::Node rclcpp::Service::SharedPtr service_server_; // Threading, sync, and result + bool request_received_; std::mutex mutex_; // Rviz visualizations diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml index ed4f0430..e4d71be1 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -38,7 +38,7 @@ - + diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp index ce24ac59..9dbaceb1 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -17,16 +17,11 @@ #include #include #include +#include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include @@ -264,9 +259,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; @@ -279,17 +273,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 = @@ -314,12 +309,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; @@ -423,55 +422,59 @@ 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; + 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(); @@ -484,14 +487,14 @@ bool CalibrationEstimator::calibrate( cv::SOLVEPNP_SQPNP); if (!success) { - RCLCPP_ERROR(rclcpp::get_logger("tier4_tag_utils"), "PNP failed"); - return false; + RCLCPP_ERROR(rclcpp::get_logger("calibration_estimator"), "PNP failed"); + return std::tuple(false, translation_vector, rotation_matrix); } translation_vector = tvec; cv::Rodrigues(rvec, rotation_matrix); - return true; + return std::tuple(true, translation_vector, rotation_matrix); } tf2::Transform CalibrationEstimator::toTf2( @@ -515,7 +518,7 @@ void CalibrationEstimator::computeCrossValidationReprojectionError( // Iterate a number of times // 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); @@ -546,12 +549,10 @@ void CalibrationEstimator::computeCrossValidationReprojectionError( } } - 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); @@ -625,28 +626,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_; + return std::tuple( + hypothesis_translation_vector_, hypothesis_rotation_matrix_); } void CalibrationEstimator::setCrossvalidationTrainingRatio(double ratio) @@ -767,3 +766,5 @@ double CalibrationEstimator::getCrossValidationReprojectionError() const { return crossvalidation_reprojection_error_; } + +int CalibrationEstimator::getConvergencePairNumber() const { return convergence_min_pairs_; } diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp index af905af0..9c75536a 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp @@ -32,6 +32,7 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::NodeOptions & options) : Node("extrinsic_tag_based_pnp_calibrator_node", options), tf_broadcaster_(this), + request_received_(false), got_initial_transform(false) { tf_buffer_ = std::make_shared(this->get_clock()); @@ -279,24 +280,30 @@ void ExtrinsicTagBasedPNPCalibrator::cameraInfoCallback( } void ExtrinsicTagBasedPNPCalibrator::requestReceivedCallback( - const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + 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; } } - tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPose(); + tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPoseAsTF(); geometry_msgs::msg::Transform transform_msg; transform_msg = tf2::toMsg(optical_axis_to_lidar_tf2); @@ -356,7 +363,7 @@ void ExtrinsicTagBasedPNPCalibrator::tfTimerCallback() return; } - tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPose(); + tf2::Transform optical_axis_to_lidar_tf2 = estimator_.getFilteredPoseAsTF(); geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped.header.stamp = header_.stamp; @@ -370,7 +377,7 @@ 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; } @@ -378,11 +385,11 @@ void ExtrinsicTagBasedPNPCalibrator::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_rot_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_rot_matrix] = estimator_.getCurrentPose(); + auto [filtered_trans_vector, filtered_rot_matrix] = estimator_.getFilteredPose(); Eigen::Isometry3d initial_transform_eigen = tf2::transformToEigen(tf2::toMsg(initial_optical_axis_to_lidar_tf2_)); @@ -407,10 +414,7 @@ void ExtrinsicTagBasedPNPCalibrator::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"); @@ -444,7 +448,10 @@ void ExtrinsicTagBasedPNPCalibrator::automaticCalibrationTimerCallback() double filtered_reprojection_error = reprojection_error(image_points, filtered_projected_points); - RCLCPP_INFO(this->get_logger(), "Partial calibration results:"); + 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( diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index 26df7327..d0199824 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -393,7 +393,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; diff --git a/sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml deleted file mode 100644 index b134a0d1..00000000 --- a/sensor/new_extrinsic_calibration_manager/launch/dummy_project/base_lidar_calibration.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..0b4f2b99 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..14d69a1e --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml new file mode 100644 index 00000000..40f4f7b9 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..823dd8a4 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml deleted file mode 100644 index b134a0d1..00000000 --- a/sensor/new_extrinsic_calibration_manager/launch/tier4_dummy_project/tier4_base_lidar_calibration.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml index 52bb4af3..293042ba 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml @@ -2,7 +2,6 @@ - diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py index c6c95f87..5ba2e19a 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py @@ -146,6 +146,11 @@ def pre_process(self): pass def post_process_internal(self): + print("Before post_process") + for parent, children_and_transforms in self.calibration_result_tfs.items(): + for child, transform in children_and_transforms.items(): + print(f"{parent}->{child}:\n{transform}") + calibration_transforms = { parent: { child: tf_message_to_transform_matrix(transform) @@ -162,6 +167,11 @@ def post_process_internal(self): for parent, children_and_transforms in calibration_transforms.items() } + print("After post_process") + for parent, children_and_transforms in self.processed_calibration_result_tfs.items(): + for child, transform in children_and_transforms.items(): + print(f"{parent}->{child}:\n{transform}") + def post_process(self, calibration_transforms) -> Dict[str, Dict[str, np.array]]: return calibration_transforms diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py index 81c5dd00..0d7af762 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py @@ -1,6 +1,5 @@ from .default_project import * # noqa: F401, F403 -from .dummy_project import * # noqa: F401, F403 -from .tier4_dummy_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 diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py deleted file mode 100644 index c790a4e7..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/__init__.py +++ /dev/null @@ -1,11 +0,0 @@ -from .dummy_base_lidar_calibrator import DummyBaseLidarCalibrator -from .dummy_camera_camera_calibrator import DummyCameraCameraCalibrator -from .dummy_camera_lidar_calibrator import DummyCameraLidarCalibrator -from .dummy_lidar_lidar_calibrator import DummyLidarLidarCalibrator - -__all__ = [ - "DummyBaseLidarCalibrator", - "DummyCameraCameraCalibrator", - "DummyCameraLidarCalibrator", - "DummyLidarLidarCalibrator", -] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py deleted file mode 100644 index 9591f87c..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_base_lidar_calibrator.py +++ /dev/null @@ -1,21 +0,0 @@ -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair - - -@CalibratorRegistry.register_calibrator( - project_name="dummy_project", calibrator_name="base_lidar_calibration" -) -class DummyBaseLidarCalibrator(CalibratorBase): - required_frames = ["base_link, sensor_kit_base_link", "velodyne_top_base_link", "velodyne_top"] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("DummyBaseLidarCalibrator") - - self.add_calibrator( - service_name="calibrate_base_lidar", - expected_calibration_frames=[FramePair(parent="base_link", child="velodyne_top")], - ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py deleted file mode 100644 index c7d89451..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_camera_calibrator.py +++ /dev/null @@ -1,29 +0,0 @@ -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair - - -@CalibratorRegistry.register_calibrator( - project_name="dummy_project", calibrator_name="camera_camera_calibration" -) -class DummyCameraCameraCalibrator(CalibratorBase): - required_frames = [ - "sensor_kit_base_link", - "camera0/camera_link", - "camera0/camera_optical_link", - "camera1/camera_optical_link", - "camera1/camera_link", - ] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("DummyCameraCameraCalibrator") - - self.add_calibrator( - service_name="calibrate_camera_camera", - expected_calibration_frames=[ - FramePair(parent="camera0/camera_link", child="camera1/camera_link"), - ], - ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py deleted file mode 100644 index 631e88d9..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_camera_lidar_calibrator.py +++ /dev/null @@ -1,29 +0,0 @@ -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair - - -@CalibratorRegistry.register_calibrator( - project_name="dummy_project", calibrator_name="camera_lidar_calibration" -) -class DummyCameraLidarCalibrator(CalibratorBase): - required_frames = [ - "sensor_kit_base_link", - "camera0/camera_link", - "camera0/camera_optical_link", - "velodyne_top_base_link", - "velodyne_top", - ] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("DummyCameraLidarCalibrator") - - self.add_calibrator( - service_name="calibrate_camera_lidar", - expected_calibration_frames=[ - FramePair(parent="velodyne_top", child="camera0/camera_link"), - ], - ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py deleted file mode 100644 index f2ad035d..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/dummy_project/dummy_lidar_lidar_calibrator.py +++ /dev/null @@ -1,32 +0,0 @@ -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair - - -@CalibratorRegistry.register_calibrator( - project_name="dummy_project", calibrator_name="lidar_lidar_calibration" -) -class DummyLidarLidarCalibrator(CalibratorBase): - required_frames = [ - "sensor_kit_base_link", - "velodyne_top_base_link", - "velodyne_top", - "velodyne_left_base_link", - "velodyne_left", - "velodyne_right_base_link", - "velodyne_right", - ] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("DummyLidarLidarCalibrator") - - self.add_calibrator( - service_name="calibrate_camera_lidar", - expected_calibration_frames=[ - FramePair(parent="velodyne_top", child="velodyne_left"), - FramePair(parent="velodyne_top", child="velodyne_right"), - ], - ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py new file mode 100644 index 00000000..2ac5e49f --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py @@ -0,0 +1,11 @@ +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 + +__all__ = [ + "MappingBasedBaseLidarCalibrator", + "MappingBasedLidarLidarCalibrator", + "MarkerRadarLidarCalibrator", + "TagBasedPNPCalibrator", +] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py new file mode 100644 index 00000000..a9348dbe --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py @@ -0,0 +1,48 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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] + ) + + print("RDV_MappingBasedBaseLidarCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py new file mode 100644 index 00000000..fc8949d8 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py @@ -0,0 +1,79 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("RDV_MappingBasedLidarLidarCalibrator") + + 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]]): + print(f"post_process\n{calibration_transforms}") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py new file mode 100644 index 00000000..954c1aee --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py @@ -0,0 +1,46 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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]) + + print("RDV_MarkerRadarLidarCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..cb90812a --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py @@ -0,0 +1,65 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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") + + print("RDV::TagBasedPNPCalibrator") + print(self.camera_name, flush=True) + + 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]]): + camera_to_lidar_transform = calibration_transforms[ + f"{self.camera_name}/camera_optical_link" + ]["pandar_top"] + + print(f"camera_to_lidar_transform={camera_to_lidar_transform}", flush=True) + + sensor_kit_to_lidar_transform = self.get_transform_matrix( + "sensor_kit_base_link", "pandar_top" + ) + + print(f"sensor_kit_to_lidar_transform={sensor_kit_to_lidar_transform}", flush=True) + + camera_to_optical_link_transform = self.get_transform_matrix( + f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link" + ) + + print(f"camera_to_optical_link_transform={camera_to_optical_link_transform}", flush=True) + + sensor_kit_camera_link_transform = np.linalg.inv( + camera_to_optical_link_transform + @ camera_to_lidar_transform + @ np.linalg.inv(sensor_kit_to_lidar_transform) + ) + + print(f"sensor_kit_camera_link_transform={sensor_kit_camera_link_transform}", flush=True) + + result = { + "sensor_kit_base_link": { + f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform + } + } + return result diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py deleted file mode 100644 index 8fb1a0e9..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from .tier4_dummy_base_lidar_calibrator import DummyBaseLidarCalibrator - -__all__ = ["DummyBaseLidarCalibrator"] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py deleted file mode 100644 index 8bc57672..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/tier4_dummy_project/tier4_dummy_base_lidar_calibrator.py +++ /dev/null @@ -1,39 +0,0 @@ -from collections import defaultdict -from typing import Dict - -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair -import numpy as np - - -@CalibratorRegistry.register_calibrator( - project_name="tier4_dummy_project", calibrator_name="tier4_base_lidar_calibration" -) -class DummyBaseLidarCalibrator(CalibratorBase): - required_frames = ["base_link", "sensor_kit_base_link", "velodyne_top"] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("Tier4DummyBaseLidarCalibrator") - - self.add_calibrator( - service_name="calibrate_base_lidar", - expected_calibration_frames=[FramePair(parent="base_link", child="velodyne_top")], - ) - - def post_process(self, calibration_transforms) -> Dict[str, Dict[str, np.array]]: - sensor_kit_to_lidar = self.get_transform_matrix( - parent="sensor_kit_base_link", child="velodyne_top" - ) - - base_link_to_sensor_kit = calibration_transforms["base_link"][ - "velodyne_top" - ] @ np.linalg.inv(sensor_kit_to_lidar) - - output_transforms = defaultdict(lambda: defaultdict(np.array)) - output_transforms["base_link"]["sensor_kit_base_link"] = base_link_to_sensor_kit - - return output_transforms diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py index 2eae86ce..8d9fad8b 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py @@ -27,7 +27,7 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.add_calibrator( service_name="calibrate_radar_lidar", expected_calibration_frames=[ - FramePair(parent=self.radar_parallel_frame, child=self.radar_frame) + FramePair(parent=self.radar_frame, child=self.lidar_frame) ], ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py index 53212ebe..9a32d340 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py @@ -1,11 +1,5 @@ -from .dummy_base_lidar_calibrator import DummyBaseLidarCalibrator -from .dummy_camera_camera_calibrator import DummyCameraCameraCalibrator -from .dummy_lidar_lidar_calibrator import DummyLidarLidarCalibrator from .tag_based_pnp_calibrator import TagBasedPNPCalibrator __all__ = [ - "DummyBaseLidarCalibrator", - "DummyCameraCameraCalibrator", "TagBasedPNPCalibrator", - "DummyLidarLidarCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py deleted file mode 100644 index 9591f87c..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_base_lidar_calibrator.py +++ /dev/null @@ -1,21 +0,0 @@ -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair - - -@CalibratorRegistry.register_calibrator( - project_name="dummy_project", calibrator_name="base_lidar_calibration" -) -class DummyBaseLidarCalibrator(CalibratorBase): - required_frames = ["base_link, sensor_kit_base_link", "velodyne_top_base_link", "velodyne_top"] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("DummyBaseLidarCalibrator") - - self.add_calibrator( - service_name="calibrate_base_lidar", - expected_calibration_frames=[FramePair(parent="base_link", child="velodyne_top")], - ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py deleted file mode 100644 index c7d89451..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_camera_camera_calibrator.py +++ /dev/null @@ -1,29 +0,0 @@ -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair - - -@CalibratorRegistry.register_calibrator( - project_name="dummy_project", calibrator_name="camera_camera_calibration" -) -class DummyCameraCameraCalibrator(CalibratorBase): - required_frames = [ - "sensor_kit_base_link", - "camera0/camera_link", - "camera0/camera_optical_link", - "camera1/camera_optical_link", - "camera1/camera_link", - ] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("DummyCameraCameraCalibrator") - - self.add_calibrator( - service_name="calibrate_camera_camera", - expected_calibration_frames=[ - FramePair(parent="camera0/camera_link", child="camera1/camera_link"), - ], - ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py deleted file mode 100644 index f2ad035d..00000000 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/dummy_lidar_lidar_calibrator.py +++ /dev/null @@ -1,32 +0,0 @@ -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair - - -@CalibratorRegistry.register_calibrator( - project_name="dummy_project", calibrator_name="lidar_lidar_calibration" -) -class DummyLidarLidarCalibrator(CalibratorBase): - required_frames = [ - "sensor_kit_base_link", - "velodyne_top_base_link", - "velodyne_top", - "velodyne_left_base_link", - "velodyne_left", - "velodyne_right_base_link", - "velodyne_right", - ] - - def __init__(self, ros_interface: RosInterface): - super().__init__(ros_interface) - - print("DummyLidarLidarCalibrator") - - self.add_calibrator( - service_name="calibrate_camera_lidar", - expected_calibration_frames=[ - FramePair(parent="velodyne_top", child="velodyne_left"), - FramePair(parent="velodyne_top", child="velodyne_right"), - ], - ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py index 355c733f..a6e31dc0 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py @@ -43,9 +43,6 @@ def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - self.buf = Buffer(node=self) - self.listener = TransformListener(self.buf, self, spin_thread=False) - self.tf_qos_profile = rclpy.qos.QoSProfile( reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, @@ -129,6 +126,9 @@ def call_calibration_service(self, service_name): 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) From 561cb79005b3380f56c36c07ec8c709aa2828b7e Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 15 Jan 2024 09:44:57 +0900 Subject: [PATCH 14/49] chore: attempting to fix CI/CD Signed-off-by: Kenzo Lobos-Tsunekawa --- sensor/new_extrinsic_calibration_manager/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensor/new_extrinsic_calibration_manager/package.xml b/sensor/new_extrinsic_calibration_manager/package.xml index cec81f47..11dabe9e 100644 --- a/sensor/new_extrinsic_calibration_manager/package.xml +++ b/sensor/new_extrinsic_calibration_manager/package.xml @@ -12,9 +12,9 @@ ros2_numpy ros2launch tier4_calibration_msgs - ament_copyright + ament_python From 8a0c6b36ec8a0241c71e813270245eb595787294 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 16 Jan 2024 17:49:43 +0900 Subject: [PATCH 15/49] feat: implemented the new api changes for the sfm calibrator and fixed bugs got discovered while doing do. Need to integrate all the options and the products Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 6 + ...extrinsic_lidar_to_lidar_2d_calibrator.cpp | 1 + .../launch/calibrator.launch.xml | 191 ---- .../rviz/x2.rviz | 894 ------------------ .../CMakeLists.txt | 9 +- .../omiya_calibration_room_2023.param.yaml | 87 ++ .../__init__.py | 0 .../calibrator_ui.py | 0 .../ros_interface.py | 2 +- .../apriltag_detection.hpp | 15 +- .../apriltag_detector.hpp | 20 +- .../calibration_scene_extractor.hpp | 33 +- .../calibration_types.hpp | 19 +- .../ceres/calibration_problem.hpp | 19 +- .../ceres/camera_residual.hpp | 23 +- .../ceres/lidar_residual.hpp | 20 +- .../ceres/sensor_residual.hpp | 19 +- .../extrinsic_tag_based_sfm_calibrator.hpp} | 66 +- .../apriltag_calibrator.hpp | 20 +- .../chessboard_calibrator.hpp | 18 +- .../intrinsics_calibrator.hpp | 18 +- .../math.hpp | 18 +- .../scene_types.hpp | 16 +- .../serialization.hpp | 34 +- .../types.hpp | 16 +- .../visualization.hpp | 20 +- .../launch/apriltag_detector.launch.py | 0 .../launch/calibrator.launch.xml | 240 +++++ .../launch/lidartag_detector.launch.xml | 31 + .../package.xml | 4 +- .../rviz/default.rviz} | 472 +++++---- .../scripts/calibrator_ui_node.py | 4 +- .../src/apriltag_detection.cpp | 40 +- .../src/apriltag_detector.cpp | 41 +- .../src/calibration_scene_extractor.cpp | 53 +- .../src/ceres/calibration_problem.cpp | 24 +- .../extrinsic_tag_based_sfm_calibrator.cpp} | 433 ++++----- .../apriltag_calibrator.cpp | 8 +- .../chessboard_calibrator.cpp | 8 +- .../intrinsics_calibrator.cpp | 8 +- .../src/main.cpp | 8 +- .../src/math.cpp | 28 +- .../src/visualization.cpp | 10 +- ...based_sfm_base_lidar_calibrator.launch.xml | 46 + .../marker_radar_lidar_calibrator.launch.xml | 6 +- .../calibrators/rdv/__init__.py | 2 + .../tag_based_sfm_base_lidar_calibrator.py | 48 + .../new_extrinsic_calibration_manager.py | 9 +- .../views/launcher_configuration_view.py | 5 +- .../views/tf_view.py | 13 +- 50 files changed, 1255 insertions(+), 1870 deletions(-) delete mode 100644 sensor/extrinsic_tag_based_base_calibrator/launch/calibrator.launch.xml delete mode 100644 sensor/extrinsic_tag_based_base_calibrator/rviz/x2.rviz rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/CMakeLists.txt (85%) create mode 100644 sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml rename sensor/{extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator}/__init__.py (100%) rename sensor/{extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator}/calibrator_ui.py (100%) rename sensor/{extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator}/ros_interface.py (99%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/apriltag_detection.hpp (87%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/apriltag_detector.hpp (78%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/calibration_scene_extractor.hpp (71%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/calibration_types.hpp (84%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/ceres/calibration_problem.hpp (93%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/ceres/camera_residual.hpp (97%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/ceres/lidar_residual.hpp (94%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/ceres/sensor_residual.hpp (81%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator.hpp => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp} (86%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/intrinsics_calibration/apriltag_calibrator.hpp (66%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/intrinsics_calibration/chessboard_calibrator.hpp (65%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/intrinsics_calibration/intrinsics_calibrator.hpp (74%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/math.hpp (88%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/scene_types.hpp (78%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/serialization.hpp (84%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/types.hpp (93%) rename sensor/{extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator}/visualization.hpp (89%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/launch/apriltag_detector.launch.py (100%) create mode 100644 sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml create mode 100644 sensor/extrinsic_tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/package.xml (91%) rename sensor/{extrinsic_tag_based_base_calibrator/rviz/xx1.rviz => extrinsic_tag_based_sfm_calibrator/rviz/default.rviz} (58%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/scripts/calibrator_ui_node.py (90%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/apriltag_detection.cpp (92%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/apriltag_detector.cpp (89%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/calibration_scene_extractor.cpp (79%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/ceres/calibration_problem.cpp (98%) rename sensor/{extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp => extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp} (82%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/intrinsics_calibration/apriltag_calibrator.cpp (95%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/intrinsics_calibration/chessboard_calibrator.cpp (94%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/intrinsics_calibration/intrinsics_calibrator.cpp (93%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/main.cpp (73%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/math.cpp (95%) rename sensor/{extrinsic_tag_based_base_calibrator => extrinsic_tag_based_sfm_calibrator}/src/visualization.cpp (97%) create mode 100644 sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py diff --git a/.cspell.json b/.cspell.json index 9321f6aa..3db440fb 100644 --- a/.cspell.json +++ b/.cspell.json @@ -13,12 +13,16 @@ "discretization", "distro", "eigen", + "extrinsics", "figsize", "gicp", "homography", "hsize", "icp", "idless", + "imread", + "imwrite", + "intrinsics", "kalman", "keyframes", "lidars", @@ -26,6 +30,7 @@ "lidartags", "matplotlib", "matx", + "misdetection", "ncols", "nrows", "permutate", @@ -55,6 +60,7 @@ "subsampled", "tvec", "tvecs", + "undistort", "undistortion", "uniformingly", "vectord", diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp index 4c00c1f7..f47d2b2f 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp +++ b/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp @@ -29,6 +29,7 @@ #include #include +#include namespace extrinsic_lidar_to_lidar_2d_calibrator { 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 9d7b4332..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/CMakeLists.txt b/sensor/extrinsic_tag_based_sfm_calibrator/CMakeLists.txt similarity index 85% rename from sensor/extrinsic_tag_based_base_calibrator/CMakeLists.txt rename to sensor/extrinsic_tag_based_sfm_calibrator/CMakeLists.txt index 56be9b6f..2a070721 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/CMakeLists.txt +++ b/sensor/extrinsic_tag_based_sfm_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_tag_based_base_calibrator) +project(extrinsic_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(extrinsic_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/extrinsic_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(extrinsic_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/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml b/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml new file mode 100644 index 00000000..8fa1cac2 --- /dev/null +++ b/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml @@ -0,0 +1,87 @@ +/**: + ros__parameters: + + # Tag configuration + lidartag_to_apriltag_scale: 0.75 + + 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 + fixed_ground_plane_model_a: -0.015014 + fixed_ground_plane_model_b: 0.028632 + fixed_ground_plane_model_c: 0.999477 + fixed_ground_plane_model_d: 2.849738 + + + # 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 + debug: false + refine_edges: true diff --git a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/__init__.py b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_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/extrinsic_tag_based_sfm_calibrator/extrinsic_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/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py similarity index 100% rename from sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/calibrator_ui.py rename to sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py diff --git a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/ros_interface.py b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py similarity index 99% rename from sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/ros_interface.py rename to sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py index 158ac000..0cdd2340 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator/ros_interface.py +++ b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py @@ -88,7 +88,7 @@ def __call__(self, files_list): class RosInterface(Node): def __init__(self): - super().__init__("extrinsic_tag_based_base_calibrator") + super().__init__("extrinsic_tag_based_sfm_calibrator") self.ros_context = None self.ros_executor = None diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detection.hpp similarity index 87% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detection.hpp index 9229e637..4f171030 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detection.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,10 +12,10 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ -#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 = ""; @@ -99,6 +100,6 @@ using ApriltagGridDetections = std::vector; using GroupedApriltagDetections = std::map; using GroupedApriltagGridDetections = std::map; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTION_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detector.hpp index 3b079ab5..28f9a2c0 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/apriltag_detector.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,11 +12,11 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTOR_HPP_ +#define EXTRINSIC_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 extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__APRILTAG_DETECTOR_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_scene_extractor.hpp index dd3ae504..04b94740 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_scene_extractor.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ +#define EXTRINSIC_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 extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_types.hpp similarity index 84% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_types.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_types.hpp index 4e7079a2..cae6dbad 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/calibration_types.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,11 +12,11 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ -#include -#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CALIBRATION_TYPES_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp similarity index 93% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/calibration_problem.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp index 31f41e4b..e65c2f4e 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/calibration_problem.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ +#define EXTRINSIC_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 extrinsic_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; @@ -252,6 +253,6 @@ class CalibrationProblem std::array shared_intrinsics_opt; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/camera_residual.hpp similarity index 97% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/camera_residual.hpp index 25271db4..76da5d61 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/camera_residual.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,12 +12,12 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { struct CameraResidual : public SensorResidual @@ -36,7 +36,8 @@ 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, + const std::array & + fixed_tag_rot_z, // cSpell:ignore SHRD bool fix_camera_pose, bool fix_tag_rot_z, bool optimize_intrinsics, bool is_ground_tag) : camera_uid_(camera_uid), intrinsics_(intrinsics), @@ -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 @@ -494,6 +495,6 @@ struct CameraResidual : public SensorResidual bool is_ground_tag_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/lidar_residual.hpp similarity index 94% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/lidar_residual.hpp index 67cacf74..41e34381 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/lidar_residual.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,12 +12,12 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { struct LidarResidual : public SensorResidual @@ -127,7 +127,7 @@ struct LidarResidual : public SensorResidual Vector3 corners_wcs[NUM_CORNERS]; Vector3 corners_lcs[NUM_CORNERS]; - Vector3 corners_lrcs[NUM_CORNERS]; + Vector3 corners_lrcs[NUM_CORNERS]; // cSpell:ignore lrcs auto transform_corners = [](auto & quaternion, auto & translation, auto & input_corners, auto & output_corners) { @@ -289,6 +289,6 @@ struct LidarResidual : public SensorResidual Eigen::Matrix3d tag_centric_rotation_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/sensor_residual.hpp similarity index 81% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/sensor_residual.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/sensor_residual.hpp index 0c15b9a2..b955acf7 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/ceres/sensor_residual.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ -#include -#include +#include +#include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ +#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp similarity index 86% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp index 3748bc8c..c6ab0815 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/extrinsic_tag_based_base_calibrator.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__EXTRINSIC_TAG_BASED_SFM_CALIBRATOR_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__EXTRINSIC_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 #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 extrinsic_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 request, + const std::shared_ptr response); /*! * Callback method for the image of the calibration cameras @@ -118,6 +109,11 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node */ std_msgs::msg::ColorRGBA getNextColor(); + /*! + * @return the UID corresponding to the main sensor + */ + UID getMainSensorUID() const; + /*! * Attempts to add external camera images to the scene * @param request A vector of files to be added as external images @@ -138,7 +134,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 +156,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 +177,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 @@ -229,10 +225,9 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr 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 +235,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,9 +253,6 @@ 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_; std::mutex mutex_; tf2_ros::StaticTransformBroadcaster tf_broadcaster_; @@ -349,6 +341,6 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node std::vector precomputed_colors_; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__EXTRINSIC_TAG_BASED_BASE_CALIBRATOR_HPP_ +#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__EXTRINSIC_TAG_BASED_SFM_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/apriltag_calibrator.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp index 0aaa47ad..549d489e 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/apriltag_calibrator.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,13 +12,13 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp index 6707213a..ea0800b7 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/chessboard_calibrator.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ +#define EXTRINSIC_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 extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp index e73e580a..0d916b43 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ -#include -#include +#include +#include #include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp similarity index 88% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/math.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp index af1f5968..967f78c7 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/math.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,19 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ #include -#include -#include -#include +#include +#include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { /* @@ -101,6 +101,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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/scene_types.hpp index 45d381a6..72db10ae 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/scene_types.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ -#include -#include +#include +#include #include #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { struct ExternalCameraFrame @@ -58,6 +58,6 @@ struct CalibrationScene std::vector external_camera_frames; }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__SCENE_TYPES_HPP_ +#endif // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp similarity index 84% rename from sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/serialization.hpp rename to sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp index 0972a4ab..b004c9e9 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/serialization.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,12 +12,12 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -152,7 +152,7 @@ void serialize(Archive & ar, cv::Affine3<_Tp> & pose, const unsigned int version template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::ApriltagDetection & detection, + Archive & ar, extrinsic_tag_based_sfm_calibrator::ApriltagDetection & detection, const unsigned int version) { (void)version; @@ -168,7 +168,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::ApriltagGridDetection & detection, + Archive & ar, extrinsic_tag_based_sfm_calibrator::ApriltagGridDetection & detection, const unsigned int version) { (void)version; @@ -187,7 +187,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::LidartagDetection & detection, + Archive & ar, extrinsic_tag_based_sfm_calibrator::LidartagDetection & detection, const unsigned int version) { (void)version; @@ -200,7 +200,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::ExternalCameraFrame & frame, + Archive & ar, extrinsic_tag_based_sfm_calibrator::ExternalCameraFrame & frame, const unsigned int version) { (void)version; @@ -211,7 +211,7 @@ void serialize( template void serialize( Archive & ar, - extrinsic_tag_based_base_calibrator::SingleCalibrationLidarDetections & lidar_detections, + extrinsic_tag_based_sfm_calibrator::SingleCalibrationLidarDetections & lidar_detections, const unsigned int version) { (void)version; @@ -223,7 +223,7 @@ void serialize( template void serialize( Archive & ar, - extrinsic_tag_based_base_calibrator::SingleCalibrationCameraDetections & camera_detections, + extrinsic_tag_based_sfm_calibrator::SingleCalibrationCameraDetections & camera_detections, const unsigned int version) { (void)version; @@ -235,7 +235,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::CalibrationScene & scene, + Archive & ar, extrinsic_tag_based_sfm_calibrator::CalibrationScene & scene, const unsigned int version) { (void)version; @@ -246,7 +246,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::UID & uid, const unsigned int version) + Archive & ar, extrinsic_tag_based_sfm_calibrator::UID & uid, const unsigned int version) { (void)version; ar & uid.type; @@ -261,7 +261,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::IntrinsicParameters & intrinsics, + Archive & ar, extrinsic_tag_based_sfm_calibrator::IntrinsicParameters & intrinsics, const unsigned int version) { (void)version; @@ -273,7 +273,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_base_calibrator::CalibrationData & data, + Archive & ar, extrinsic_tag_based_sfm_calibrator::CalibrationData & data, const unsigned int version) { (void)version; @@ -310,4 +310,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 // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/types.hpp index 3463a16c..d8523e23 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/types.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ #include #include @@ -26,7 +26,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { struct ApriltagDetectorParameters @@ -231,15 +231,15 @@ struct UID // types }; -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_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 extrinsic_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 // EXTRINSIC_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/visualization.hpp index a4ee4f95..e2094e23 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/include/extrinsic_tag_based_base_calibrator/visualization.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_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,11 +12,11 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ -#include -#include +#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__VISUALIZATION_HPP_ +#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/launch/apriltag_detector.launch.py b/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.py similarity index 100% rename from sensor/extrinsic_tag_based_base_calibrator/launch/apriltag_detector.launch.py rename to sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.py diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml new file mode 100644 index 00000000..82018413 --- /dev/null +++ b/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml @@ -0,0 +1,240 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml b/sensor/extrinsic_tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml new file mode 100644 index 00000000..01630092 --- /dev/null +++ b/sensor/extrinsic_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/extrinsic_tag_based_sfm_calibrator/package.xml similarity index 91% rename from sensor/extrinsic_tag_based_base_calibrator/package.xml rename to sensor/extrinsic_tag_based_sfm_calibrator/package.xml index 6a2a0393..99acf62c 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/package.xml +++ b/sensor/extrinsic_tag_based_sfm_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_tag_based_base_calibrator + extrinsic_tag_based_sfm_calibrator 0.0.1 - The extrinsic_tag_based_base_calibrator package + The extrinsic_tag_based_sfm_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_tag_based_base_calibrator/rviz/xx1.rviz b/sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz similarity index 58% rename from sensor/extrinsic_tag_based_base_calibrator/rviz/xx1.rviz rename to sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz index d5320dcd..65e2a436 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/rviz/xx1.rviz +++ b/sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz @@ -7,9 +7,8 @@ Panels: - /Global Options1 - /Status1 - /calibration markers1/Namespaces1 - - /base_link_grid1 Splitter Ratio: 0.5 - Tree Height: 746 + Tree Height: 719 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -28,17 +27,17 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: left + SyncSource: "" Visualization Manager: Class: "" Displays: - Class: rviz_default_plugins/Axes - Enabled: false + Enabled: true Length: 1 Name: Axes Radius: 0.009999999776482582 Reference Frame: - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -57,7 +56,7 @@ Visualization Manager: Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: top + Name: lidar0 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -69,7 +68,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/top/pointcloud_raw + Value: lidar0_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -82,8 +81,8 @@ 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 Invert Rainbow: false @@ -91,7 +90,7 @@ Visualization Manager: Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: left + Name: lidar1 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -103,7 +102,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/left/pointcloud_raw + Value: lidar1_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -116,8 +115,8 @@ 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 Invert Rainbow: false @@ -125,7 +124,7 @@ Visualization Manager: Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: right + Name: lidar2 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -137,7 +136,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/right/pointcloud_raw + Value: lidar2_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -150,8 +149,8 @@ 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 Invert Rainbow: false @@ -159,7 +158,7 @@ Visualization Manager: Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 - Name: rear + Name: lidar3 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -171,158 +170,158 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /sensing/lidar/rear/pointcloud_raw + Value: lidar3_pointcloud Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz_default_plugins/MarkerArray + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -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: lidar4 + 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: lidar4_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: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: lidar5 + 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: lidar5_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: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: lidar6 + 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: lidar6_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: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: lidar7 + 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: lidar7_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false 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 + Value: false - Alpha: 0.5 Cell Size: 0.10000000149011612 Class: rviz_default_plugins/Grid @@ -360,29 +359,74 @@ Visualization Manager: Reference Frame: estimated_base_link Value: false - Class: rviz_default_plugins/Axes - Enabled: true + Enabled: false Length: 1 Name: base link Radius: 0.009999999776482582 Reference Frame: base_link - Value: true + Value: false - Class: rviz_default_plugins/Axes - Enabled: true + 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: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar0/lidartag/tag_frame Value: true - - Class: rviz_default_plugins/Axes + - Class: rviz_default_plugins/Marker Enabled: true - Length: 1 - Name: velodyne_top - Radius: 0.009999999776482582 - Reference Frame: velodyne_top + Name: lidar1_detections_frames + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar1/lidartag/tag_frame + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: lidar2_detections_frames + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar2/lidartag/tag_frame + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: lidar3_detections_frames + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar3/lidartag/tag_frame Value: true - Class: rviz_default_plugins/Marker Enabled: true - Name: rear_detections_frames + Name: lidar4_detections_frames Namespaces: {} Topic: @@ -391,11 +435,11 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /rear/lidartag/tag_frame + Value: /lidar4/lidartag/tag_frame Value: true - Class: rviz_default_plugins/Marker Enabled: true - Name: left_detections_frames + Name: lidar5_detections_frames Namespaces: {} Topic: @@ -404,11 +448,11 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /left/lidartag/tag_frame + Value: /lidar5/lidartag/tag_frame Value: true - Class: rviz_default_plugins/Marker Enabled: true - Name: right_detections_frames + Name: lidar6_detections_frames Namespaces: {} Topic: @@ -417,24 +461,24 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /right/lidartag/tag_frame + Value: /lidar6/lidartag/tag_frame Value: true - Class: rviz_default_plugins/Marker Enabled: true - Name: top_detections_frames + Name: lidar7_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: /lidar7/lidartag/tag_frame Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: rear_detections_ids + Name: lidar0_detections_ids Namespaces: {} Topic: @@ -442,11 +486,11 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /rear/lidartag/id_markers + Value: /lidar0/lidartag/id_markers Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: left_detections_ids + Name: lidar1_detections_ids Namespaces: {} Topic: @@ -454,11 +498,11 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /left/lidartag/id_markers + Value: /lidar1/lidartag/id_markers Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: right_detections_ids + Name: lidar2_detections_ids Namespaces: {} Topic: @@ -466,24 +510,72 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /right/lidartag/id_markers + Value: /lidar2/lidartag/id_markers Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: top_detections_ids + Name: lidar3_detections_ids Namespaces: - Text0: true + {} + 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: /top/lidartag/id_markers + Value: /lidar7/lidartag/id_markers Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: velodyne_top + Fixed Frame: main_sensor Frame Rate: 30 Name: root Tools: @@ -541,15 +633,15 @@ Visualization Manager: Z: 4.948040962219238 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 1.6139578819274902 + Yaw: 1.5689579248428345 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001cd00000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044e0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001cd0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044e0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -559,5 +651,5 @@ Window Geometry: Views: collapsed: false Width: 1846 - X: 74 - Y: 0 + X: 1994 + Y: 27 diff --git a/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py b/sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py similarity index 90% rename from sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py rename to sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py index 0abdfe88..1226b30a 100755 --- a/sensor/extrinsic_tag_based_base_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py @@ -18,8 +18,8 @@ import sys from PySide2.QtWidgets import QApplication -from extrinsic_tag_based_base_calibrator import CalibratorUI -from extrinsic_tag_based_base_calibrator import RosInterface +from extrinsic_tag_based_sfm_calibrator import CalibratorUI +from extrinsic_tag_based_sfm_calibrator import RosInterface import rclpy diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp similarity index 92% rename from sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp index 9438bab7..9840f101 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detection.cpp +++ b/sensor/extrinsic_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,21 +14,16 @@ #include #include -#include -#include +#include +#include #include #include #include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else #include -#endif #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { LidartagDetection LidartagDetection::fromLidartagDetectionMsg( @@ -73,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()); @@ -156,11 +154,11 @@ double ApriltagDetection::computeReprojectionError(double cx, double cy, double 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(); @@ -190,7 +188,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; @@ -254,7 +252,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; @@ -262,13 +260,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(); @@ -358,4 +356,4 @@ double ApriltagGridDetection::detectionDiagonalRatio() const (rows * cols); } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detector.cpp similarity index 89% rename from sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detector.cpp index 4c63dd48..556cc1e6 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/apriltag_detector.cpp +++ b/sensor/extrinsic_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,8 +14,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -26,7 +26,7 @@ #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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); } @@ -241,8 +249,8 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c 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", + "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,8 +261,8 @@ 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", + "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; @@ -262,7 +270,8 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c 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", + "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); @@ -324,4 +333,4 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c return grid_detections_map; } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/calibration_scene_extractor.cpp b/sensor/extrinsic_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/extrinsic_tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp index a3a90c8c..349f32a9 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/calibration_scene_extractor.cpp +++ b/sensor/extrinsic_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,27 +12,22 @@ // 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 +#include #include #include #include #include #include +#include +#include #include +#include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/ceres/calibration_problem.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp similarity index 98% rename from sensor/extrinsic_tag_based_base_calibrator/src/ceres/calibration_problem.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp index 51f3acc8..f62db563 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/ceres/calibration_problem.cpp +++ b/sensor/extrinsic_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,11 @@ // limitations under the License. #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -34,7 +34,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { void CalibrationProblem::setOptimizeIntrinsics(bool ba_optimize_intrinsics) @@ -190,7 +190,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, 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 +558,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 +907,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; @@ -1153,6 +1156,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], @@ -1210,4 +1214,4 @@ void CalibrationProblem::printCalibrationResults() } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp similarity index 82% rename from sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp index 1642b4eb..72d3fb6b 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/extrinsic_tag_based_base_calibrator.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_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,14 +13,14 @@ // limitations under the License. #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -43,12 +43,12 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( const rclcpp::NodeOptions & options) -: Node("extrinsic_tag_based_base_calibrator_node", options), +: Node("extrinsic_tag_based_sfm_calibrator_node", options), tf_broadcaster_(this), calibration_done_(false), data_(std::make_shared()) @@ -58,21 +58,6 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( 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 +75,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_); @@ -117,39 +98,22 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( calibration_image_detections_topics = remove_empty_strings(calibration_image_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_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 +121,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 +181,75 @@ 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); + ba_fixed_ground_plane_model_a_ = this->declare_parameter("ba.fixed_ground_plane_model_a"); + ba_fixed_ground_plane_model_b_ = this->declare_parameter("ba.fixed_ground_plane_model_b"); + ba_fixed_ground_plane_model_c_ = this->declare_parameter("ba.fixed_ground_plane_model_c"); + ba_fixed_ground_plane_model_d_ = this->declare_parameter("ba.fixed_ground_plane_model_d"); // 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); + this->declare_parameter("apriltag.max_out_of_plane_angle"); apriltag_detector_parameters_.max_reprojection_error = - this->declare_parameter("apriltag_max_reprojection_error", 10.0); + 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] = @@ -337,27 +290,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 +313,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 +376,34 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( } void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( - __attribute__((unused)) - const std::shared_ptr + [[maybe_unused]] const std::shared_ptr< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> 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< + tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Response> + 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::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); + + 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 +414,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; @@ -476,76 +433,23 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( 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; + 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 = @@ -575,6 +479,50 @@ 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()); + + // Format the output + auto cv_to_eigen_pose = [](const cv::Affine3d & pose_cv) -> Eigen::Affine3d { + Eigen::Matrix4d matrix; + cv::cv2eigen(pose_cv.matrix, matrix); + return Eigen::Affine3d(matrix); + }; + + 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(cv_to_eigen_pose(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(cv_to_eigen_pose(*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( @@ -958,6 +906,25 @@ 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; +} + bool ExtrinsicTagBasedBaseCalibrator::addExternalCameraImagesCallback( const std::shared_ptr request, std::shared_ptr response) @@ -998,7 +965,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 +974,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 +1117,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 +1201,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 +1311,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); @@ -1464,7 +1413,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 +1472,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 +1482,4 @@ bool ExtrinsicTagBasedBaseCalibrator::saveDatabaseCallback( return true; } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp b/sensor/extrinsic_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/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp index 56df1ac4..a292ac54 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp +++ b/sensor/extrinsic_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,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -22,7 +22,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { void ApriltagBasedCalibrator::extractCalibrationPoints() @@ -128,4 +128,4 @@ void ApriltagBasedCalibrator::writeDebugImages(const IntrinsicParameters & intri } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp b/sensor/extrinsic_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/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp index 9add0671..6eb6e589 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp +++ b/sensor/extrinsic_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,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -22,7 +22,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { void ChessboardBasedCalibrator::extractCalibrationPoints() @@ -112,4 +112,4 @@ void ChessboardBasedCalibrator::writeDebugImages(const IntrinsicParameters & int } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp similarity index 93% rename from sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp index 07f43f44..b2faedb2 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp +++ b/sensor/extrinsic_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,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -22,7 +22,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_tag_based_sfm_calibrator { void IntrinsicsCalibrator::setCalibrationImageFiles( @@ -103,4 +103,4 @@ bool IntrinsicsCalibrator::calibrate(IntrinsicParameters & intrinsics) return true; } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/main.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/main.cpp similarity index 73% rename from sensor/extrinsic_tag_based_base_calibrator/src/main.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/main.cpp index 8e4e92f0..9b467983 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/main.cpp +++ b/sensor/extrinsic_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,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( + 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/extrinsic_tag_based_sfm_calibrator/src/math.cpp similarity index 95% rename from sensor/extrinsic_tag_based_base_calibrator/src/math.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp index a26f516e..84648e2d 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/math.cpp +++ b/sensor/extrinsic_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,15 +12,15 @@ // 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 EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ +#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -30,7 +30,7 @@ #include #include -namespace extrinsic_tag_based_base_calibrator +namespace extrinsic_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 @@ -327,7 +327,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 +335,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 +349,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 +403,7 @@ 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 { std::map>> raw_poses_map; @@ -445,6 +445,6 @@ void estimateInitialPoses( } } -} // namespace extrinsic_tag_based_base_calibrator +} // namespace extrinsic_tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_BASE_CALIBRATOR__MATH_HPP_ +#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ diff --git a/sensor/extrinsic_tag_based_base_calibrator/src/visualization.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/visualization.cpp similarity index 97% rename from sensor/extrinsic_tag_based_base_calibrator/src/visualization.cpp rename to sensor/extrinsic_tag_based_sfm_calibrator/src/visualization.cpp index 0dd2181b..6a06e2fc 100644 --- a/sensor/extrinsic_tag_based_base_calibrator/src/visualization.cpp +++ b/sensor/extrinsic_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 extrinsic_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 extrinsic_tag_based_sfm_calibrator diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..02c48b6b --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml index a70c9abb..177ba8a9 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml @@ -32,9 +32,9 @@ - - - + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py index 2ac5e49f..176a01ef 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py @@ -2,10 +2,12 @@ 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 __all__ = [ "MappingBasedBaseLidarCalibrator", "MappingBasedLidarLidarCalibrator", "MarkerRadarLidarCalibrator", "TagBasedPNPCalibrator", + "TagBasedSfmBaseLidarCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..77eb5483 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py @@ -0,0 +1,48 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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] + ) + + print("RDV_tagBasedSfmBaseCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py index 47113fce..06d83bbf 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py @@ -67,6 +67,7 @@ def __init__(self): # self.setWindowTitle("New extrinsic calibration manager") self.ros_interface: RosInterface = None + self.tfs_dict: Dict[str, Dict[str, None]] = defaultdict(lambda: dict) # Threading variables self.lock = threading.RLock() @@ -156,7 +157,10 @@ def launch_calibrators( + calibrator_name + ".launch.xml" ) - self.process = subprocess.Popen(["ros2", "launch", launcher_path] + argument_list) + + command_list = ["ros2", "launch", launcher_path] + argument_list + print(f"Launching calibrator with the following command: {command_list}", flush=True) + self.process = subprocess.Popen(command_list) # Recover all the launcher arguments (in addition to user defined in launch_arguments) try: @@ -287,7 +291,8 @@ def tf_graph_callback2(self, tfs_dict): if self.calibrator.state != CalibratorState.WAITING_TFS: return - self.tfs_dict = tfs_dict + 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 6b2e1c90..779404f8 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py @@ -229,7 +229,10 @@ def is_list(arg: str): for key, value in args_dict.items(): if is_list(value): - args_dict[key] = [item.strip() for item in value.strip("[]").split(",")] + 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]] print(args_dict, flush=True) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py index d40cca51..ef0c02f3 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py @@ -112,11 +112,12 @@ def getSlicedTree( if current_node.frame in target_frames: sliced_children_frames = sliced_children_frames + [current_node.frame] - return ( - (sliced_node, sliced_children_frames) - if len(sliced_children) > 0 - else (None, sliced_children_frames) - ) + 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] @@ -198,7 +199,7 @@ def setTfs( graph_list.append("}") graph_string = "".join(graph_list) - # print(graph_string) + # print(f"graph_string={graph_string}") graphs = pydot.graph_from_dot_data(graph_string) graph = graphs[0] From 3ab1fbb4c8af764f30115c27a6c74bba12904f89 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 16 Jan 2024 22:45:42 +0900 Subject: [PATCH 16/49] feat: base-lidars working on the rdv Signed-off-by: Kenzo Lobos-Tsunekawa --- .../calibrator_ui.py | 6 +- .../extrinsic_tag_based_sfm_calibrator.hpp | 3 +- .../launch/calibrator.launch.xml | 4 + .../rviz/default.rviz | 45 +++++++--- .../extrinsic_tag_based_sfm_calibrator.cpp | 53 +++++++++++- .../src/math.cpp | 49 +++++++++++ ...based_sfm_base_lidar_calibrator.launch.xml | 4 +- ...ased_sfm_base_lidars_calibrator.launch.xml | 71 ++++++++++++++++ .../calibrators/rdv/__init__.py | 2 + .../tag_based_sfm_base_lidars_calibrator.py | 83 +++++++++++++++++++ .../new_extrinsic_calibration_manager.py | 2 +- 11 files changed, 298 insertions(+), 24 deletions(-) create mode 100644 sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py index 7fbe1232..c40be91c 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py +++ b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py @@ -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_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp index c6ab0815..17543277 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp @@ -223,6 +223,7 @@ 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 rclcpp::CallbackGroup::SharedPtr calibration_api_srv_group_; @@ -253,7 +254,7 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node // Calibration API parameters and variables std::string base_frame_; - + bool publish_tfs_; std::mutex mutex_; tf2_ros::StaticTransformBroadcaster tf_broadcaster_; std::shared_ptr tf_buffer_; diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml index 82018413..dd87a807 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml @@ -8,6 +8,8 @@ + + @@ -145,6 +147,8 @@ + + diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz b/sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz index 65e2a436..4e001325 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz +++ b/sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz @@ -27,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: lidar0 Visualization Manager: Class: "" Displays: @@ -311,22 +311,42 @@ Visualization Manager: Use rainbow: true Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: false + 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: 0.5 Cell Size: 0.10000000149011612 Class: rviz_default_plugins/Grid Color: 171; 171; 171 - Enabled: true + Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines @@ -339,7 +359,7 @@ Visualization Manager: Plane: XY Plane Cell Count: 80 Reference Frame: base_link - Value: true + Value: false - Alpha: 0.5 Cell Size: 0.10000000149011612 Class: rviz_default_plugins/Grid @@ -376,7 +396,7 @@ Visualization Manager: Enabled: true Name: lidar0_detections_frames Namespaces: - {} + "": true Topic: Depth: 5 Durability Policy: Volatile @@ -480,7 +500,8 @@ Visualization Manager: Enabled: true Name: lidar0_detections_ids Namespaces: - {} + Text0: true + Text5: true Topic: Depth: 5 Durability Policy: Volatile @@ -626,14 +647,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6899994015693665 + Pitch: 1.5647963285446167 Position: - X: 0.07426369190216064 - Y: -8.048012733459473 - Z: 4.948040962219238 + X: -0.49250850081443787 + Y: 0.09322844445705414 + Z: 11.653377532958984 Target Frame: Value: FPS (rviz_default_plugins) - Yaw: 1.5689579248428345 + Yaw: 4.707127094268799 Saved: ~ Window Geometry: Displays: diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp index 72d3fb6b..1d53a51e 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp @@ -56,6 +56,7 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( tf_buffer_ = std::make_shared(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); + publish_tfs_ = this->declare_parameter("publish_tfs"); base_frame_ = this->declare_parameter("base_frame", "base_link"); main_calibration_sensor_frame_ = @@ -283,6 +284,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), @@ -630,6 +633,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"; @@ -829,11 +833,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) { @@ -844,9 +849,43 @@ 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 cv_to_eigen_pose = [](const cv::Affine3d & pose_cv) -> Eigen::Affine3d { + Eigen::Matrix4d matrix; + cv::cv2eigen(pose_cv.matrix, matrix); + return Eigen::Affine3d(matrix); + }; + + 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(cv_to_eigen_pose(*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++) { @@ -873,8 +912,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); } } } @@ -885,7 +924,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() diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp index 84648e2d..1299c17b 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp @@ -405,6 +405,55 @@ void estimateInitialPoses( std::map estimated_poses; 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; raw_poses_map[main_sensor_uid].push_back( diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml index 02c48b6b..1103f6e5 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -4,6 +4,7 @@ + @@ -19,7 +20,7 @@ name="auxiliar_tag_ids" default="[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49]" /> - + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml new file mode 100644 index 00000000..b6828de2 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py index 176a01ef..93c04238 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py @@ -3,6 +3,7 @@ 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 __all__ = [ "MappingBasedBaseLidarCalibrator", @@ -10,4 +11,5 @@ "MarkerRadarLidarCalibrator", "TagBasedPNPCalibrator", "TagBasedSfmBaseLidarCalibrator", + "TagBasedSfmBaseLidarsCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py new file mode 100644 index 00000000..d149cd99 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py @@ -0,0 +1,83 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("RDV_tagBasedSfmBaseLidarsCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py index 06d83bbf..0ede87db 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py @@ -67,7 +67,7 @@ def __init__(self): # self.setWindowTitle("New extrinsic calibration manager") self.ros_interface: RosInterface = None - self.tfs_dict: Dict[str, Dict[str, None]] = defaultdict(lambda: dict) + self.tfs_dict: Dict[str, Dict[str, None]] = defaultdict(lambda: defaultdict(None)) # Threading variables self.lock = threading.RLock() From 1d2344b7fa9c2163de0a2fefc0c0fb275c15abf0 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 17 Jan 2024 11:48:51 +0900 Subject: [PATCH 17/49] feat: sfm calibrator integrated completely in rdv and although untested in the xx1. can use the initial calibration as a fixed ground plane Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 11 ++ .../joint_icp_extended_impl.hpp | 2 +- .../voxel_grid_triplets.hpp | 4 +- .../voxel_grid_triplets_impl.hpp | 3 + .../tier4_tag_utils/lidartag_hypothesis.hpp | 3 +- .../tier4_tag_utils/src/apriltag_filter.cpp | 25 ++-- .../src/apriltag_hypothesis.cpp | 2 +- .../interactive_calibrator.py | 4 +- .../omiya_calibration_room_2023.param.yaml | 4 - .../extrinsic_tag_based_sfm_calibrator.hpp | 10 +- .../launch/apriltag_detector.launch.py | 8 +- .../launch/apriltag_detector.launch.xml | 44 +++++++ .../launch/calibrator.launch.xml | 118 +++++++++++++---- .../package.xml | 1 + .../src/apriltag_detection.cpp | 3 +- .../src/ceres/calibration_problem.cpp | 10 +- .../extrinsic_tag_based_sfm_calibrator.cpp | 97 +++++++++----- ...based_sfm_base_lidar_calibrator.launch.xml | 49 +++++++ ...ased_sfm_base_lidars_calibrator.launch.xml | 73 +++++++++++ ..._base_lidars_cameras_calibrator.launch.xml | 121 ++++++++++++++++++ ...ing_based_base_lidar_calibrator.launch.xml | 4 + ...based_sfm_base_lidar_calibrator.launch.xml | 10 +- ...ased_sfm_base_lidars_calibrator.launch.xml | 2 + ..._base_lidars_cameras_calibrator.launch.xml | 121 ++++++++++++++++++ .../xx1/tag_based_pnp_calibrator.launch.xml | 70 ++++++++++ ...based_sfm_base_lidar_calibrator.launch.xml | 50 ++++++++ ..._base_lidars_cameras_calibrator.launch.xml | 103 +++++++++++++++ .../calibrators/default_project/__init__.py | 6 + .../tag_based_sfm_base_lidar_calibrator.py | 26 ++++ .../tag_based_sfm_base_lidars_calibrator.py | 42 ++++++ ...ased_sfm_base_lidars_cameras_calibrator.py | 57 +++++++++ .../calibrators/rdv/__init__.py | 2 + ...ased_sfm_base_lidars_cameras_calibrator.py | 121 ++++++++++++++++++ .../calibrators/xx1/__init__.py | 2 + .../tag_based_sfm_base_lidar_calibrator.py | 46 +++++++ .../calibrators/xx1_15/__init__.py | 2 + ...ased_sfm_base_lidars_cameras_calibrator.py | 92 +++++++++++++ .../views/tf_view.py | 1 + 38 files changed, 1251 insertions(+), 98 deletions(-) create mode 100644 sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py diff --git a/.cspell.json b/.cspell.json index 3db440fb..186acc88 100644 --- a/.cspell.json +++ b/.cspell.json @@ -6,16 +6,21 @@ "apriltags", "autoware", "auxiliar", + "axisd", "calib", "coeffs", "crossval", "crossvalidation", "discretization", "distro", + "downsampling", + "downsample", "eigen", + "eulers", "extrinsics", "figsize", "gicp", + "hesai", "homography", "hsize", "icp", @@ -28,11 +33,15 @@ "lidars", "lidartag", "lidartags", + "linalg", "matplotlib", "matx", "misdetection", + "nanosec", + "neighbours", "ncols", "nrows", + "pandar", "permutate", "pnp", "pointcloud", @@ -63,8 +72,10 @@ "undistort", "undistortion", "uniformingly", + "velodyne", "vectord", "voxel", + "voxels", "xaxis", "xlabel", "xlim", 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..9ca0077a 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 @@ -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..da833b1f 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 @@ -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..fc90da0f 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 @@ -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_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp b/common/tier4_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp index 1eee9295..f2637a38 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 @@ -56,7 +56,8 @@ class LidartagHypothesis void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); - void setMaxConvergenceThreshold(double transl, double tansl_dot, double angle, double angle_dot); + void setMaxConvergenceThreshold( + double transl, double translation_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); diff --git a/common/tier4_tag_utils/src/apriltag_filter.cpp b/common/tier4_tag_utils/src/apriltag_filter.cpp index 27ab4123..91c1eeca 100644 --- a/common/tier4_tag_utils/src/apriltag_filter.cpp +++ b/common/tier4_tag_utils/src/apriltag_filter.cpp @@ -12,23 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#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 { @@ -50,14 +43,16 @@ ApriltagFilter::ApriltagFilter(const rclcpp::NodeOptions & options) 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( diff --git a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp index 14d7c8aa..22b0dde7 100644 --- a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp @@ -133,7 +133,7 @@ std::vector ApriltagHypothesis::getPoints3d( pinhole_camera_model_.distortionCoeffs(), rvec, tvec, false, cv::SOLVEPNP_SQPNP); if (!success) { - RCLCPP_ERROR(rclcpp::get_logger("teir4_tag_utils"), "PNP failed"); + RCLCPP_ERROR(rclcpp::get_logger("tier4_tag_utils"), "PNP failed"); return object_points; } diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py index b9938d22..e777efb5 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py @@ -149,7 +149,7 @@ def __init__(self, ros_interface): self.make_data_collection_options() # Visualization group - self.make_vizualization_options() + self.make_visualization_options() # self.menu_layout.addWidget(label) self.left_menu_layout.addWidget(self.calibration_api_group) @@ -423,7 +423,7 @@ def republish_data_callback(state): data_collection_options_layout.addStretch(1) self.data_collection_options_group.setLayout(data_collection_options_layout) - def make_vizualization_options(self): + def make_visualization_options(self): self.visualization_options_group = QGroupBox("Visualization options") self.visualization_options_group.setFlat(True) diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml b/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml index 8fa1cac2..a0fc7211 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml +++ b/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml @@ -49,10 +49,6 @@ external_camera_optimization_weight: 0.6 fixed_ground_plane_model: false - fixed_ground_plane_model_a: -0.015014 - fixed_ground_plane_model_b: 0.028632 - fixed_ground_plane_model_c: 0.999477 - fixed_ground_plane_model_d: 2.849738 # Initial intrinsics calibration diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp index 17543277..7913d769 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp @@ -114,6 +114,12 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node */ 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 @@ -308,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 diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.py b/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.py index c0c19bc5..646e1525 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.py +++ b/sensor/extrinsic_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/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml b/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml new file mode 100644 index 00000000..84a2d1e4 --- /dev/null +++ b/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml index dd87a807..858514bc 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml @@ -9,6 +9,7 @@ + @@ -90,23 +91,32 @@ - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -148,6 +163,7 @@ + @@ -157,9 +173,9 @@ - + - + @@ -240,5 +256,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/package.xml b/sensor/extrinsic_tag_based_sfm_calibrator/package.xml index 99acf62c..b2d8d0c1 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/package.xml +++ b/sensor/extrinsic_tag_based_sfm_calibrator/package.xml @@ -37,6 +37,7 @@ tf2_ros tier4_autoware_utils tier4_calibration_msgs + tier4_ground_plane_utils tier4_tag_utils visualization_msgs diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp index 9840f101..5f6a5625 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp @@ -106,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; } diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp index f62db563..1ccb2193 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp @@ -1007,13 +1007,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, diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp index 1d53a51e..626d3617 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -88,15 +89,15 @@ 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); for (std::size_t lidar_index = 0; lidar_index < calibration_lidar_frames_vector_.size(); @@ -112,7 +113,7 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( const std::string camera_name = calibration_camera_frames_vector_[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]; } @@ -196,10 +197,6 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( 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"); - ba_fixed_ground_plane_model_b_ = this->declare_parameter("ba.fixed_ground_plane_model_b"); - ba_fixed_ground_plane_model_c_ = this->declare_parameter("ba.fixed_ground_plane_model_c"); - ba_fixed_ground_plane_model_d_ = this->declare_parameter("ba.fixed_ground_plane_model_d"); // Initial intrinsic calibration parameters initial_intrinsic_calibration_board_type_ = @@ -390,7 +387,7 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( // Get some of the initial tfs before calibration geometry_msgs::msg::Transform initial_base_link_to_lidar_msg; - Eigen::Affine3d initial_base_link_to_lidar_pose; + 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 { @@ -435,7 +432,7 @@ 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); + 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 @@ -455,7 +452,7 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( 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 = initial_base_link_to_calibrated_base_link_pose.rotation(); @@ -483,20 +480,30 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( RCLCPP_INFO( this->get_logger(), "\t z: %.3f m", initial_base_link_to_calibrated_base_link_translation.z()); - // Format the output - auto cv_to_eigen_pose = [](const cv::Affine3d & pose_cv) -> Eigen::Affine3d { - Eigen::Matrix4d matrix; - cv::cv2eigen(pose_cv.matrix, matrix); - return Eigen::Affine3d(matrix); - }; + 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(cv_to_eigen_pose(calibrated_main_sensor_to_base_link_pose_)); + 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); @@ -509,7 +516,7 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( "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(cv_to_eigen_pose(*pose)); + result.transform_stamped = tf2::eigenToTransform(cvToEigenPose(*pose)); result.transform_stamped.header.frame_id = main_calibration_sensor_frame_; if (sensor_uid == main_sensor_uid) { @@ -855,17 +862,11 @@ void ExtrinsicTagBasedBaseCalibrator::visualizationTimerCallback() 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 cv_to_eigen_pose = [](const cv::Affine3d & pose_cv) -> Eigen::Affine3d { - Eigen::Matrix4d matrix; - cv::cv2eigen(pose_cv.matrix, matrix); - return Eigen::Affine3d(matrix); - }; - 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(cv_to_eigen_pose(*pose)); + transform_stamped_msg = tf2::eigenToTransform(cvToEigenPose(*pose)); transform_stamped_msg.header.frame_id = main_calibration_sensor_frame_; if (sensor_uid == main_sensor_uid) { @@ -970,6 +971,13 @@ UID ExtrinsicTagBasedBaseCalibrator::getMainSensorUID() const 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) @@ -1405,14 +1413,39 @@ 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); + } + 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_, diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..aec0fd7e --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml new file mode 100644 index 00000000..bdf94703 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..c8267c3d --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml index 0b4f2b99..6993a975 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml @@ -7,6 +7,8 @@ + + @@ -57,6 +59,8 @@ + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml index 1103f6e5..18eae754 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -5,16 +5,17 @@ + - - + + - + - + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml index b6828de2..c4474ae1 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -5,6 +5,7 @@ + @@ -43,6 +44,7 @@ + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..3ad91a21 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..823dd8a4 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..91c6d1fd --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..49c2ec69 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py index bb5c08e3..c09d5c9b 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py @@ -2,10 +2,16 @@ from .lidar_lidar_2d_calibrator import LidarLidar2DCalibrator from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator 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", "MappingBasedLidarLidarCalibrator", "TagBasedPNPCalibrator", + "TagBasedSfmBaseLidarCalibrator", + "TagBasedSfmBaseLidarsCalibrator", + "TagBasedSfmBaseLidarsCamerasCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..9974ac52 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py @@ -0,0 +1,26 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py new file mode 100644 index 00000000..c1d07ba7 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py @@ -0,0 +1,42 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..6b4f0f62 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,57 @@ +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py index 93c04238..3e4f24c5 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py @@ -4,6 +4,7 @@ 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", @@ -12,4 +13,5 @@ "TagBasedPNPCalibrator", "TagBasedSfmBaseLidarCalibrator", "TagBasedSfmBaseLidarsCalibrator", + "TagBasedSfmBaseLidarsCamerasCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..9fc5330d --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,121 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("RDV_tagBasedSfmBaseLidarsCamerasCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py index 4d724c46..f4442dce 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py @@ -1,9 +1,11 @@ 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_sfm_base_lidar_calibrator import TagBasedSfmBaseLidarCalibrator __all__ = [ "GroundPlaneCalibrator", "MappingBasedBaseLidarCalibrator", "MappingBasedLidarLidarCalibrator", + "TagBasedSfmBaseLidarCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..3b6075df --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py @@ -0,0 +1,46 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py index 9a32d340..69c0988f 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py @@ -1,5 +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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..54268a11 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,92 @@ +from typing import Dict +from typing import List + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("RDV_tagBasedSfmBaseLidarsCamerasCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py index ef0c02f3..2bee20b3 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py @@ -209,6 +209,7 @@ def setTfs( # cspell: ignore imgdata imgdata = graph.create_svg() + # cSpell:ignore savefig # imgdata = StringIO() # figure.savefig(imgdata, format='svg') # imgdata.seek(0) From 9a3febd2d3e4c10eb74372f272021a5e1d6a41e3 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 18 Jan 2024 16:11:20 +0900 Subject: [PATCH 18/49] feat: integrated all the sfm methods to the x2 Signed-off-by: Kenzo Lobos-Tsunekawa --- .../omiya_calibration_room_2023.param.yaml | 1 + .../ceres/calibration_problem.hpp | 8 +- .../extrinsic_tag_based_sfm_calibrator.hpp | 1 + .../math.hpp | 14 +- .../serialization.hpp | 70 +++----- .../src/ceres/calibration_problem.cpp | 9 +- .../extrinsic_tag_based_sfm_calibrator.cpp | 45 +++-- .../src/math.cpp | 17 +- ...based_sfm_base_lidar_calibrator.launch.xml | 1 + ...ased_sfm_base_lidars_calibrator.launch.xml | 2 +- ...based_sfm_base_lidar_calibrator.launch.xml | 50 ++++++ ...ased_sfm_base_lidars_calibrator.launch.xml | 73 ++++++++ ..._base_lidars_cameras_calibrator.launch.xml | 121 +++++++++++++ .../calibrators/x2/__init__.py | 6 + .../x2/tag_based_sfm_base_lidar_calibrator.py | 46 +++++ .../tag_based_sfm_base_lidars_calibrator.py | 96 +++++++++++ ...ased_sfm_base_lidars_cameras_calibrator.py | 161 ++++++++++++++++++ 17 files changed, 634 insertions(+), 87 deletions(-) create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml b/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml index a0fc7211..572012ad 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml +++ b/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml @@ -3,6 +3,7 @@ # Tag configuration lidartag_to_apriltag_scale: 0.75 + write_debug_images: true waypoint_tag: family: "tag16h5" diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp index e65c2f4e..7f953efa 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp @@ -230,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_; diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp index 7913d769..7f6f5531 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp @@ -261,6 +261,7 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node // Calibration API parameters and variables std::string base_frame_; bool publish_tfs_; + bool write_debug_images_; std::mutex mutex_; tf2_ros::StaticTransformBroadcaster tf_broadcaster_; std::shared_ptr tf_buffer_; diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp index 967f78c7..aab34db4 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp @@ -22,6 +22,7 @@ #include #include +#include #include namespace extrinsic_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 dpose */ -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 diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp index b004c9e9..451366d9 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp @@ -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_sfm_calibrator::ApriltagDetection & detection, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & detection.family; ar & detection.id; ar & detection.image_corners; @@ -169,9 +160,8 @@ void serialize( template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::ApriltagGridDetection & detection, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & detection.cols; ar & detection.rows; ar & detection.sub_detections; @@ -188,9 +178,8 @@ void serialize( template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::LidartagDetection & detection, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & detection.id; ar & detection.object_corners; ar & detection.template_corners; @@ -201,9 +190,8 @@ void serialize( template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::ExternalCameraFrame & frame, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & frame.image_filename; ar & frame.detections; } @@ -212,9 +200,8 @@ template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::SingleCalibrationLidarDetections & lidar_detections, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & lidar_detections.calibration_frame; ar & lidar_detections.calibration_lidar_id; ar & lidar_detections.detections; @@ -224,9 +211,8 @@ template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::SingleCalibrationCameraDetections & camera_detections, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & camera_detections.calibration_frame; ar & camera_detections.calibration_camera_id; ar & camera_detections.grouped_detections; @@ -236,9 +222,8 @@ void serialize( template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::CalibrationScene & scene, - const unsigned int version) + [[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 +231,9 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::UID & uid, const unsigned int version) + Archive & ar, extrinsic_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; @@ -262,9 +247,8 @@ void serialize( template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::IntrinsicParameters & intrinsics, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & intrinsics.size; ar & intrinsics.camera_matrix; ar & intrinsics.dist_coeffs; @@ -274,9 +258,8 @@ void serialize( template void serialize( Archive & ar, extrinsic_tag_based_sfm_calibrator::CalibrationData & data, - const unsigned int version) + [[maybe_unused]] const unsigned int version) { - (void)version; ar & data.scenes; ar & data.main_calibration_sensor_uid; ar & data.uid_connections_map; @@ -300,9 +283,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; } diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp index 1ccb2193..a97bd8d2 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp @@ -33,6 +33,7 @@ #include #include +#include namespace extrinsic_tag_based_sfm_calibrator { @@ -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,7 @@ 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, + 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], diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp index 626d3617..fc989b8d 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp @@ -58,6 +58,7 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( 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"); main_calibration_sensor_frame_ = @@ -798,30 +799,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); } } @@ -1441,6 +1443,9 @@ bool ExtrinsicTagBasedBaseCalibrator::calibrationCallback( 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_); @@ -1461,24 +1466,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; diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp b/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp index 1299c17b..c865f961 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp @@ -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( diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml index aec0fd7e..f45373a5 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -33,6 +33,7 @@ + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml index c4474ae1..6f69fb86 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml new file mode 100644 index 00000000..5fe18728 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml new file mode 100644 index 00000000..3e9ee13d --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml new file mode 100644 index 00000000..719d3c72 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py index c70b97f4..b820c225 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py @@ -2,10 +2,16 @@ 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_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", + "TagBasedSfmBaseLidarCalibrator", + "TagBasedSfmBaseLidarsCalibrator", + "TagBasedSfmBaseLidarsCamerasCalibrator", ] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py new file mode 100644 index 00000000..dbaab6b8 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py @@ -0,0 +1,46 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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]) + + print("RDV_tagBasedSfmBaseCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py new file mode 100644 index 00000000..0072b5b7 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py @@ -0,0 +1,96 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("X2_tagBasedSfmBaseLidarsCalibrator") + + 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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py new file mode 100644 index 00000000..792006e2 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -0,0 +1,161 @@ +from collections import defaultdict +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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, + ] + ) + + print("X2_tagBasedSfmBaseLidarsCamerasCalibrator") + + 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 From 790b363dfde8847ae702c5000035a7fce6750672 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 18 Jan 2024 18:47:34 +0900 Subject: [PATCH 19/49] feat: integrated the camera-lidar into the remaining project/products Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 1 + .../tier4_tag_utils/src/lidartag_filter.cpp | 17 ++-- .../src/filters/object_detection_filter.cpp | 7 +- .../extrinsic_tag_based_pnp_calibrator.cpp | 7 +- .../math.hpp | 2 +- .../rdv/tag_based_pnp_calibrator.launch.xml | 2 + .../x2/tag_based_pnp_calibrator.launch.xml | 90 +++++++++++++++++++ .../xx1/tag_based_pnp_calibrator.launch.xml | 6 +- .../tag_based_pnp_calibrator.launch.xml | 2 + .../calibrators/x2/__init__.py | 2 + .../x2/tag_based_pnp_calibrator.py | 85 ++++++++++++++++++ .../calibrators/xx1/__init__.py | 2 + .../xx1/tag_based_pnp_calibrator.py | 54 +++++++++++ 13 files changed, 250 insertions(+), 27 deletions(-) create mode 100644 sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py create mode 100644 sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py diff --git a/.cspell.json b/.cspell.json index 186acc88..4300ab1d 100644 --- a/.cspell.json +++ b/.cspell.json @@ -11,6 +11,7 @@ "coeffs", "crossval", "crossvalidation", + "opencv", "discretization", "distro", "downsampling", diff --git a/common/tier4_tag_utils/src/lidartag_filter.cpp b/common/tier4_tag_utils/src/lidartag_filter.cpp index abc10a4e..ee460ab3 100644 --- a/common/tier4_tag_utils/src/lidartag_filter.cpp +++ b/common/tier4_tag_utils/src/lidartag_filter.cpp @@ -12,22 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - #include #include #include #include +#include +#include +#include + +#include namespace tier4_tag_utils { diff --git a/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp b/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp index ff16b060..63b36aa9 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp +++ b/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp @@ -15,16 +15,11 @@ #include #include #include +#include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp index 9c75536a..b5e4e38a 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp +++ b/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include @@ -23,12 +24,6 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::NodeOptions & options) : Node("extrinsic_tag_based_pnp_calibrator_node", options), tf_broadcaster_(this), diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp index aab34db4..3131de7c 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp +++ b/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp @@ -48,7 +48,7 @@ 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 - * @returns the ground dpose + * @returns the ground pose */ std::optional computeGroundPlane(const std::vector & points); diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml index 823dd8a4..ab5f20be 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml @@ -24,6 +24,7 @@ + @@ -41,6 +42,7 @@ + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml new file mode 100644 index 00000000..fc97be51 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml index 823dd8a4..241994e1 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml @@ -22,8 +22,9 @@ - - + + + @@ -41,6 +42,7 @@ + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml index 21ddcca9..aada10f2 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml @@ -26,6 +26,7 @@ + @@ -43,6 +44,7 @@ + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py index b820c225..783eb337 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py @@ -2,6 +2,7 @@ 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 @@ -11,6 +12,7 @@ "MappingBasedBaseLidarCalibrator", "MappingBasedLidarLidarCalibrator", "MarkerRadarLidarCalibrator", + "TagBasedPNPCalibrator", "TagBasedSfmBaseLidarCalibrator", "TagBasedSfmBaseLidarsCalibrator", "TagBasedSfmBaseLidarsCamerasCalibrator", diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..53e9a147 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py @@ -0,0 +1,85 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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", + ] + ) + + print("X2::TagBasedPNPCalibrator") + + 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]]): + camera_to_lidar_transform = calibration_transforms[ + f"{self.camera_name}/camera_optical_link" + ][self.lidar_frame] + + print(f"camera_to_lidar_transform={camera_to_lidar_transform}", flush=True) + + sensor_kit_to_lidar_transform = self.get_transform_matrix( + self.sensor_kit_frame, self.lidar_frame + ) + + print(f"sensor_kit_to_lidar_transform={sensor_kit_to_lidar_transform}", flush=True) + + camera_to_optical_link_transform = self.get_transform_matrix( + f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link" + ) + + print(f"camera_to_optical_link_transform={camera_to_optical_link_transform}", flush=True) + + sensor_kit_camera_link_transform = np.linalg.inv( + camera_to_optical_link_transform + @ camera_to_lidar_transform + @ np.linalg.inv(sensor_kit_to_lidar_transform) + ) + + print(f"sensor_kit_camera_link_transform={sensor_kit_camera_link_transform}", flush=True) + + result = { + self.sensor_kit_frame: { + f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform + } + } + return result diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py index f4442dce..b5c0103c 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py @@ -1,11 +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/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py new file mode 100644 index 00000000..a5cf34e2 --- /dev/null +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py @@ -0,0 +1,54 @@ +from typing import Dict + +from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from new_extrinsic_calibration_manager.ros_interface import RosInterface +from new_extrinsic_calibration_manager.types import FramePair +import numpy as np + + +@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") + + print("TagBasedPNPCalibrator") + print(self.camera_name, flush=True) + + 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]]): + camera_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 + @ camera_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 From 352f854dc3525c639e605c81e9f7273fd3ee2e7b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 01:47:51 +0900 Subject: [PATCH 20/49] feat: refactored the itneractive ui, integrated it into the new api, and implemented the fixes for the spell checking Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 17 + common/tier4_calibration_views/CMakeLists.txt | 34 + common/tier4_calibration_views/package.xml | 29 + .../resource/tier4_calibration_views | 0 .../scripts/image_view_node.py | 53 ++ .../test/test_pep257.py | 23 + .../tier4_calibration_views/__init__.py | 0 .../tier4_calibration_views/image_view.py | 824 ++++++++++++++++++ .../image_view_ros_interface.py | 239 +++++ .../tier4_calibration_views/image_view_ui.py | 422 +++++++++ .../tier4_calibration_views/utils.py | 85 ++ sensor/docs/how_to_extrinsic_interactive.md | 4 +- .../calibrator.py | 26 +- .../image_view.py | 28 +- .../interactive_calibrator.py | 519 ++--------- .../ros_interface.py | 336 +------ .../extrinsic_interactive_calibrator/utils.py | 139 +-- .../package.xml | 2 +- .../scripts/calibrator_ui_node.py | 3 +- .../scripts/metrics_plotter_node.py | 2 +- .../calibrator_ui.py | 2 +- .../ros_interface.py | 2 +- .../scripts/calibrator_ui_node.py | 5 +- .../CMakeLists.txt | 28 - .../launch/calibration.launch.xml | 37 - .../launch/camera_bfs.launch.xml | 16 - .../launch/camera_li.launch.xml | 15 - .../launch/optimizer.launch.xml | 16 - .../intrinsic_camera_calibration/package.xml | 25 - .../scripts/camera_intrinsics_optimizer.py | 281 ------ .../apriltag_grid_detection.py | 2 +- .../board_detections/array_board_detection.py | 2 +- .../board_detections/board_detection.py | 2 +- .../board_detections/chess_board_detection.py | 2 +- .../board_detections/dotboard_detection.py | 2 +- .../board_detectors/apriltag_grid_detector.py | 4 +- .../board_detectors/board_detector.py | 2 +- .../board_detectors/board_detector_factory.py | 2 +- .../board_detectors/chessboard_detector.py | 2 +- .../board_detectors/dotboard_detector.py | 2 +- .../apriltag_grid_parameters.py | 2 +- .../board_parameters/board_parameters.py | 2 +- .../board_parameters_factory.py | 2 +- .../intrinsic_camera_calibrator/boards.py | 2 +- .../calibrators/calibrator.py | 11 +- .../calibrators/calibrator_factory.py | 2 +- .../calibrators/ceres_calibrator.py | 2 +- .../calibrators/opencv_calibrator.py | 2 +- .../calibrators/utils.py | 2 +- .../camera_calibrator.py | 37 +- .../camera_model.py | 2 +- .../data_collector.py | 2 +- .../data_sources/data_source.py | 2 +- .../data_sources/data_source_factory.py | 2 +- .../data_sources/image_files_data_source.py | 6 +- .../data_sources/ros_bag_data_source.py | 5 +- .../data_sources/ros_topic_data_source.py | 2 +- .../data_sources/video_file_data_source.py | 2 +- .../intrinsic_camera_calibrator/parameter.py | 2 +- .../intrinsic_camera_calibrator/types.py | 2 +- .../intrinsic_camera_calibrator/utils.py | 2 +- .../views/data_collector_view.py | 2 +- .../views/image_files_view.py | 2 +- .../views/image_view.py | 2 +- .../views/initialization_view.py | 2 +- .../views/parameter_view.py | 5 +- .../views/ros_bag_view.py | 2 +- .../views/ros_topic_view.py | 6 +- .../tag_based_pnp_calibrator.launch.xml | 13 +- .../rdv/tag_based_pnp_calibrator.launch.xml | 12 +- .../x2/tag_based_pnp_calibrator.launch.xml | 11 +- .../xx1/tag_based_pnp_calibrator.launch.xml | 11 +- .../tag_based_pnp_calibrator.launch.xml | 11 +- .../calibration_manager_model.py | 20 +- .../calibrator_base.py | 45 +- .../calibrator_registry.py | 16 + .../calibrator_wrapper.py | 35 +- .../ground_plane_calibrator.py | 19 +- .../lidar_lidar_2d_calibrator.py | 18 +- .../mapping_based_lidar_lidar_calibrator.py | 19 +- .../tag_based_pnp_calibrator.py | 19 +- .../tag_based_sfm_base_lidar_calibrator.py | 16 + .../tag_based_sfm_base_lidars_calibrator.py | 16 + ...ased_sfm_base_lidars_cameras_calibrator.py | 16 + .../mapping_based_base_lidar_calibrator.py | 18 +- .../mapping_based_lidar_lidar_calibrator.py | 20 +- .../rdv/marker_radar_lidar_calibrator.py | 18 +- .../rdv/tag_based_pnp_calibrator.py | 27 +- .../tag_based_sfm_base_lidar_calibrator.py | 18 +- .../tag_based_sfm_base_lidars_calibrator.py | 18 +- ...ased_sfm_base_lidars_cameras_calibrator.py | 18 +- .../calibrators/x1/ground_plane_calibrator.py | 18 +- .../calibrators/x2/ground_plane_calibrator.py | 18 +- .../x2/mapping_based_base_lidar_calibrator.py | 18 +- .../mapping_based_lidar_lidar_calibrator.py | 18 +- .../x2/marker_radar_lidar_calibrator.py | 18 +- .../x2/tag_based_pnp_calibrator.py | 26 +- .../x2/tag_based_sfm_base_lidar_calibrator.py | 18 +- .../tag_based_sfm_base_lidars_calibrator.py | 18 +- ...ased_sfm_base_lidars_cameras_calibrator.py | 18 +- .../xx1/ground_plane_calibrator.py | 18 +- .../mapping_based_base_lidar_calibrator.py | 18 +- .../mapping_based_lidar_lidar_calibrator.py | 20 +- .../xx1/tag_based_pnp_calibrator.py | 19 +- .../tag_based_sfm_base_lidar_calibrator.py | 16 + .../xx1_15/tag_based_pnp_calibrator.py | 19 +- ...ased_sfm_base_lidars_cameras_calibrator.py | 19 +- .../new_extrinsic_calibration_manager.py | 31 +- .../ros_interface.py | 4 +- .../types.py | 16 + .../views/calibrator_selector_view.py | 5 +- .../views/launcher_configuration_view.py | 12 +- .../views/tf_view.py | 14 +- 113 files changed, 2661 insertions(+), 1502 deletions(-) create mode 100644 common/tier4_calibration_views/CMakeLists.txt create mode 100644 common/tier4_calibration_views/package.xml create mode 100644 common/tier4_calibration_views/resource/tier4_calibration_views create mode 100755 common/tier4_calibration_views/scripts/image_view_node.py create mode 100644 common/tier4_calibration_views/test/test_pep257.py create mode 100644 common/tier4_calibration_views/tier4_calibration_views/__init__.py create mode 100644 common/tier4_calibration_views/tier4_calibration_views/image_view.py create mode 100644 common/tier4_calibration_views/tier4_calibration_views/image_view_ros_interface.py create mode 100644 common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py create mode 100644 common/tier4_calibration_views/tier4_calibration_views/utils.py delete mode 100644 sensor/intrinsic_camera_calibration/CMakeLists.txt delete mode 100644 sensor/intrinsic_camera_calibration/launch/calibration.launch.xml delete mode 100644 sensor/intrinsic_camera_calibration/launch/camera_bfs.launch.xml delete mode 100644 sensor/intrinsic_camera_calibration/launch/camera_li.launch.xml delete mode 100644 sensor/intrinsic_camera_calibration/launch/optimizer.launch.xml delete mode 100644 sensor/intrinsic_camera_calibration/package.xml delete mode 100755 sensor/intrinsic_camera_calibration/scripts/camera_intrinsics_optimizer.py diff --git a/.cspell.json b/.cspell.json index 4300ab1d..99c308a7 100644 --- a/.cspell.json +++ b/.cspell.json @@ -2,12 +2,16 @@ "words": [ "3dpoints", "Rodrigues", + "antialiasing", "apriltag", "apriltags", + "arange", "autoware", + "astype", "auxiliar", "axisd", "calib", + "cmap", "coeffs", "crossval", "crossvalidation", @@ -16,6 +20,7 @@ "distro", "downsampling", "downsample", + "dtype", "eigen", "eulers", "extrinsics", @@ -26,7 +31,10 @@ "hsize", "icp", "idless", + "idxs", + "imdecode", "imread", + "imshow", "imwrite", "intrinsics", "kalman", @@ -37,6 +45,7 @@ "linalg", "matplotlib", "matx", + "meshgrid", "misdetection", "nanosec", "neighbours", @@ -44,12 +53,15 @@ "nrows", "pandar", "permutate", + "pixmap", "pnp", "pointcloud", "pointclouds", + "polyline", "prerejective", "pydot", "pyplot", + "qcolor", "quaterniond", "ransac", "rclcpp", @@ -58,8 +70,11 @@ "registrators", "remappings", "representer", + "reprojected", "reprojection", "rosbag", + "rosidl", + "ruamel", "rvec", "rvecs", "rviz", @@ -68,6 +83,8 @@ "sqpnp", "srvs", "subsampled", + "subsamples", + "subsampling", "tvec", "tvecs", "undistort", diff --git a/common/tier4_calibration_views/CMakeLists.txt b/common/tier4_calibration_views/CMakeLists.txt new file mode 100644 index 00000000..029104b5 --- /dev/null +++ b/common/tier4_calibration_views/CMakeLists.txt @@ -0,0 +1,34 @@ +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) +#find_package(sensor_msgs REQUIRED) +#find_package(nav_msgs REQUIRED) +#find_package(geometry_msgs REQUIRED) + +autoware_package() +ament_python_install_package(${PROJECT_NAME}) + +#if(BUILD_TESTING) +# find_package(rclpy REQUIRED) +# find_package(ament_cmake_nose REQUIRED) + +# ament_add_nose_test(pointclouds test/test_pointclouds.py) +# ament_add_nose_test(images test/test_images.py) +# ament_add_nose_test(occupancygrids test/test_occupancygrids.py) +# ament_add_nose_test(geometry test/test_geometry.py) +# ament_add_nose_test(quaternions test/test_quat.py) +#endif() + +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/common/tier4_calibration_views/resource/tier4_calibration_views b/common/tier4_calibration_views/resource/tier4_calibration_views new file mode 100644 index 00000000..e69de29b 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/common/tier4_calibration_views/test/test_pep257.py b/common/tier4_calibration_views/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/common/tier4_calibration_views/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/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/common/tier4_calibration_views/tier4_calibration_views/image_view.py b/common/tier4_calibration_views/tier4_calibration_views/image_view.py new file mode 100644 index 00000000..976e4c07 --- /dev/null +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view.py @@ -0,0 +1,824 @@ +#!/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 + + 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 widet coordinate system + pointdloud_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 / pointdloud_z + else: + factor = self.data_renderer.marker_size_pixels * self.width_image_to_widget_factor + scale_px = factor * np.ones_like(pointdloud_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 = pointdloud_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) + pass + + 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) + + 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 widet 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) + + 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 widet 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 + + 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 widet 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/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..a38d96c2 --- /dev/null +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py @@ -0,0 +1,422 @@ +#!/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 + pass + + 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/common/tier4_calibration_views/tier4_calibration_views/utils.py b/common/tier4_calibration_views/tier4_calibration_views/utils.py new file mode 100644 index 00000000..efd98887 --- /dev/null +++ b/common/tier4_calibration_views/tier4_calibration_views/utils.py @@ -0,0 +1,85 @@ +#!/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 cv2 +from geometry_msgs.msg import TransformStamped +import numpy as np +import transforms3d + + +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)) + + transform_matrix[0:3, 0:3] = rot_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 + + return transform_matrix + + +def transform_matrix_to_tf_message(transform_matrix): + q = transforms3d.quaternions.mat2quat(transform_matrix[0:3, 0:3]) + + msg = TransformStamped() + msg.transform.translation.x = transform_matrix[0, 3] + msg.transform.translation.y = transform_matrix[1, 3] + msg.transform.translation.z = transform_matrix[2, 3] + msg.transform.rotation.x = q[1] + msg.transform.rotation.y = q[2] + msg.transform.rotation.z = q[3] + msg.transform.rotation.w = q[0] + + return msg + + +def transform_matrix_to_cv(transform_matrix): + rotation_matrix = transform_matrix[0:3, 0:3] + rvec, _ = cv2.Rodrigues(rotation_matrix) + tvec = transform_matrix[0:3, 3].reshape(3, 1) + + return tvec, rvec + + +def cv_to_transformation_matrix(tvec, rvec): + transform_matrix = np.eye(4) + + rotation_matrix, _ = cv2.Rodrigues(rvec) + + transform_matrix[0:3, 0:3] = rotation_matrix + transform_matrix[0:3, 3] = tvec.reshape( + 3, + ) + + return transform_matrix + + +def decompose_transformation_matrix(transformation): + return transformation[0:3, 3].reshape(3, 1), transformation[0:3, 0:3] + + +def transform_points(translation_vector, rotation_matrix, point_array): + _, dim = point_array.shape + assert dim == 3 + + return np.dot(point_array, np.transpose(rotation_matrix)) + translation_vector.reshape(1, 3) + + +def stamp_to_seconds(time): + return time.sec + 1e-9 * time.nanosec diff --git a/sensor/docs/how_to_extrinsic_interactive.md b/sensor/docs/how_to_extrinsic_interactive.md index c55d24e0..5c22af0b 100644 --- a/sensor/docs/how_to_extrinsic_interactive.md +++ b/sensor/docs/how_to_extrinsic_interactive.md @@ -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/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/calibrator.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/calibrator.py index 7c4fe170..cf10f8ba 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/calibrator.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_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: @@ -81,7 +85,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 +109,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 +129,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/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py index 10819f61..84dbf1eb 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/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): @@ -483,8 +484,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 +494,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 +514,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 +526,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) @@ -597,7 +597,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 @@ -668,7 +668,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 @@ -775,7 +775,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, @@ -817,7 +817,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/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py index e777efb5..d17285fd 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/interactive_calibrator.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_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 extrinsic_interactive_calibrator.ros_interface import InteractiveCalibratorRosInterface +from extrinsic_interactive_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_visualization_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() @@ -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 is 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_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 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,14 +354,16 @@ 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 @@ -643,16 +379,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 = 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,11 +400,7 @@ 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() @@ -724,7 +458,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 +485,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 +498,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 +519,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 +548,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() @@ -963,17 +586,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 +594,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 +606,14 @@ def main(args=None): try: signal.signal(signal.SIGINT, sigint_handler) - ros_interface = RosInterface() + ros_interface = InteractiveCalibratorRosInterface() ex = InteractiveCalibratorUI(ros_interface) # noqa: F841 ros_interface.spin() sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quitting...") + logging.info("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py index 0f5e1aea..58dd12c8 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.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,93 +14,44 @@ # 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 +from tier4_calibration_msgs.msg import CalibrationResult +from tier4_calibration_msgs.srv import NewExtrinsicCalibrator +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 -from visualization_msgs.msg import MarkerArray -class RosInterface(Node): +class InteractiveCalibratorRosInterface(ImageViewRosInterface): def __init__(self): super().__init__("interactive_calibrator") self.lock = threading.RLock() - 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_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 @@ -108,64 +59,18 @@ def __init__(self): # 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, + NewExtrinsicCalibrator, "extrinsic_calibration", self.calibration_api_service_callback, callback_group=self.service_callback_group, @@ -176,9 +81,11 @@ def __init__(self): def send_calibration_api_result(self, calibration_error): with self.lock: self.calibration_api_sent_pending = True - self.calibration_error = -calibration_error + self.calibration_error = calibration_error - def calibration_api_service_callback(self, request, response): + def calibration_api_service_callback( + self, request: NewExtrinsicCalibrator.Request, response: NewExtrinsicCalibrator.Response + ): # Notify the UI that a request was received self.calibration_api_request_received_callback() @@ -193,93 +100,44 @@ def calibration_api_service_callback(self, request, response): 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 + 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.score = self.calibration_error + response.results.append(result) 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: self.output_transform_msg = transform_matrix_to_tf_message( camera_optical_lidar_transform ) - self.output_transform_msg.header.frame_id = self.camera_frame + 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 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) @@ -291,83 +149,10 @@ def save_calibration_tfs(self, output_dir): 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)) + with open(os.path.join(output_dir, "tf.json"), "w") as f: + f.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): + 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: @@ -394,78 +179,13 @@ def point_callback(self, point): 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}" - ) + 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 - - 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/extrinsic_interactive_calibrator/utils.py b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/utils.py index 4ab9d364..ec31f747 100644 --- a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/utils.py +++ b/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/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. @@ -14,72 +14,87 @@ # See the License for the specific language governing permissions and # limitations under the License. +import array +from copy import deepcopy + import cv2 -from geometry_msgs.msg import TransformStamped import numpy as np -import transforms3d - - -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)) - - transform_matrix[0:3, 0:3] = rot_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 - - return transform_matrix - - -def transform_matrix_to_tf_message(transform_matrix): - q = transforms3d.quaternions.mat2quat(transform_matrix[0:3, 0:3]) - - msg = TransformStamped() - msg.transform.translation.x = transform_matrix[0, 3] - msg.transform.translation.y = transform_matrix[1, 3] - msg.transform.translation.z = transform_matrix[2, 3] - msg.transform.rotation.x = q[1] - msg.transform.rotation.y = q[2] - msg.transform.rotation.z = q[3] - msg.transform.rotation.w = q[0] - - return msg - - -def transform_matrix_to_cv(transform_matrix): - rotation_matrix = transform_matrix[0:3, 0:3] - rvec, _ = cv2.Rodrigues(rotation_matrix) - tvec = transform_matrix[0:3, 3].reshape(3, 1) - - return tvec, rvec - - -def cv_to_transformation_matrix(tvec, rvec): - transform_matrix = np.eye(4) - - rotation_matrix, _ = cv2.Rodrigues(rvec) - - transform_matrix[0:3, 0:3] = rotation_matrix - transform_matrix[0:3, 3] = tvec.reshape( - 3, +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, ) - return transform_matrix - - -def decompose_transformation_matrix(transformation): - return transformation[0:3, 3].reshape(3, 1), transformation[0:3, 0:3] + 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, + ) -def transform_points(translation_vector, rotation_matrix, point_array): - num_points, dim = point_array.shape - assert dim == 3 - - return np.dot(point_array, np.transpose(rotation_matrix)) + translation_vector.reshape(1, 3) + p = np.zeros((3, 4), dtype=np.float64) + for j in range(3): + for i in range(3): + p[j, i] = ncm[j, i] -def stamp_to_seconds(time): - return time.sec + 1e-9 * time.nanosec + optimized_camera_info.p = p.reshape(-1) + return optimized_camera_info diff --git a/sensor/extrinsic_interactive_calibrator/package.xml b/sensor/extrinsic_interactive_calibrator/package.xml index 61b93703..3b39d748 100644 --- a/sensor/extrinsic_interactive_calibrator/package.xml +++ b/sensor/extrinsic_interactive_calibrator/package.xml @@ -7,13 +7,13 @@ 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/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py b/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py index 103f1c56..10a99c0d 100755 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging import signal import sys @@ -38,7 +39,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quitting...", flush=True) + logging.info("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py b/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py index c3d3ab7a..b3f4f0dc 100755 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py +++ b/sensor/extrinsic_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_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py index c40be91c..0b6f30ec 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py +++ b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_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. diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py index 0cdd2340..f18459b7 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py +++ b/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_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. diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py b/sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py index 1226b30a..675e1253 100755 --- a/sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/extrinsic_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,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging import signal import sys @@ -38,7 +39,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quitting...", flush=True) + logging.info("Received sigint. Quitting...") rclpy.shutdown() 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..6ae864f6 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 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..2de50074 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. 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..761ba22c 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. 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..a4136c8e 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. 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..f26dbbea 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. 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..c96d6f14 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. 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/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml index 93cae250..d4d62048 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml @@ -7,6 +7,7 @@ + @@ -29,18 +30,24 @@ - + - + - + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml index ab5f20be..73e7011e 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml @@ -10,6 +10,7 @@ + @@ -48,7 +49,7 @@ - + @@ -59,8 +60,13 @@ - - + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml index fc97be51..f5477d43 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml @@ -10,6 +10,7 @@ + @@ -66,7 +67,7 @@ - + @@ -77,7 +78,13 @@ - + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml index 241994e1..bff28620 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml @@ -10,6 +10,7 @@ + @@ -48,7 +49,7 @@ - + @@ -59,7 +60,13 @@ - + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml index aada10f2..7430cdab 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml +++ b/sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml @@ -1,6 +1,7 @@ + @@ -50,7 +51,7 @@ - + @@ -61,7 +62,13 @@ - + + + + + + + diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py index b989083f..30dbbbd0 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py @@ -1,3 +1,20 @@ +#!/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 @@ -23,7 +40,7 @@ def __init__(self, calibrator_service_wrapper_list: List[CalibratorServiceWrappe calibrator_wrapper.data_changed.connect(self.on_changed) def on_changed(self): - # print(f"CalibratorManagerModel: on_changed", flush=True) + logging.debug("CalibratorManagerModel: on_changed") self.dataChanged.emit( self.createIndex(0, 0), self.createIndex(self.rowCount(0), self.columnCount(0)) ) @@ -33,7 +50,6 @@ def data(self, index, role): value = self.calibrator_service_wrapper_list[ self.index_to_calibrator_index[index.row()] ].get_data(self.index_to_frame_index[index.row()])[index.column()] - # print(f"data: index={index} role={role} value={value}c") return str(value) def rowCount(self, index): diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py index 5ba2e19a..edcf6d63 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py @@ -1,10 +1,23 @@ -# from abc import ABC -# from abc import abstractmethod +#!/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 OrderedDict from collections import defaultdict +import logging from typing import Dict from typing import List @@ -30,7 +43,7 @@ class CalibratorBase(QObject): calibration_finished_signal = Signal() def __init__(self, ros_interface: RosInterface): - print("CalibratorBase: constructor start", flush=True) + logging.debug("CalibratorBase: constructor start") super().__init__() self.ros_interface = ros_interface self.calibrators: List[CalibratorServiceWrapper] = [] @@ -43,14 +56,14 @@ def __init__(self, ros_interface: RosInterface): self.check_tf_timer = QTimer() self.check_tf_timer.timeout.connect(self.on_check_tf_timer) self.check_tf_timer.start(500) - print("CalibratorBase: constructor end", flush=True) + logging.debug("CalibratorBase: constructor end") def init(): - print("CalibratorBase: Calibrator init?", flush=True) + logging.debug("CalibratorBase: Calibrator init?") pass def on_check_tf_timer(self): - print("CalibratorBase: on_check_tf_timer", flush=True) + 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) @@ -61,11 +74,11 @@ def on_check_tf_timer(self): self.state = CalibratorState.WAITING_SERVICES self.state_changed_signal.emit(self.state) self.check_tf_timer.stop() - print("CalibratorBase: on_check_tf_timer stop", flush=True) + 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): - print(f"could not transform {self.required_frames[0]} to {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: @@ -74,7 +87,7 @@ def get_transform_matrix(self, parent: str, child: str) -> np.array: return tf_message_to_transform_matrix(tf_msg) def add_calibrator(self, service_name: str, expected_calibration_frames: List[FramePair]): - print("CalibratorBase: add_calibrator", flush=True) + logging.debug("CalibratorBase: add_calibrator") for pair in expected_calibration_frames: assert ( @@ -101,7 +114,7 @@ def on_service_status_changed(self): self.state_changed_signal.emit(self.state) def on_calibration_result(self): - print("CalibratorBase: on_calibration_result", flush=True) + logging.debug("CalibratorBase: on_calibration_result") for calibrator in self.calibrators: d = calibrator.get_calibration_results() @@ -146,10 +159,10 @@ def pre_process(self): pass def post_process_internal(self): - print("Before post_process") + logging.debug("Before post_process") for parent, children_and_transforms in self.calibration_result_tfs.items(): for child, transform in children_and_transforms.items(): - print(f"{parent}->{child}:\n{transform}") + logging.debug(f"{parent}->{child}:\n{transform}") calibration_transforms = { parent: { @@ -167,10 +180,10 @@ def post_process_internal(self): for parent, children_and_transforms in calibration_transforms.items() } - print("After post_process") + 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(): - print(f"{parent}->{child}:\n{transform}") + logging.debug(f"{parent}->{child}:\n{transform}") def post_process(self, calibration_transforms) -> Dict[str, Dict[str, np.array]]: return calibration_transforms diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py index 715c300b..03a36c98 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py @@ -1,3 +1,19 @@ +#!/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 diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py index 3d13d716..f0064c96 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py @@ -1,4 +1,21 @@ +#!/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 @@ -28,7 +45,6 @@ def __init__( ): super().__init__() - # TODO: Consider making timer to display the elapsed time self.ros_interface = ros_interface self.service_name = service_name self.expected_calibration_frames = expected_calibration_frames @@ -102,18 +118,16 @@ def on_result(self, calibration_results: List[CalibrationResult]): child_frame = calibration_result.transform_stamped.child_frame_id if parent_frame not in self.parents or child_frame not in self.children: - print( - f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers", - flush=True, + 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: - print( - f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers", - flush=True, + logging.error( + f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers" ) continue @@ -135,12 +149,8 @@ def on_result(self, calibration_results: List[CalibrationResult]): self.data_changed.emit() def on_status(self, status: bool): - # print(f"on_status: self.service_status={self.service_status} new_status={status}", flush=True) - # print(f"{threading.get_ident() }: on_status") - if self.service_status != status: self.service_status = status - # print(f"emitting data changed", flush=True) self.data_changed.emit() self.status_changed_signal.emit() @@ -151,7 +161,7 @@ def finished(self): return all(self.finished_list) def result_ros_callback(self, result: NewExtrinsicCalibrator.Response): - print(f"{threading.get_ident() }: result_ros_callback", flush=True) + logging.debug(f"{threading.get_ident() }: result_ros_callback") self.result_signal.emit(result.results) def status_ros_callback(self, status: bool): @@ -161,7 +171,6 @@ def status_ros_callback(self, status: bool): def get_data(self, index) -> list: if not self.service_called: status_message = "Service ready" if self.service_status else "Service not available" - # print(f"getData: status_message={status_message}") else: status_message = self.status_messages[index] diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py index 636fd791..a17c3aa1 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py @@ -1,4 +1,19 @@ -# from typing import Dict +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry @@ -20,8 +35,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.base_frame, self.lidar_frame]) - print("DefaultProject_GroundPlane2DCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py index 363fd96e..8342e20a 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py @@ -1,4 +1,18 @@ -# from typing import Dict +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry @@ -21,8 +35,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.base_frame, self.source_frame, self.target_frame]) - print("DefaultProject_LidarLidar2DCalibrator") - self.add_calibrator( service_name="calibrate_lidar_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py index 622a79de..152b4fa0 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py @@ -1,3 +1,20 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry from new_extrinsic_calibration_manager.ros_interface import RosInterface @@ -18,8 +35,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.mapping_lidar_frame, *self.calibration_lidar_frames]) - print("default_MappingBasedLidarLidarCalibrator") - self.add_calibrator( service_name="calibrate_lidar_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py index 5d138803..89b85c04 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py @@ -1,4 +1,19 @@ -# from typing import Dict +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry @@ -23,8 +38,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.append(self.camera_optical_link_frame) self.required_frames.append(self.lidar_frame) - print("DefaultProject_TagBasedPNPCalibrator") - self.add_calibrator( service_name="calibrate_camera_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py index 9974ac52..e2950041 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry from new_extrinsic_calibration_manager.ros_interface import RosInterface diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py index c1d07ba7..cc2c2325 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry from new_extrinsic_calibration_manager.ros_interface import RosInterface diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py index 6b4f0f62..f39ea3f8 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry from new_extrinsic_calibration_manager.ros_interface import RosInterface diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py index a9348dbe..2dc20d46 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -25,8 +41,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): [self.base_frame, self.sensor_kit_frame, self.mapping_lidar_frame] ) - print("RDV_MappingBasedBaseLidarCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py index fc8949d8..716de366 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -34,8 +50,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("RDV_MappingBasedLidarLidarCalibrator") - self.add_calibrator( service_name="calibrate_lidar_lidar", expected_calibration_frames=[ @@ -45,8 +59,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - print(f"post_process\n{calibration_transforms}") - sensor_kit_to_lidar_transform = self.get_transform_matrix( self.sensor_kit_frame, self.mapping_lidar_frame ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py index 954c1aee..b6f1e38c 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -22,8 +38,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) - print("RDV_MarkerRadarLidarCalibrator") - self.add_calibrator( service_name="calibrate_radar_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py index cb90812a..f8563507 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -20,9 +36,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.append(f"{self.camera_name}/camera_link") self.required_frames.append(f"{self.camera_name}/camera_optical_link") - print("RDV::TagBasedPNPCalibrator") - print(self.camera_name, flush=True) - self.add_calibrator( service_name="calibrate_camera_lidar", expected_calibration_frames=[ @@ -35,28 +48,20 @@ def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): f"{self.camera_name}/camera_optical_link" ]["pandar_top"] - print(f"camera_to_lidar_transform={camera_to_lidar_transform}", flush=True) - sensor_kit_to_lidar_transform = self.get_transform_matrix( "sensor_kit_base_link", "pandar_top" ) - print(f"sensor_kit_to_lidar_transform={sensor_kit_to_lidar_transform}", flush=True) - camera_to_optical_link_transform = self.get_transform_matrix( f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link" ) - print(f"camera_to_optical_link_transform={camera_to_optical_link_transform}", flush=True) - sensor_kit_camera_link_transform = np.linalg.inv( camera_to_optical_link_transform @ camera_to_lidar_transform @ np.linalg.inv(sensor_kit_to_lidar_transform) ) - print(f"sensor_kit_camera_link_transform={sensor_kit_camera_link_transform}", flush=True) - result = { "sensor_kit_base_link": { f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py index 77eb5483..65db531b 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -25,8 +41,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): [self.base_frame, self.sensor_kit_frame, self.main_sensor_frame] ) - print("RDV_tagBasedSfmBaseCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py index d149cd99..258d1920 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -39,8 +55,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("RDV_tagBasedSfmBaseLidarsCalibrator") - self.add_calibrator( service_name="calibrate_base_lidars", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py index 9fc5330d..b7da4732 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -55,8 +71,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("RDV_tagBasedSfmBaseLidarsCamerasCalibrator") - self.add_calibrator( service_name="calibrate_base_lidars_cameras", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py index 0063c128..b41fdb38 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -22,8 +38,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.base_frame, self.sensor_kit_frame, self.lidar_frame]) - print("X1_GroundPlane2DCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py index 14af1df7..6f572250 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -22,8 +38,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.base_frame, self.sensor_kit_frame, self.lidar_frame]) - print("X2_GroundPlane2DCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py index 8cebe83e..bab52adb 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -25,8 +41,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): [self.base_frame, self.top_sensor_kit_frame, self.mapping_lidar_frame] ) - print("X2_MappingBasedBaseLidarCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py index 80a7403d..3c8f2bdf 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 @@ -55,8 +71,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("X2_MappingBasedLidarLidarCalibrator") - self.add_calibrator( service_name="calibrate_lidar_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py index 8d9fad8b..cf62daab 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -22,8 +38,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) - print("X2_MarkerRadarLidarCalibrator") - self.add_calibrator( service_name="calibrate_radar_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py index 53e9a147..baac6069 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -41,8 +57,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("X2::TagBasedPNPCalibrator") - self.add_calibrator( service_name="calibrate_camera_lidar", expected_calibration_frames=[ @@ -55,28 +69,20 @@ def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): f"{self.camera_name}/camera_optical_link" ][self.lidar_frame] - print(f"camera_to_lidar_transform={camera_to_lidar_transform}", flush=True) - sensor_kit_to_lidar_transform = self.get_transform_matrix( self.sensor_kit_frame, self.lidar_frame ) - print(f"sensor_kit_to_lidar_transform={sensor_kit_to_lidar_transform}", flush=True) - camera_to_optical_link_transform = self.get_transform_matrix( f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link" ) - print(f"camera_to_optical_link_transform={camera_to_optical_link_transform}", flush=True) - sensor_kit_camera_link_transform = np.linalg.inv( camera_to_optical_link_transform @ camera_to_lidar_transform @ np.linalg.inv(sensor_kit_to_lidar_transform) ) - print(f"sensor_kit_camera_link_transform={sensor_kit_camera_link_transform}", flush=True) - result = { self.sensor_kit_frame: { f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py index dbaab6b8..f19bb45d 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -23,8 +39,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.base_frame, self.top_unit_frame, self.main_sensor_frame]) - print("RDV_tagBasedSfmBaseCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py index 0072b5b7..c0b2a1d2 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -43,8 +59,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("X2_tagBasedSfmBaseLidarsCalibrator") - self.add_calibrator( service_name="calibrate_base_lidars", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py index 792006e2..fe3a074a 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -1,3 +1,19 @@ +#!/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 @@ -60,8 +76,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("X2_tagBasedSfmBaseLidarsCamerasCalibrator") - self.add_calibrator( service_name="calibrate_base_lidars_cameras", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py index 94e4fae2..8f2bf095 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -22,8 +38,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.extend([self.base_frame, self.sensor_kit_frame, self.lidar_frame]) - print("XX1_GroundPlane2DCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py index 6ed48f3c..8c21e3bf 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -25,8 +41,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): [self.base_frame, self.sensor_kit_frame, self.mapping_lidar_frame] ) - print("XX1_MappingBasedBaseLidarCalibrator") - self.add_calibrator( service_name="calibrate_base_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py index 16c24e46..bfa6408a 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -30,8 +46,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("XX1_MappingBasedLidarLidarCalibrator") - self.add_calibrator( service_name="calibrate_lidar_lidar", expected_calibration_frames=[ @@ -41,8 +55,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - print(f"post_process\n{calibration_transforms}") - sensor_kit_to_lidar_transform = self.get_transform_matrix( self.sensor_kit_frame, self.mapping_lidar_frame ) diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py index a5cf34e2..2747b11a 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -20,9 +36,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.append(f"{self.camera_name}/camera_link") self.required_frames.append(f"{self.camera_name}/camera_optical_link") - print("TagBasedPNPCalibrator") - print(self.camera_name, flush=True) - self.add_calibrator( service_name="calibrate_camera_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py index 3b6075df..0cadd8c5 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py index 24b43449..22fc6ea9 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py @@ -1,3 +1,19 @@ +#!/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 new_extrinsic_calibration_manager.calibrator_base import CalibratorBase @@ -20,9 +36,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): self.required_frames.append(f"{self.camera_name}/camera_link") self.required_frames.append(f"{self.camera_name}/camera_optical_link") - print("TagBasedPNPCalibrator") - print(self.camera_name, flush=True) - self.add_calibrator( service_name="calibrate_camera_lidar", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py index 54268a11..95168db7 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py @@ -1,3 +1,20 @@ +#!/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 @@ -46,8 +63,6 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ] ) - print("RDV_tagBasedSfmBaseLidarsCamerasCalibrator") - self.add_calibrator( service_name="calibrate_base_lidars_cameras", expected_calibration_frames=[ diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py index 0ede87db..68855f80 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py @@ -17,6 +17,7 @@ from collections import defaultdict import copy from functools import partial +import logging import os import signal import subprocess @@ -128,15 +129,13 @@ def __init__(self): self.hide() def on_selected_calibrator(self, project_name, calibrator_name): - print( - f"on_selected_calibrator: project_name={project_name} calibrator_name={calibrator_name}", - flush=True, + 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) ) - pass def launch_calibrators( self, project_name: str, calibrator_name: str, launch_argument_dict: Dict @@ -145,7 +144,6 @@ def launch_calibrators( self.show() # Execute the launcher - print(launch_argument_dict, flush=True) argument_list = [f"{k}:={v}" for k, v in launch_argument_dict.items()] package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") @@ -159,7 +157,7 @@ def launch_calibrators( ) command_list = ["ros2", "launch", launcher_path] + argument_list - print(f"Launching calibrator with the following command: {command_list}", flush=True) + 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) @@ -167,7 +165,7 @@ def launch_calibrators( with open(launcher_path) as f: root_entity, parser = Parser.load(f) except Exception as e: - print("Failed reading xml file. Either not-existent or invalid") + logging.error("Failed reading xml file. Either not-existent or invalid") raise e ld: LaunchDescription = parser.parse_description(root_entity) @@ -178,8 +176,6 @@ def launch_calibrators( if isinstance(e, (DeclareLaunchArgument, SetLaunchConfiguration)): e.visit(context) - print(context.launch_configurations) - # Start the ROS interface self.ros_interface = RosInterface() self.ros_interface.set_tfs_graph_callback(self.tfs_graph_callback) @@ -225,7 +221,7 @@ def on_calibrator_state_changed(self, state: CalibratorState): self.action_button.setEnabled(False) def on_calibration_request(self): - print("on_calibration_request", flush=True) + logging.debug("on_calibration_request") self.calibrator.start_calibration() def on_calibration_finished(self): @@ -265,20 +261,20 @@ def on_calibration_finished(self): ) def on_save_request(self): - print("on_save_request", flush=True) + logging.debug("on_save_request") output_path = QFileDialog.getSaveFileName( None, "Save File", f"{os.getcwd()}/calibration_results.yaml", "YAML files (*.yaml)" ) - print(output_path, flush=True) + logging.debug(output_path) output_path = output_path[0] if output_path is None or output_path == "": - print("Invalid path", flush=True) + logging.error("Invalid path") return - print(f"Saving calibration results to {output_path}") + 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): @@ -298,11 +294,6 @@ def tf_graph_callback2(self, tfs_dict): self.initial_tfs_view.setTfs( self.tfs_dict, required_frames=self.calibrator.required_frames ) - # self.calibration_tfs_view.setTfs(self.tfs_dict) - # self.final_tfs_view.setTfs(self.tfs_dict) - - # self.tf_view.plot.setTfs(self.tfs_msg.transforms) - # print("SECOND", flush=True) def main(args=None): @@ -317,7 +308,7 @@ def main(args=None): sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): - print("Received sigint. Quitting...") + logging.info("Received sigint. Quitting...") rclpy.shutdown() diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py index a6e31dc0..18cf402a 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/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. @@ -72,8 +72,6 @@ def set_tfs_graph_callback(self, callback): self.tfs_graph_ui_callback = callback def tf_callback(self, msg): - # print("TF MSG", flush=True) - for transform in msg.transforms: self.tf_msg[transform.header.frame_id][transform.child_frame_id] = transform.transform diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py index 16de8e97..6f612736 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py @@ -1,3 +1,19 @@ +#!/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 diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py index 5901b64b..9086d6c0 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_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,7 @@ # 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 @@ -89,4 +90,4 @@ def closeEvent(self, event): # self.closed.emit() event.accept() self.deleteLater() - print("Closing calibrator selector view", flush=True) + logging.info("Closing calibrator selector view") diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py index 779404f8..f38ac4d5 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_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,8 +14,8 @@ # 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 @@ -71,13 +71,13 @@ def __init__(self, project_name, calibrator_name): + ".launch.xml" ) - print(f"Reading xml from: {launcher_path}") + 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: - print("Failed reading xml file. Either not-existent or invalid") + logging.error("Failed reading xml file. Either not-existent or invalid") raise e ld: LaunchDescription = parser.parse_description(root_entity) @@ -214,7 +214,7 @@ def check_argument_status(self, text=None): ], ) ) - print("check_argument_status", flush=True) + logging.debug("check_argument_status") def on_click(self): args_dict: Dict[str, str] = { @@ -234,8 +234,6 @@ def is_list(arg: str): ] args_dict[key] = [int(v2) if v2.isnumeric() else v2 for v2 in args_dict[key]] - print(args_dict, flush=True) - self.launcher_parameters.emit(args_dict) self.close() diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py index 2bee20b3..34854afa 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py +++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py @@ -15,6 +15,7 @@ # limitations under the License. from collections import defaultdict +import logging from typing import Dict from typing import List from typing import Optional @@ -216,7 +217,6 @@ def setTfs( xmlreader = QXmlStreamReader(imgdata) self.renderer = QSvgRenderer(xmlreader) self.renderer.setAspectRatioMode(Qt.AspectRatioMode.KeepAspectRatio) - # print(f"bits post savefig= {len(imgdata.getvalue())}", flush=True) def paintEvent(self, event): if self.renderer is None: @@ -227,7 +227,6 @@ def paintEvent(self, event): self.renderer.render(p) p.end() # print(self.geometry()) - # print(f"PAINTED", flush=True) class TfView(QGraphicsView): @@ -257,17 +256,14 @@ def setTfs( 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) - # print(f"=======Tf plot size: {self.plot.size()}", flush=True) def resizeEvent(self, event): - print( - f"PRE resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}", - flush=True, + logging.debug( + f"PRE resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}" ) super().resizeEvent(event) - print( - f"POST resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}", - flush=True, + logging.debug( + f"POST resizeEvent: event.size()={event.size()} event.oldSize()={event.oldSize()}" ) # scaled_pix_size = self.pix.size() From 452737cf521b4b9dd3b24915dd299bdc33afb499 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 02:37:26 +0900 Subject: [PATCH 21/49] chore: deleted the old manager/client and renamed the new calibrators Signed-off-by: Kenzo Lobos-Tsunekawa --- .../CMakeLists.txt | 27 - .../extrinsic_calibration_client/package.xml | 22 - .../src/extrinsic_calibration_client.cpp | 59 -- .../CMakeLists.txt | 15 - .../aip_x1/extrinsic_manual_calibration.rviz | 416 ----------- .../aip_x2/extrinsic_manual_calibration.rviz | 590 --------------- .../aip_xx1/extrinsic_manual_calibration.rviz | 416 ----------- .../config/extrinsic_calibration.rviz | 355 --------- .../x2/extrinsic_interactive_calibrator.rviz | 658 ----------------- .../extrinsic_calibration_manager_node.hpp | 87 --- .../launch/aip_x1/ground_plane.launch.xml | 12 - .../aip_x1/ground_plane_sensor_kit.launch.xml | 74 -- .../aip_x1/lidar_to_lidar_2d.launch.xml | 14 - .../lidar_to_lidar_2d_sensor_kit.launch.xml | 68 -- .../launch/aip_x1/manual.launch.xml | 20 - .../aip_x1/manual_sensor_kit.launch.xml | 51 -- .../launch/aip_x1/manual_sensors.launch.xml | 29 - .../launch/aip_x1/map_based.launch.xml | 37 - .../aip_x1/map_based_sensor_kit.launch.xml | 92 --- .../launch/aip_x1/mapping_based.launch.xml | 14 - .../mapping_based_sensor_kit.launch.xml | 108 --- .../launch/aip_x2/ground_plane.launch.xml | 67 -- .../aip_x2/ground_plane_front_unit.launch.xml | 54 -- .../aip_x2/ground_plane_rear_unit.launch.xml | 49 -- .../aip_x2/ground_plane_top_unit.launch.xml | 46 -- .../launch/aip_x2/interactive.launch.xml | 38 - .../aip_x2/interactive_front_unit.launch.xml | 54 -- .../aip_x2/interactive_rear_unit.launch.xml | 54 -- .../aip_x2/interactive_top_unit.launch.xml | 63 -- .../launch/aip_x2/manual.launch.xml | 34 - .../aip_x2/manual_front_unit.launch.xml | 37 - .../launch/aip_x2/manual_rear_unit.launch.xml | 44 -- .../launch/aip_x2/manual_sensors.launch.xml | 44 -- .../launch/aip_x2/manual_top_unit.launch.xml | 117 --- .../launch/aip_x2/mapping_based.launch.xml | 248 ------- .../launch/aip_x2/reflector_based.launch.xml | 25 - .../reflector_based_front_unit.launch.xml | 45 -- .../reflector_based_rear_unit.launch.xml | 45 -- .../launch/aip_x2/tag_based.launch.xml | 34 - .../launch/aip_x2/tag_based_all.launch.xml | 690 ------------------ .../launch/aip_x2/tag_based_base.launch.xml | 155 ---- .../aip_x2/tag_based_front_unit.launch.xml | 73 -- .../launch/aip_x2/tag_based_lidars.launch.xml | 270 ------- .../aip_x2/tag_based_rear_unit.launch.xml | 73 -- .../aip_x2/tag_based_top_unit.launch.xml | 112 --- .../launch/aip_xx1/ground_plane.launch.xml | 12 - .../aip_xx1/ground_plane_sensors.launch.xml | 68 -- .../launch/aip_xx1/interactive.launch.xml | 16 - .../aip_xx1/interactive_sensor_kit.launch.xml | 76 -- .../launch/aip_xx1/manual.launch.xml | 20 - .../aip_xx1/manual_sensor_kit.launch.xml | 111 --- .../launch/aip_xx1/manual_sensors.launch.xml | 51 -- .../launch/aip_xx1/map_based.launch.xml | 44 -- .../aip_xx1/map_based_sensor_kit.launch.xml | 70 -- .../aip_xx1/map_based_sensors.launch.xml | 49 -- .../launch/aip_xx1/mapping_based.launch.xml | 183 ----- .../launch/aip_xx1/reflector_based.launch.xml | 77 -- .../launch/aip_xx1/tag_based.launch.xml | 16 - .../launch/aip_xx1/tag_based_all.launch.xml | 579 --------------- .../launch/aip_xx1/tag_based_base.launch.xml | 154 ---- .../aip_xx1/tag_based_lidars.launch.xml | 250 ------- .../aip_xx1/tag_based_sensor_kit.launch.xml | 113 --- .../launch/calibration.launch.xml | 147 ---- .../launch/camera_republisher.launch.xml | 12 - .../extrinsic_calibration_manager/package.xml | 39 - .../extrinsic_calibration_manager_node.cpp | 221 ------ .../setup.cfg | 4 - .../CMakeLists.txt | 8 +- .../ground_plane_calibrator.hpp} | 12 +- .../ground_plane_calibrator}/utils.hpp | 6 +- .../launch/calibrator.launch.xml | 4 +- .../package.xml | 4 +- .../rviz/default.rviz | 0 .../src/ground_plane_calibrator.cpp} | 8 +- .../src/main.cpp | 7 +- .../__init__.py | 0 .../calibrator.py | 0 .../image_view.py | 0 .../interactive_calibrator.py | 6 +- .../ros_interface.py | 0 .../utils.py | 0 .../package.xml | 2 +- .../interactive_camera_lidar_calibrator} | 0 .../setup.cfg | 4 + .../setup.py | 4 +- .../test/test_pep257.py | 0 .../intrinsic_camera_calibrator/package.xml | 1 - .../CMakeLists.txt | 8 +- .../lidar_to_lidar_2d_calibrator.hpp} | 10 +- .../launch/calibrator.launch.xml | 4 +- .../package.xml | 4 +- .../rviz/default.rviz | 0 .../src/lidar_to_lidar_2d_calibrator.cpp} | 10 +- .../src/main.cpp | 7 +- .../CMakeLists.txt | 8 +- .../base_lidar_calibrator.hpp | 12 +- .../calibration_mapper.hpp | 8 +- .../camera_calibrator.hpp | 12 +- .../filters/best_frames_filter.hpp | 10 +- .../filters/dynamics_filter.hpp | 10 +- .../filters/filter.hpp | 8 +- .../filters/lost_state_filter.hpp | 10 +- .../filters/object_detection_filter.hpp | 10 +- .../filters/sequential_filter.hpp | 10 +- .../lidar_calibrator.hpp | 12 +- .../mapping_based_calibrator.hpp} | 16 +- .../sensor_calibrator.hpp | 8 +- .../serialization.hpp | 8 +- .../mapping_based_calibrator}/types.hpp | 6 +- .../mapping_based_calibrator}/utils.hpp | 8 +- .../voxel_grid_filter_wrapper.hpp | 6 +- .../launch/calibrator.launch.xml | 4 +- .../package.xml | 4 +- .../rviz/default.rviz | 0 .../src/base_lidar_calibrator.cpp | 6 +- .../src/calibration_mapper.cpp | 6 +- .../src/camera_calibrator.cpp | 12 +- .../src/filters/best_frames_filter.cpp | 2 +- .../src/filters/dynamics_filter.cpp | 2 +- .../src/filters/lost_state_filter.cpp | 2 +- .../src/filters/object_detection_filter.cpp | 4 +- .../src/lidar_calibrator.cpp | 16 +- .../src/main.cpp | 2 +- .../src/mapping_based_calibrator.cpp} | 10 +- .../src/sensor_calibrator.cpp | 4 +- .../src/utils.cpp | 2 +- .../CMakeLists.txt | 8 +- .../marker_radar_lidar_calibrator.hpp} | 14 +- .../marker_radar_lidar_calibrator}/track.hpp | 12 +- .../marker_radar_lidar_calibrator}/types.hpp | 10 +- .../launch/calibrator.launch.xml | 8 +- .../__init__.py | 0 .../calibrator_ui.py | 0 .../ros_interface.py | 2 +- .../package.xml | 4 +- .../rviz/default.rviz | 0 .../scripts/calibrator_ui_node.py | 4 +- .../scripts/metrics_plotter_node.py | 0 .../src/main.cpp | 8 +- .../src/marker_radar_lidar_calibrator.cpp} | 10 +- .../src/track.cpp | 8 +- .../setup.cfg | 4 - .../ground_plane_calibrator.launch.xml | 2 +- .../lidar_lidar_2d_calibrator.launch.xml | 2 +- ...ng_based_lidar_lidar_calibrator.launch.xml | 2 +- .../tag_based_pnp_calibrator.launch.xml | 6 +- ...based_sfm_base_lidar_calibrator.launch.xml | 2 +- ...ased_sfm_base_lidars_calibrator.launch.xml | 2 +- ..._base_lidars_cameras_calibrator.launch.xml | 2 +- ...ing_based_base_lidar_calibrator.launch.xml | 2 +- ...ng_based_lidar_lidar_calibrator.launch.xml | 2 +- .../marker_radar_lidar_calibrator.launch.xml | 2 +- .../rdv/tag_based_pnp_calibrator.launch.xml | 6 +- ...based_sfm_base_lidar_calibrator.launch.xml | 2 +- ...ased_sfm_base_lidars_calibrator.launch.xml | 2 +- ..._base_lidars_cameras_calibrator.launch.xml | 2 +- .../x1/ground_plane_calibrator.launch.xml | 2 +- .../x2/ground_plane_calibrator.launch.xml | 2 +- ...ing_based_base_lidar_calibrator.launch.xml | 2 +- ...ng_based_lidar_lidar_calibrator.launch.xml | 2 +- .../marker_radar_lidar_calibrator.launch.xml | 2 +- .../x2/tag_based_pnp_calibrator.launch.xml | 6 +- ...based_sfm_base_lidar_calibrator.launch.xml | 2 +- ...ased_sfm_base_lidars_calibrator.launch.xml | 2 +- ..._base_lidars_cameras_calibrator.launch.xml | 2 +- .../xx1/ground_plane_calibrator.launch.xml | 2 +- ...ing_based_base_lidar_calibrator.launch.xml | 2 +- ...ng_based_lidar_lidar_calibrator.launch.xml | 2 +- .../xx1/tag_based_pnp_calibrator.launch.xml | 6 +- ...based_sfm_base_lidar_calibrator.launch.xml | 2 +- .../tag_based_pnp_calibrator.launch.xml | 6 +- ..._base_lidars_cameras_calibrator.launch.xml | 2 +- .../package.xml | 10 +- .../resource/sensor_calibration_manager} | 0 .../sensor_calibration_manager/__init__.py} | 0 .../calibration_manager_model.py | 2 +- .../calibrator_base.py | 12 +- .../calibrator_registry.py | 2 +- .../calibrator_wrapper.py | 4 +- .../calibrators/__init__.py | 0 .../calibrators/default_project/__init__.py | 0 .../ground_plane_calibrator.py | 8 +- .../lidar_lidar_2d_calibrator.py | 8 +- .../mapping_based_lidar_lidar_calibrator.py | 8 +- .../tag_based_pnp_calibrator.py | 8 +- .../tag_based_sfm_base_lidar_calibrator.py | 8 +- .../tag_based_sfm_base_lidars_calibrator.py | 8 +- ...ased_sfm_base_lidars_cameras_calibrator.py | 8 +- .../calibrators/rdv/__init__.py | 0 .../mapping_based_base_lidar_calibrator.py | 8 +- .../mapping_based_lidar_lidar_calibrator.py | 8 +- .../rdv/marker_radar_lidar_calibrator.py | 8 +- .../rdv/tag_based_pnp_calibrator.py | 8 +- .../tag_based_sfm_base_lidar_calibrator.py | 8 +- .../tag_based_sfm_base_lidars_calibrator.py | 8 +- ...ased_sfm_base_lidars_cameras_calibrator.py | 8 +- .../calibrators/x1/__init__.py | 0 .../calibrators/x1/ground_plane_calibrator.py | 8 +- .../calibrators/x2/__init__.py | 0 .../calibrators/x2/ground_plane_calibrator.py | 8 +- .../x2/mapping_based_base_lidar_calibrator.py | 8 +- .../mapping_based_lidar_lidar_calibrator.py | 8 +- .../x2/marker_radar_lidar_calibrator.py | 8 +- .../x2/tag_based_pnp_calibrator.py | 8 +- .../x2/tag_based_sfm_base_lidar_calibrator.py | 8 +- .../tag_based_sfm_base_lidars_calibrator.py | 8 +- ...ased_sfm_base_lidars_cameras_calibrator.py | 8 +- .../calibrators/xx1/__init__.py | 0 .../xx1/ground_plane_calibrator.py | 8 +- .../mapping_based_base_lidar_calibrator.py | 8 +- .../mapping_based_lidar_lidar_calibrator.py | 8 +- .../xx1/tag_based_pnp_calibrator.py | 8 +- .../tag_based_sfm_base_lidar_calibrator.py | 8 +- .../calibrators/xx1_15/__init__.py | 0 .../xx1_15/tag_based_pnp_calibrator.py | 8 +- ...ased_sfm_base_lidars_cameras_calibrator.py | 8 +- .../ros_interface.py | 2 +- .../sensor_calibration_manager.py} | 20 +- .../sensor_calibration_manager}/types.py | 0 .../sensor_calibration_manager}/utils.py | 0 .../views/calibrator_selector_view.py | 2 +- .../views/launcher_configuration_view.py | 2 +- .../views/tf_view.py | 0 sensor/sensor_calibration_manager/setup.cfg | 4 + .../setup.py | 8 +- .../test/test_pep257.py | 0 .../sensor_calibration_tools/CMakeLists.txt | 5 + sensor/sensor_calibration_tools/package.xml | 31 + .../CMakeLists.txt | 8 +- .../brute_force_matcher.hpp | 6 +- .../calibration_estimator.hpp | 6 +- .../tag_based_pnp_calibrator.hpp} | 10 +- .../tag_calibrator_visualizer.hpp | 8 +- .../launch/apriltag_16h5.launch.py | 0 .../launch/calibrator.launch.xml | 4 +- .../package.xml | 4 +- .../rviz/default_profile.rviz | 0 .../src/brute_force_matcher.cpp | 2 +- .../src/calibration_estimator.cpp | 4 +- .../src/main.cpp | 2 +- .../src/tag_based_pnp_calibrator.cpp} | 4 +- .../src/tag_calibrator_visualizer.cpp | 2 +- .../CMakeLists.txt | 8 +- .../omiya_calibration_room_2023.param.yaml | 0 .../apriltag_detection.hpp | 12 +- .../apriltag_detector.hpp | 14 +- .../calibration_scene_extractor.hpp | 16 +- .../calibration_types.hpp | 14 +- .../ceres/calibration_problem.hpp | 14 +- .../ceres/camera_residual.hpp | 16 +- .../ceres/lidar_residual.hpp | 16 +- .../ceres/sensor_residual.hpp | 14 +- .../apriltag_calibrator.hpp | 18 +- .../chessboard_calibrator.hpp | 16 +- .../intrinsics_calibrator.hpp | 14 +- .../tag_based_sfm_calibrator}/math.hpp | 16 +- .../tag_based_sfm_calibrator}/scene_types.hpp | 14 +- .../serialization.hpp | 35 +- .../tag_based_sfm_calibrator.hpp} | 22 +- .../tag_based_sfm_calibrator}/types.hpp | 14 +- .../visualization.hpp | 14 +- .../launch/apriltag_detector.launch.py | 0 .../launch/apriltag_detector.launch.xml | 2 +- .../launch/calibrator.launch.xml | 38 +- .../launch/lidartag_detector.launch.xml | 0 .../package.xml | 4 +- .../rviz/default.rviz | 0 .../scripts/calibrator_ui_node.py | 4 +- .../src/apriltag_detection.cpp | 8 +- .../src/apriltag_detector.cpp | 8 +- .../src/calibration_scene_extractor.cpp | 8 +- .../src/ceres/calibration_problem.cpp | 14 +- .../apriltag_calibrator.cpp | 6 +- .../chessboard_calibrator.cpp | 6 +- .../intrinsics_calibrator.cpp | 6 +- .../src/main.cpp | 9 +- .../src/math.cpp | 16 +- .../src/tag_based_sfm_calibrator.cpp} | 22 +- .../src/visualization.cpp | 8 +- .../tag_based_sfm_calibrator}/__init__.py | 0 .../calibrator_ui.py | 0 .../ros_interface.py | 2 +- 282 files changed, 699 insertions(+), 8619 deletions(-) delete mode 100644 sensor/extrinsic_calibration_client/CMakeLists.txt delete mode 100644 sensor/extrinsic_calibration_client/package.xml delete mode 100644 sensor/extrinsic_calibration_client/src/extrinsic_calibration_client.cpp delete mode 100644 sensor/extrinsic_calibration_manager/CMakeLists.txt delete mode 100644 sensor/extrinsic_calibration_manager/config/aip_x1/extrinsic_manual_calibration.rviz delete mode 100644 sensor/extrinsic_calibration_manager/config/aip_x2/extrinsic_manual_calibration.rviz delete mode 100644 sensor/extrinsic_calibration_manager/config/aip_xx1/extrinsic_manual_calibration.rviz delete mode 100644 sensor/extrinsic_calibration_manager/config/extrinsic_calibration.rviz delete mode 100644 sensor/extrinsic_calibration_manager/config/x2/extrinsic_interactive_calibrator.rviz delete mode 100644 sensor/extrinsic_calibration_manager/include/extrinsic_calibration_manager/extrinsic_calibration_manager_node.hpp delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/ground_plane_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/lidar_to_lidar_2d_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/manual.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/manual_sensors.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/map_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/map_based_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x1/mapping_based_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_front_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_rear_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_top_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/interactive.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_front_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_rear_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/interactive_top_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/manual.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/manual_front_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/manual_rear_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/manual_sensors.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/manual_top_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/mapping_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_front_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/reflector_based_rear_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_all.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_base.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_front_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_lidars.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_rear_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/tag_based_top_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensors.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/interactive_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/manual.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/manual_sensors.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/map_based_sensors.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/mapping_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/reflector_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_all.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_base.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_lidars.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/tag_based_sensor_kit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/calibration.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/camera_republisher.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/package.xml delete mode 100644 sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp delete mode 100644 sensor/extrinsic_interactive_calibrator/setup.cfg rename sensor/{extrinsic_ground_plane_calibrator => ground_plane_calibrator}/CMakeLists.txt (61%) rename sensor/{extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/extrinsic_ground_plane_calibrator.hpp => ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp} (94%) rename sensor/{extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator => ground_plane_calibrator/include/ground_plane_calibrator}/utils.hpp (87%) rename sensor/{extrinsic_ground_plane_calibrator => ground_plane_calibrator}/launch/calibrator.launch.xml (93%) rename sensor/{extrinsic_ground_plane_calibrator => ground_plane_calibrator}/package.xml (90%) rename sensor/{extrinsic_ground_plane_calibrator => ground_plane_calibrator}/rviz/default.rviz (100%) rename sensor/{extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp => ground_plane_calibrator/src/ground_plane_calibrator.cpp} (98%) rename sensor/{extrinsic_ground_plane_calibrator => ground_plane_calibrator}/src/main.cpp (75%) rename sensor/{extrinsic_interactive_calibrator/extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator}/__init__.py (100%) rename sensor/{extrinsic_interactive_calibrator/extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator}/calibrator.py (100%) rename sensor/{extrinsic_interactive_calibrator/extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator}/image_view.py (100%) rename sensor/{extrinsic_interactive_calibrator/extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator}/interactive_calibrator.py (98%) rename sensor/{extrinsic_interactive_calibrator/extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator}/ros_interface.py (100%) rename sensor/{extrinsic_interactive_calibrator/extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator}/utils.py (100%) rename sensor/{extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator}/package.xml (94%) rename sensor/{extrinsic_interactive_calibrator/resource/extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator/resource/interactive_camera_lidar_calibrator} (100%) create mode 100644 sensor/interactive_camera_lidar_calibrator/setup.cfg rename sensor/{extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator}/setup.py (79%) rename sensor/{extrinsic_interactive_calibrator => interactive_camera_lidar_calibrator}/test/test_pep257.py (100%) rename sensor/{extrinsic_lidar_to_lidar_2d_calibrator => lidar_to_lidar_2d_calibrator}/CMakeLists.txt (65%) rename sensor/{extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_lidar_to_lidar_2d_calibrator.hpp => lidar_to_lidar_2d_calibrator/include/lidar_to_lidar_2d_calibrator/lidar_to_lidar_2d_calibrator.hpp} (95%) rename sensor/{extrinsic_lidar_to_lidar_2d_calibrator => lidar_to_lidar_2d_calibrator}/launch/calibrator.launch.xml (89%) rename sensor/{extrinsic_lidar_to_lidar_2d_calibrator => lidar_to_lidar_2d_calibrator}/package.xml (88%) rename sensor/{extrinsic_lidar_to_lidar_2d_calibrator => lidar_to_lidar_2d_calibrator}/rviz/default.rviz (100%) rename sensor/{extrinsic_lidar_to_lidar_2d_calibrator/src/extrinsic_lidar_to_lidar_2d_calibrator.cpp => lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp} (98%) rename sensor/{extrinsic_lidar_to_lidar_2d_calibrator => lidar_to_lidar_2d_calibrator}/src/main.cpp (74%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/CMakeLists.txt (84%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/base_lidar_calibrator.hpp (84%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/calibration_mapper.hpp (96%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/camera_calibrator.hpp (88%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/filters/best_frames_filter.hpp (78%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/filters/dynamics_filter.hpp (79%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/filters/filter.hpp (84%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/filters/lost_state_filter.hpp (79%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/filters/object_detection_filter.hpp (86%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/filters/sequential_filter.hpp (85%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/lidar_calibrator.hpp (92%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/extrinsic_mapping_based_calibrator.hpp => mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp} (90%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/sensor_calibrator.hpp (90%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/serialization.hpp (95%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/types.hpp (97%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/utils.hpp (96%) rename sensor/{extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator => mapping_based_calibrator/include/mapping_based_calibrator}/voxel_grid_filter_wrapper.hpp (86%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/launch/calibrator.launch.xml (96%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/package.xml (92%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/rviz/default.rviz (100%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/base_lidar_calibrator.cpp (97%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/calibration_mapper.cpp (99%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/camera_calibrator.cpp (95%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/filters/best_frames_filter.cpp (98%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/filters/dynamics_filter.cpp (98%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/filters/lost_state_filter.cpp (97%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/filters/object_detection_filter.cpp (98%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/lidar_calibrator.cpp (97%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/main.cpp (92%) rename sensor/{extrinsic_mapping_based_calibrator/src/extrinsic_mapping_based_calibrator.cpp => mapping_based_calibrator/src/mapping_based_calibrator.cpp} (99%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/sensor_calibrator.cpp (96%) rename sensor/{extrinsic_mapping_based_calibrator => mapping_based_calibrator}/src/utils.cpp (99%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator}/CMakeLists.txt (70%) rename sensor/{extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp => marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp} (95%) rename sensor/{extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator}/track.hpp (88%) rename sensor/{extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator}/types.hpp (84%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator}/launch/calibrator.launch.xml (85%) rename sensor/{extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator/marker_radar_lidar_calibrator}/__init__.py (100%) rename sensor/{extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator/marker_radar_lidar_calibrator}/calibrator_ui.py (100%) rename sensor/{extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator/marker_radar_lidar_calibrator}/ros_interface.py (98%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator}/package.xml (89%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator}/rviz/default.rviz (100%) rename sensor/{extrinsic_tag_based_sfm_calibrator => marker_radar_lidar_calibrator}/scripts/calibrator_ui_node.py (91%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator}/scripts/metrics_plotter_node.py (100%) rename sensor/{extrinsic_tag_based_sfm_calibrator => marker_radar_lidar_calibrator}/src/main.cpp (75%) rename sensor/{extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp => marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp} (99%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => marker_radar_lidar_calibrator}/src/track.cpp (96%) delete mode 100644 sensor/new_extrinsic_calibration_manager/setup.cfg rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/default_project/ground_plane_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/default_project/lidar_lidar_2d_calibrator.launch.xml (95%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml (95%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/default_project/tag_based_pnp_calibrator.launch.xml (89%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml (95%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml (98%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml (96%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/rdv/marker_radar_lidar_calibrator.launch.xml (92%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/rdv/tag_based_pnp_calibrator.launch.xml (90%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml (95%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml (98%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x1/ground_plane_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/ground_plane_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/mapping_based_base_lidar_calibrator.launch.xml (98%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/marker_radar_lidar_calibrator.launch.xml (96%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/tag_based_pnp_calibrator.launch.xml (93%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml (95%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml (98%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/xx1/ground_plane_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml (97%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml (95%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/xx1/tag_based_pnp_calibrator.launch.xml (90%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml (95%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/xx1_15/tag_based_pnp_calibrator.launch.xml (92%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml (98%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/package.xml (62%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/__init__.py => sensor_calibration_manager/resource/sensor_calibration_manager} (100%) rename sensor/{new_extrinsic_calibration_manager/resource/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager/__init__.py} (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibration_manager_model.py (96%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrator_base.py (94%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrator_registry.py (97%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrator_wrapper.py (97%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/__init__.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/__init__.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/ground_plane_calibrator.py (81%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/lidar_lidar_2d_calibrator.py (82%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py (83%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/tag_based_pnp_calibrator.py (82%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py (81%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py (86%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py (89%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/__init__.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/mapping_based_base_lidar_calibrator.py (87%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py (91%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/marker_radar_lidar_calibrator.py (86%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/tag_based_pnp_calibrator.py (88%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py (87%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py (92%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py (94%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x1/__init__.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x1/ground_plane_calibrator.py (86%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/__init__.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/ground_plane_calibrator.py (86%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/mapping_based_base_lidar_calibrator.py (87%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/mapping_based_lidar_lidar_calibrator.py (95%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/marker_radar_lidar_calibrator.py (86%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/tag_based_pnp_calibrator.py (90%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py (87%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py (93%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py (95%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1/__init__.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1/ground_plane_calibrator.py (86%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1/mapping_based_base_lidar_calibrator.py (87%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py (91%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1/tag_based_pnp_calibrator.py (88%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py (87%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1_15/__init__.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1_15/tag_based_pnp_calibrator.py (88%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py (93%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/ros_interface.py (98%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py => sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py} (93%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/types.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/utils.py (100%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/views/calibrator_selector_view.py (97%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/views/launcher_configuration_view.py (98%) rename sensor/{new_extrinsic_calibration_manager/new_extrinsic_calibration_manager => sensor_calibration_manager/sensor_calibration_manager}/views/tf_view.py (100%) create mode 100644 sensor/sensor_calibration_manager/setup.cfg rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/setup.py (82%) rename sensor/{new_extrinsic_calibration_manager => sensor_calibration_manager}/test/test_pep257.py (100%) create mode 100644 sensor/sensor_calibration_tools/CMakeLists.txt create mode 100644 sensor/sensor_calibration_tools/package.xml rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/CMakeLists.txt (65%) rename sensor/{extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator/include/tag_based_pnp_calibrator}/brute_force_matcher.hpp (86%) rename sensor/{extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator/include/tag_based_pnp_calibrator}/calibration_estimator.hpp (96%) rename sensor/{extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp => tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp} (92%) rename sensor/{extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator/include/tag_based_pnp_calibrator}/tag_calibrator_visualizer.hpp (93%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/launch/apriltag_16h5.launch.py (100%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/launch/calibrator.launch.xml (93%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/package.xml (90%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/rviz/default_profile.rviz (100%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/src/brute_force_matcher.cpp (99%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/src/calibration_estimator.cpp (99%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/src/main.cpp (92%) rename sensor/{extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp => tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp} (99%) rename sensor/{extrinsic_tag_based_pnp_calibrator => tag_based_pnp_calibrator}/src/tag_calibrator_visualizer.cpp (99%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/CMakeLists.txt (85%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/config/omiya_calibration_room_2023.param.yaml (100%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/apriltag_detection.hpp (89%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/apriltag_detector.hpp (85%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/calibration_scene_extractor.hpp (86%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/calibration_types.hpp (88%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/ceres/calibration_problem.hpp (95%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/ceres/camera_residual.hpp (97%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/ceres/lidar_residual.hpp (95%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/ceres/sensor_residual.hpp (86%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/intrinsics_calibration/apriltag_calibrator.hpp (67%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/intrinsics_calibration/chessboard_calibrator.hpp (67%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/intrinsics_calibration/intrinsics_calibrator.hpp (79%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/math.hpp (88%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/scene_types.hpp (80%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/serialization.hpp (84%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp} (95%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/types.hpp (94%) rename sensor/{extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/include/tag_based_sfm_calibrator}/visualization.hpp (92%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/launch/apriltag_detector.launch.py (100%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/launch/apriltag_detector.launch.xml (94%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/launch/calibrator.launch.xml (85%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/launch/lidartag_detector.launch.xml (100%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/package.xml (92%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/rviz/default.rviz (100%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => tag_based_sfm_calibrator}/scripts/calibrator_ui_node.py (90%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/apriltag_detection.cpp (98%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/apriltag_detector.cpp (98%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/calibration_scene_extractor.cpp (96%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/ceres/calibration_problem.cpp (99%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/intrinsics_calibration/apriltag_calibrator.cpp (96%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/intrinsics_calibration/chessboard_calibrator.cpp (95%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/intrinsics_calibration/intrinsics_calibrator.cpp (94%) rename sensor/{extrinsic_marker_radar_lidar_calibrator => tag_based_sfm_calibrator}/src/main.cpp (73%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/math.cpp (97%) rename sensor/{extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp => tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp} (98%) rename sensor/{extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator}/src/visualization.cpp (98%) rename sensor/{extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/tag_based_sfm_calibrator}/__init__.py (100%) rename sensor/{extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/tag_based_sfm_calibrator}/calibrator_ui.py (100%) rename sensor/{extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator => tag_based_sfm_calibrator/tag_based_sfm_calibrator}/ros_interface.py (99%) 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/config/x2/extrinsic_interactive_calibrator.rviz b/sensor/extrinsic_calibration_manager/config/x2/extrinsic_interactive_calibrator.rviz deleted file mode 100644 index 8e375c55..00000000 --- a/sensor/extrinsic_calibration_manager/config/x2/extrinsic_interactive_calibrator.rviz +++ /dev/null @@ -1,658 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /concatenated_pointcloud1/Topic1 - Splitter Ratio: 0.7346938848495483 - 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: concatenated_pointcloud -Visualization Manager: - Class: "" - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: concatenated_pointcloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: front_lower - 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: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -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: 255 - Min Color: 0; 0; 0 - Min Intensity: 10 - Name: front_upper - 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_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 - 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.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_lower/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 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: left_upper - 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: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -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: 255 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: rear_lower - 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: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -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: 255 - Min Color: 0; 0; 0 - Min Intensity: 10 - Name: rear_upper - 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_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 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 9 - Name: right_lower - 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_lower/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 - 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 - 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 - 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 - 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 - 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: republished_pointcloud_camera1 - 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: /top_unit/top_unit_base_link/camera1/camera_link/calibration/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: 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 - 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 - 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: republished_pointcloud_camera3 - 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: /top_unit/rear_unit_base_link/camera3/camera_link/calibration/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: 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 - 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 - 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: republished_pointcloud_camera5 - 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: /top_unit/top_unit_base_link/camera5/camera_link/calibration/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: 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 - 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 - 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: 1.5697963237762451 - Position: - X: 5 - Y: 0 - Z: 30 - Target Frame: - Value: FPS (rviz_default_plugins) - Yaw: 0 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001870000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004960000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - 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 1e206633..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/mapping_based.launch.xml +++ /dev/null @@ -1,183 +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_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_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 94% 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 f5d51711..f8aa59bb 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 @@ -12,11 +12,11 @@ // 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_ #include -#include +#include #include #include #include @@ -43,7 +43,7 @@ #include #include -namespace extrinsic_ground_plane_calibrator +namespace ground_plane_calibrator { using PointType = pcl::PointXYZ; @@ -193,6 +193,6 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node std::vector outlier_models_; }; -} // namespace extrinsic_ground_plane_calibrator +} // namespace ground_plane_calibrator -#endif // EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ +#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 87% 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..bb92db14 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 @@ -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 93% rename from sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml rename to sensor/ground_plane_calibrator/launch/calibrator.launch.xml index efaddcce..4d5bbf87 100644 --- a/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml +++ b/sensor/ground_plane_calibrator/launch/calibrator.launch.xml @@ -7,7 +7,7 @@ - + @@ -32,7 +32,7 @@ - + diff --git a/sensor/extrinsic_ground_plane_calibrator/package.xml b/sensor/ground_plane_calibrator/package.xml similarity index 90% rename from sensor/extrinsic_ground_plane_calibrator/package.xml rename to sensor/ground_plane_calibrator/package.xml index 2ce7a073..65738128 100644 --- a/sensor/extrinsic_ground_plane_calibrator/package.xml +++ b/sensor/ground_plane_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_ground_plane_calibrator + ground_plane_calibrator 0.0.1 - The extrinsic_ground_plane_calibrator package + The ground_plane_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_ground_plane_calibrator/rviz/default.rviz b/sensor/ground_plane_calibrator/rviz/default.rviz similarity index 100% rename from sensor/extrinsic_ground_plane_calibrator/rviz/default.rviz rename to sensor/ground_plane_calibrator/rviz/default.rviz diff --git a/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp similarity index 98% rename from sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp rename to sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp index d4b8a305..e0df215b 100644 --- a/sensor/extrinsic_ground_plane_calibrator/src/extrinsic_ground_plane_calibrator.cpp +++ b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -20,11 +20,11 @@ #include -namespace extrinsic_ground_plane_calibrator +namespace ground_plane_calibrator { ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::NodeOptions & options) -: Node("extrinsic_ground_plane_calibrator_node", options), tf_broadcaster_(this) +: Node("ground_plane_calibrator_node", options), tf_broadcaster_(this) { tf_buffer_ = std::make_shared(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); @@ -533,4 +533,4 @@ void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_pl tf_broadcaster_.sendTransform(lidar_to_calibrated_base_transform_msg); } -} // namespace extrinsic_ground_plane_calibrator +} // 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 8a16e5da..08af84a5 100644 --- a/sensor/extrinsic_ground_plane_calibrator/src/main.cpp +++ b/sensor/ground_plane_calibrator/src/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -23,9 +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_interactive_calibrator/extrinsic_interactive_calibrator/__init__.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/__init__.py similarity index 100% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/__init__.py rename to sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/__init__.py 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 100% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/calibrator.py rename to sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/image_view.py similarity index 100% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/image_view.py rename to sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/image_view.py 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 98% 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 d17285fd..57fb3eac 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 @@ -35,9 +35,9 @@ 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.ros_interface import InteractiveCalibratorRosInterface -from extrinsic_interactive_calibrator.utils import camera_lidar_calibrate_intrinsics +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 diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py similarity index 100% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/ros_interface.py rename to sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/ros_interface.py diff --git a/sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/utils.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/utils.py similarity index 100% rename from sensor/extrinsic_interactive_calibrator/extrinsic_interactive_calibrator/utils.py rename to sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/utils.py diff --git a/sensor/extrinsic_interactive_calibrator/package.xml b/sensor/interactive_camera_lidar_calibrator/package.xml similarity index 94% rename from sensor/extrinsic_interactive_calibrator/package.xml rename to sensor/interactive_camera_lidar_calibrator/package.xml index 3b39d748..3e381a86 100644 --- a/sensor/extrinsic_interactive_calibrator/package.xml +++ b/sensor/interactive_camera_lidar_calibrator/package.xml @@ -1,7 +1,7 @@ - extrinsic_interactive_calibrator + interactive_camera_lidar_calibrator 0.0.0 TODO: Package description Kenzo Lobos Tsunekawa diff --git a/sensor/extrinsic_interactive_calibrator/resource/extrinsic_interactive_calibrator b/sensor/interactive_camera_lidar_calibrator/resource/interactive_camera_lidar_calibrator similarity index 100% rename from sensor/extrinsic_interactive_calibrator/resource/extrinsic_interactive_calibrator rename to sensor/interactive_camera_lidar_calibrator/resource/interactive_camera_lidar_calibrator 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/extrinsic_interactive_calibrator/test/test_pep257.py b/sensor/interactive_camera_lidar_calibrator/test/test_pep257.py similarity index 100% rename from sensor/extrinsic_interactive_calibrator/test/test_pep257.py rename to sensor/interactive_camera_lidar_calibrator/test/test_pep257.py 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/extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_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 similarity index 95% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_lidar_to_lidar_2d_calibrator.hpp rename to sensor/lidar_to_lidar_2d_calibrator/include/lidar_to_lidar_2d_calibrator/lidar_to_lidar_2d_calibrator.hpp index 737340dc..365d429a 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/include/extrinsic_lidar_to_lidar_2d_calibrator/extrinsic_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 @@ -12,8 +12,8 @@ // 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_ +#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 @@ -44,7 +44,7 @@ #include #include -namespace extrinsic_lidar_to_lidar_2d_calibrator +namespace lidar_to_lidar_2d_calibrator { using PointType = pcl::PointXYZ; @@ -209,6 +209,6 @@ class LidarToLidar2DCalibrator : public rclcpp::Node bool first_observation_{true}; }; -} // namespace extrinsic_lidar_to_lidar_2d_calibrator +} // namespace lidar_to_lidar_2d_calibrator -#endif // EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR__EXTRINSIC_LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ +#endif // LIDAR_TO_LIDAR_2D_CALIBRATOR__LIDAR_TO_LIDAR_2D_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml b/sensor/lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml similarity index 89% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml rename to sensor/lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml index fad1332c..f82cf848 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml +++ b/sensor/lidar_to_lidar_2d_calibrator/launch/calibrator.launch.xml @@ -21,11 +21,11 @@ - + - + diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/package.xml b/sensor/lidar_to_lidar_2d_calibrator/package.xml similarity index 88% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/package.xml rename to sensor/lidar_to_lidar_2d_calibrator/package.xml index b2dfffe4..b94aedcd 100755 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/package.xml +++ b/sensor/lidar_to_lidar_2d_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_lidar_to_lidar_2d_calibrator + lidar_to_lidar_2d_calibrator 0.0.1 - The extrinsic_lidar_to_lidar_2d_calibrator package + The lidar_to_lidar_2d_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/default.rviz b/sensor/lidar_to_lidar_2d_calibrator/rviz/default.rviz similarity index 100% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/rviz/default.rviz rename to sensor/lidar_to_lidar_2d_calibrator/rviz/default.rviz 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 similarity index 98% 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 f47d2b2f..14f4b803 100644 --- 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 @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -31,11 +31,11 @@ #include #include -namespace extrinsic_lidar_to_lidar_2d_calibrator +namespace lidar_to_lidar_2d_calibrator { LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & options) -: Node("extrinsic_lidar_to_lidar_2d_calibrator", options), tf_broadcaster_(this) +: Node("lidar_to_lidar_2d_calibrator", options), tf_broadcaster_(this) { using std::chrono_literals::operator""ms; @@ -523,6 +523,6 @@ LidarToLidar2DCalibrator::findBestTransform( } return std::make_tuple<>(best_aligned_pointcloud_ptr, best_transform, best_score); -} // extrinsic_lidar_to_lidar_2d_calibrator +} // lidar_to_lidar_2d_calibrator -} // namespace extrinsic_lidar_to_lidar_2d_calibrator +} // namespace lidar_to_lidar_2d_calibrator diff --git a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp b/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp similarity index 74% rename from sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp rename to sensor/lidar_to_lidar_2d_calibrator/src/main.cpp index d956e1f3..258b1290 100644 --- a/sensor/extrinsic_lidar_to_lidar_2d_calibrator/src/main.cpp +++ b/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -23,9 +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_mapping_based_calibrator/CMakeLists.txt b/sensor/mapping_based_calibrator/CMakeLists.txt similarity index 84% rename from sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt rename to sensor/mapping_based_calibrator/CMakeLists.txt index 3ce411e4..cb61be46 100644 --- a/sensor/extrinsic_mapping_based_calibrator/CMakeLists.txt +++ b/sensor/mapping_based_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_mapping_based_calibrator) +project(mapping_based_calibrator) #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") @@ -26,13 +26,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 @@ -41,7 +41,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/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/base_lidar_calibrator.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/base_lidar_calibrator.hpp similarity index 84% rename from sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/base_lidar_calibrator.hpp rename to sensor/mapping_based_calibrator/include/mapping_based_calibrator/base_lidar_calibrator.hpp index d66cf2e2..7056d844 100644 --- a/sensor/extrinsic_mapping_based_calibrator/include/extrinsic_mapping_based_calibrator/base_lidar_calibrator.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/base_lidar_calibrator.hpp @@ -12,12 +12,12 @@ // 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_ +#ifndef MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ +#define MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -67,4 +67,4 @@ class BaseLidarCalibrator : public SensorCalibrator PointPublisher::SharedPtr ground_pointcloud_pub_; }; -#endif // EXTRINSIC_MAPPING_BASED_CALIBRATOR__BASE_LIDAR_CALIBRATOR_HPP_ +#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 96% 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 58a84acf..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 @@ -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 @@ -237,4 +237,4 @@ class CalibrationMapper 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 88% 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 397e195d..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 @@ -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 @@ -88,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 78% 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 f82a7c16..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 @@ -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 79% 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 ca373855..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 @@ -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 84% 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 e7cca00e..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 @@ -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 79% 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 1c35a087..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 @@ -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 86% 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 adcb0164..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 @@ -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 85% 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 dfa45f3a..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 @@ -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 92% 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 d9f27277..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 @@ -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 @@ -123,4 +123,4 @@ class LidarCalibrator : public SensorCalibrator 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 90% 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 832877f9..b7625446 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 @@ -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 @@ -149,4 +149,4 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node 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 90% 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 463a6a32..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 @@ -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 @@ -73,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 95% 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 3f865187..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 @@ -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 @@ -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 97% 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 9e8de59d..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 @@ -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 @@ -242,4 +242,4 @@ struct CalibrationParameters 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 96% 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 0bfea953..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 @@ -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 @@ -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 86% 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 e0645cae..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 @@ -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 96% rename from sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml rename to sensor/mapping_based_calibrator/launch/calibrator.launch.xml index feef894e..126498ff 100644 --- a/sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml +++ b/sensor/mapping_based_calibrator/launch/calibrator.launch.xml @@ -2,7 +2,7 @@ - + @@ -66,7 +66,7 @@ - + diff --git a/sensor/extrinsic_mapping_based_calibrator/package.xml b/sensor/mapping_based_calibrator/package.xml similarity index 92% rename from sensor/extrinsic_mapping_based_calibrator/package.xml rename to sensor/mapping_based_calibrator/package.xml index 8ae3cf4e..9dbb03da 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 diff --git a/sensor/extrinsic_mapping_based_calibrator/rviz/default.rviz b/sensor/mapping_based_calibrator/rviz/default.rviz similarity index 100% rename from sensor/extrinsic_mapping_based_calibrator/rviz/default.rviz rename to sensor/mapping_based_calibrator/rviz/default.rviz diff --git a/sensor/extrinsic_mapping_based_calibrator/src/base_lidar_calibrator.cpp b/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp similarity index 97% rename from sensor/extrinsic_mapping_based_calibrator/src/base_lidar_calibrator.cpp rename to sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp index 4d076448..66d1a27e 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/base_lidar_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include +#include #include #include diff --git a/sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp b/sensor/mapping_based_calibrator/src/calibration_mapper.cpp similarity index 99% rename from sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp rename to sensor/mapping_based_calibrator/src/calibration_mapper.cpp index f68580fd..ff4f5b98 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/calibration_mapper.cpp +++ b/sensor/mapping_based_calibrator/src/calibration_mapper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include +#include #include #include diff --git a/sensor/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp b/sensor/mapping_based_calibrator/src/camera_calibrator.cpp similarity index 95% rename from sensor/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp rename to sensor/mapping_based_calibrator/src/camera_calibrator.cpp index 405a520c..a554852d 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/camera_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/camera_calibrator.cpp @@ -12,12 +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 +#include #include #include #include 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 98% 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..6e1c6704 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/best_frames_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/best_frames_filter.cpp @@ -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 98% rename from sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp rename to sensor/mapping_based_calibrator/src/filters/dynamics_filter.cpp index 3ca31cac..e1419555 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/dynamics_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/dynamics_filter.cpp @@ -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; 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..0765f335 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/lost_state_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/lost_state_filter.cpp @@ -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 98% 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 63b36aa9..0d7abc4d 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/filters/object_detection_filter.cpp +++ b/sensor/mapping_based_calibrator/src/filters/object_detection_filter.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include diff --git a/sensor/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp b/sensor/mapping_based_calibrator/src/lidar_calibrator.cpp similarity index 97% rename from sensor/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp rename to sensor/mapping_based_calibrator/src/lidar_calibrator.cpp index 065c4c46..08e64e4f 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/lidar_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/lidar_calibrator.cpp @@ -12,14 +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 #include #include diff --git a/sensor/extrinsic_mapping_based_calibrator/src/main.cpp b/sensor/mapping_based_calibrator/src/main.cpp similarity index 92% rename from sensor/extrinsic_mapping_based_calibrator/src/main.cpp rename to sensor/mapping_based_calibrator/src/main.cpp index c3f3dd88..8d81a21a 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/main.cpp +++ b/sensor/mapping_based_calibrator/src/main.cpp @@ -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 99% 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 e164217a..07293c2a 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/extrinsic_mapping_based_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include +#include #include #include #include @@ -43,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); } } @@ -51,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_)), diff --git a/sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp b/sensor/mapping_based_calibrator/src/sensor_calibrator.cpp similarity index 96% rename from sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp rename to sensor/mapping_based_calibrator/src/sensor_calibrator.cpp index fb8705ef..13f6ae63 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/sensor_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/sensor_calibrator.cpp @@ -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 99% rename from sensor/extrinsic_mapping_based_calibrator/src/utils.cpp rename to sensor/mapping_based_calibrator/src/utils.cpp index 5d94a0ee..9a3fd26a 100644 --- a/sensor/extrinsic_mapping_based_calibrator/src/utils.cpp +++ b/sensor/mapping_based_calibrator/src/utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt b/sensor/marker_radar_lidar_calibrator/CMakeLists.txt similarity index 70% rename from sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt rename to sensor/marker_radar_lidar_calibrator/CMakeLists.txt index d4df8e9a..abb003dd 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/CMakeLists.txt +++ b/sensor/marker_radar_lidar_calibrator/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(extrinsic_marker_radar_lidar_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_marker_radar_lidar_calibrator - src/extrinsic_marker_radar_lidar_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_marker_radar_lidar_calibrator +target_link_libraries(marker_radar_lidar_calibrator ${OpenCV_LIBS} ) diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp similarity index 95% rename from sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp rename to sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp index 66fda229..d0a02b58 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ -#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_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 @@ -50,7 +50,7 @@ #include #include -namespace extrinsic_marker_radar_lidar_calibrator +namespace marker_radar_lidar_calibrator { class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node @@ -269,6 +269,6 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node static constexpr int MARKER_SIZE_PER_TRACK = 8; }; -} // namespace extrinsic_marker_radar_lidar_calibrator +} // namespace marker_radar_lidar_calibrator -#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ +#endif // MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/track.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp similarity index 88% rename from sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/track.hpp rename to sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp index 18c66b01..d6a7a968 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/track.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ -#define EXTRINSIC_MARKER_RADAR_LIDAR_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 extrinsic_marker_radar_lidar_calibrator +namespace marker_radar_lidar_calibrator { class Track @@ -84,6 +84,6 @@ class TrackFactory double max_matching_distance_; }; -} // namespace extrinsic_marker_radar_lidar_calibrator +} // namespace marker_radar_lidar_calibrator -#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ +#endif // MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/types.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp similarity index 84% rename from sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/types.hpp rename to sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp index b8d755ad..a5700ad1 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/include/extrinsic_marker_radar_lidar_calibrator/types.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ -#define EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +#define MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ #include @@ -25,7 +25,7 @@ #include #include -namespace extrinsic_marker_radar_lidar_calibrator +namespace marker_radar_lidar_calibrator { struct BackgroundModel @@ -57,6 +57,6 @@ struct BackgroundModel TreeType tree_; }; -} // namespace extrinsic_marker_radar_lidar_calibrator +} // namespace marker_radar_lidar_calibrator -#endif // EXTRINSIC_MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +#endif // MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml b/sensor/marker_radar_lidar_calibrator/launch/calibrator.launch.xml similarity index 85% rename from sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml rename to sensor/marker_radar_lidar_calibrator/launch/calibrator.launch.xml index 30f050d2..f6b11b6a 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml +++ b/sensor/marker_radar_lidar_calibrator/launch/calibrator.launch.xml @@ -3,7 +3,7 @@ - + @@ -28,7 +28,7 @@ - + @@ -54,8 +54,8 @@ - - + + diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/__init__.py b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/__init__.py similarity index 100% rename from sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/__init__.py rename to sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/__init__.py diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/calibrator_ui.py b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py similarity index 100% rename from sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/calibrator_ui.py rename to sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py similarity index 98% rename from sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py rename to sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py index 57982519..e736c6a2 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py +++ b/sensor/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py @@ -59,7 +59,7 @@ def __call__(self): class RosInterface(Node): def __init__(self): - super().__init__("extrinsic_marker_radar_lidar_calibrator_ui") + super().__init__("marker_radar_lidar_calibrator_ui") self.ros_context = None self.ros_executor = None diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/package.xml b/sensor/marker_radar_lidar_calibrator/package.xml similarity index 89% rename from sensor/extrinsic_marker_radar_lidar_calibrator/package.xml rename to sensor/marker_radar_lidar_calibrator/package.xml index 84e838f5..3c4059b9 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/package.xml +++ b/sensor/marker_radar_lidar_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_marker_radar_lidar_calibrator + marker_radar_lidar_calibrator 0.0.1 - The extrinsic_marker_radar_lidar_calibrator package + The marker_radar_lidar_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz b/sensor/marker_radar_lidar_calibrator/rviz/default.rviz similarity index 100% rename from sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz rename to sensor/marker_radar_lidar_calibrator/rviz/default.rviz diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py b/sensor/marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py similarity index 91% rename from sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py rename to sensor/marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py index 675e1253..7fb8c626 100755 --- a/sensor/extrinsic_tag_based_sfm_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py @@ -19,8 +19,8 @@ import sys from PySide2.QtWidgets import QApplication -from extrinsic_tag_based_sfm_calibrator import CalibratorUI -from extrinsic_tag_based_sfm_calibrator import RosInterface +from marker_radar_lidar_calibrator import CalibratorUI +from marker_radar_lidar_calibrator import RosInterface import rclpy diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py b/sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py similarity index 100% rename from sensor/extrinsic_marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py rename to sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/main.cpp b/sensor/marker_radar_lidar_calibrator/src/main.cpp similarity index 75% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/main.cpp rename to sensor/marker_radar_lidar_calibrator/src/main.cpp index 9b467983..65ef82a9 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/main.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/main.cpp @@ -12,19 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#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( + std::shared_ptr node = + std::make_shared( node_options); executor.add_node(node); executor.spin(); diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp similarity index 99% rename from sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp rename to sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index 9490c45f..27dbb422 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -50,13 +50,13 @@ void update_param( if (it != parameters.cend()) { value = it->template get_value(); RCLCPP_INFO_STREAM( - rclcpp::get_logger("extrinsic_marker_radar_lidar_calibrator"), + rclcpp::get_logger("marker_radar_lidar_calibrator"), "Setting parameter [" << name << "] to " << value); } } } // namespace -namespace extrinsic_marker_radar_lidar_calibrator +namespace marker_radar_lidar_calibrator { rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::paramCallback( @@ -118,7 +118,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( const rclcpp::NodeOptions & options) -: Node("extrinsic_marker_radar_lidar_calibrator_node", options), tf_broadcaster_(this) +: 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_); @@ -1816,4 +1816,4 @@ double ExtrinsicReflectorBasedCalibrator::getYawError( return std::abs(std::acos(v1.dot(v2) / (v1.norm() * v2.norm()))); } -} // namespace extrinsic_marker_radar_lidar_calibrator +} // namespace marker_radar_lidar_calibrator diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/src/track.cpp b/sensor/marker_radar_lidar_calibrator/src/track.cpp similarity index 96% rename from sensor/extrinsic_marker_radar_lidar_calibrator/src/track.cpp rename to sensor/marker_radar_lidar_calibrator/src/track.cpp index c7cc8b7e..f2ce0a62 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/src/track.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/track.cpp @@ -13,11 +13,11 @@ // limitations under the License. #include -#include -#include #include +#include +#include -namespace extrinsic_marker_radar_lidar_calibrator +namespace marker_radar_lidar_calibrator { Track::Track( @@ -155,4 +155,4 @@ Track TrackFactory::makeTrack( timeout_thresh_, max_matching_distance_); } -} // namespace extrinsic_marker_radar_lidar_calibrator +} // namespace marker_radar_lidar_calibrator diff --git a/sensor/new_extrinsic_calibration_manager/setup.cfg b/sensor/new_extrinsic_calibration_manager/setup.cfg deleted file mode 100644 index e4a17e51..00000000 --- a/sensor/new_extrinsic_calibration_manager/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/new_extrinsic_calibration_manager -[install] -install_scripts=$base/lib/new_extrinsic_calibration_manager diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml index a46f4b69..4990e9da 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/default_project/ground_plane_calibrator.launch.xml @@ -25,7 +25,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/default_project/lidar_lidar_2d_calibrator.launch.xml index f29ea9e5..4ffc7fe5 100644 --- a/sensor/new_extrinsic_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 @@ -24,7 +24,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/default_project/mapping_based_lidar_lidar_calibrator.launch.xml index 639c8cb6..91e08665 100644 --- a/sensor/new_extrinsic_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 @@ -26,7 +26,7 @@ '']"/> - + diff --git a/sensor/new_extrinsic_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 similarity index 89% rename from sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml index d4d62048..97a99e41 100644 --- a/sensor/new_extrinsic_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 @@ -10,7 +10,7 @@ - + @@ -19,7 +19,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml index f45373a5..06ceaae5 100644 --- a/sensor/new_extrinsic_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 @@ -30,7 +30,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_calibrator.launch.xml index bdf94703..ad4e2cd4 100644 --- a/sensor/new_extrinsic_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 @@ -41,7 +41,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 98% rename from sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/default_project/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml index c8267c3d..f7426e64 100644 --- a/sensor/new_extrinsic_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 @@ -65,7 +65,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/rdv/mapping_based_base_lidar_calibrator.launch.xml index 6993a975..18472445 100644 --- a/sensor/new_extrinsic_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 @@ -40,7 +40,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 96% rename from sensor/new_extrinsic_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/rdv/mapping_based_lidar_lidar_calibrator.launch.xml index 14d69a1e..4fef1ff4 100644 --- a/sensor/new_extrinsic_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 @@ -33,7 +33,7 @@ /sensing/lidar/right/pointcloud_raw]"/> - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml similarity index 92% rename from sensor/new_extrinsic_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml index 40f4f7b9..e4034e2e 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml @@ -21,7 +21,7 @@ - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml similarity index 90% rename from sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml index 73e7011e..0543dba6 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml @@ -22,7 +22,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -49,7 +49,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml index 18eae754..b94a8a4c 100644 --- a/sensor/new_extrinsic_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 @@ -30,7 +30,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml index 6f69fb86..eba58f68 100644 --- a/sensor/new_extrinsic_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 @@ -41,7 +41,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 98% rename from sensor/new_extrinsic_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml index 3ad91a21..5fdff88f 100644 --- a/sensor/new_extrinsic_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 @@ -65,7 +65,7 @@ - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml index de209df0..fbe556ba 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/x1/ground_plane_calibrator.launch.xml @@ -24,7 +24,7 @@ - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml index ee479fda..161e7cc4 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/x2/ground_plane_calibrator.launch.xml @@ -24,7 +24,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 98% rename from sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/mapping_based_base_lidar_calibrator.launch.xml index de73e9dc..e7d7983c 100644 --- a/sensor/new_extrinsic_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 @@ -53,7 +53,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/mapping_based_lidar_lidar_calibrator.launch.xml index 9cbb96f2..4f0dff1a 100644 --- a/sensor/new_extrinsic_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 @@ -57,7 +57,7 @@ /> - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml similarity index 96% rename from sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml index 177ba8a9..10b47c0a 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml @@ -39,7 +39,7 @@ - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml similarity index 93% rename from sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml index f5477d43..6a2707ad 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/x2/tag_based_pnp_calibrator.launch.xml @@ -31,7 +31,7 @@ - + @@ -53,7 +53,7 @@ - + @@ -67,7 +67,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidar_calibrator.launch.xml index 5fe18728..a6651b9d 100644 --- a/sensor/new_extrinsic_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 @@ -30,7 +30,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_calibrator.launch.xml index 3e9ee13d..67637af6 100644 --- a/sensor/new_extrinsic_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 @@ -41,7 +41,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 98% rename from sensor/new_extrinsic_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/x2/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml index 719d3c72..2db1d8d9 100644 --- a/sensor/new_extrinsic_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 @@ -65,7 +65,7 @@ - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml index de209df0..fbe556ba 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/xx1/ground_plane_calibrator.launch.xml @@ -24,7 +24,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 97% rename from sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/xx1/mapping_based_base_lidar_calibrator.launch.xml index 293042ba..b0f539fe 100644 --- a/sensor/new_extrinsic_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 @@ -38,7 +38,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/xx1/mapping_based_lidar_lidar_calibrator.launch.xml index b055959b..5e63d9dd 100644 --- a/sensor/new_extrinsic_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 @@ -26,7 +26,7 @@ /sensing/lidar/right/rectified/pointcloud_ex]"/> - + diff --git a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml b/sensor/sensor_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml similarity index 90% rename from sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml index bff28620..715ea834 100644 --- a/sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml +++ b/sensor/sensor_calibration_manager/launch/xx1/tag_based_pnp_calibrator.launch.xml @@ -22,7 +22,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -49,7 +49,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/xx1/tag_based_sfm_base_lidar_calibrator.launch.xml index 91c6d1fd..32e1848f 100644 --- a/sensor/new_extrinsic_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 @@ -30,7 +30,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 92% rename from sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/xx1_15/tag_based_pnp_calibrator.launch.xml index 7430cdab..edf191ac 100644 --- a/sensor/new_extrinsic_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 @@ -24,7 +24,7 @@ - + @@ -37,7 +37,7 @@ - + @@ -51,7 +51,7 @@ - + diff --git a/sensor/new_extrinsic_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 similarity index 98% rename from sensor/new_extrinsic_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml rename to sensor/sensor_calibration_manager/launch/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.launch.xml index 49c2ec69..aaad0766 100644 --- a/sensor/new_extrinsic_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 @@ -56,7 +56,7 @@ - + diff --git a/sensor/new_extrinsic_calibration_manager/package.xml b/sensor/sensor_calibration_manager/package.xml similarity index 62% rename from sensor/new_extrinsic_calibration_manager/package.xml rename to sensor/sensor_calibration_manager/package.xml index 11dabe9e..5335b216 100644 --- a/sensor/new_extrinsic_calibration_manager/package.xml +++ b/sensor/sensor_calibration_manager/package.xml @@ -1,20 +1,16 @@ - new_extrinsic_calibration_manager + sensor_calibration_manager 0.0.0 - TODO: Package description + the sensor_calibration_manager package Kenzo Lobos Tsunekawa - TODO: License declaration + Apache License 2.0 python3-pyside2.qtquick python3-transforms3d - ros2_numpy ros2launch tier4_calibration_msgs - ament_python diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/__init__.py b/sensor/sensor_calibration_manager/resource/sensor_calibration_manager similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/__init__.py rename to sensor/sensor_calibration_manager/resource/sensor_calibration_manager diff --git a/sensor/new_extrinsic_calibration_manager/resource/new_extrinsic_calibration_manager b/sensor/sensor_calibration_manager/sensor_calibration_manager/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/resource/new_extrinsic_calibration_manager rename to sensor/sensor_calibration_manager/sensor_calibration_manager/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibration_manager_model.py similarity index 96% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibration_manager_model.py index 30dbbbd0..2361d608 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibration_manager_model.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibration_manager_model.py @@ -19,7 +19,7 @@ from PySide2.QtCore import QAbstractTableModel from PySide2.QtCore import Qt -from new_extrinsic_calibration_manager.calibrator_wrapper import CalibratorServiceWrapper +from sensor_calibration_manager.calibrator_wrapper import CalibratorServiceWrapper class CalibratorManagerModel(QAbstractTableModel): diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py similarity index 94% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py index edcf6d63..b9e90f8f 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_base.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py @@ -25,13 +25,13 @@ from PySide2.QtCore import QTimer from PySide2.QtCore import Signal from geometry_msgs.msg import Transform -from new_extrinsic_calibration_manager.calibrator_wrapper import CalibratorServiceWrapper -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import CalibratorState -from new_extrinsic_calibration_manager.types import FramePair -from new_extrinsic_calibration_manager.utils import tf_message_to_transform_matrix -from new_extrinsic_calibration_manager.utils import transform_matrix_to_tf_message 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 diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py similarity index 97% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py index 03a36c98..6d13c3b6 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_registry.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py @@ -19,7 +19,7 @@ from typing import Callable from typing import List -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase +from sensor_calibration_manager.calibrator_base import CalibratorBase class CalibratorRegistry: diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py similarity index 97% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py index f0064c96..3b8eb70b 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrator_wrapper.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py @@ -25,8 +25,8 @@ from PySide2.QtCore import QTimer from PySide2.QtCore import Signal from geometry_msgs.msg import Transform -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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 NewExtrinsicCalibrator diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/__init__.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/__init__.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/ground_plane_calibrator.py similarity index 81% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/ground_plane_calibrator.py index a17c3aa1..851935f6 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/ground_plane_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/ground_plane_calibrator.py @@ -15,10 +15,10 @@ # limitations under the License. -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 82% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/lidar_lidar_2d_calibrator.py index 8342e20a..ce82c261 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -14,10 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 83% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_lidar_lidar_calibrator.py index 152b4fa0..68570624 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -15,10 +15,10 @@ # limitations under the License. -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 82% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_pnp_calibrator.py index 89b85c04..753da0c5 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -15,10 +15,10 @@ # limitations under the License. -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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 diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 81% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidar_calibrator.py index e2950041..1311d8db 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -14,10 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 86% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_calibrator.py index cc2c2325..70b78bcd 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -14,10 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 89% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/tag_based_sfm_base_lidars_cameras_calibrator.py index f39ea3f8..51614df7 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -14,10 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair +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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/__init__.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 87% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_base_lidar_calibrator.py index 2dc20d46..efcd1904 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 91% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/mapping_based_lidar_lidar_calibrator.py index 716de366..dde171b3 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py similarity index 86% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py index b6f1e38c..45fbb11f 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py similarity index 88% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py index f8563507..5734d19b 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_pnp_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 87% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidar_calibrator.py index 65db531b..c5832354 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 92% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_calibrator.py index 258d1920..7fab90aa 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 94% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/tag_based_sfm_base_lidars_cameras_calibrator.py index b7da4732..42b6e3a5 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/__init__.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/ground_plane_calibrator.py similarity index 86% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/ground_plane_calibrator.py index b41fdb38..b25e3af6 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x1/ground_plane_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x1/ground_plane_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/__init__.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/ground_plane_calibrator.py similarity index 86% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/ground_plane_calibrator.py index 6f572250..f2b6fd8e 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/ground_plane_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/ground_plane_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 87% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_base_lidar_calibrator.py index bab52adb..123bb166 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/mapping_based_lidar_lidar_calibrator.py index 3c8f2bdf..55222bd3 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -17,11 +17,11 @@ from collections import defaultdict from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py similarity index 86% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py index cf62daab..96188498 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py similarity index 90% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py index baac6069..c3c589d0 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_pnp_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 87% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidar_calibrator.py index f19bb45d..11536eff 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 93% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_calibrator.py index c0b2a1d2..e54b3cec 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 95% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/tag_based_sfm_base_lidars_cameras_calibrator.py index fe3a074a..4373c4a2 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -17,11 +17,11 @@ from collections import defaultdict from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/__init__.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/ground_plane_calibrator.py similarity index 86% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/ground_plane_calibrator.py index 8f2bf095..fda4d2dc 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/ground_plane_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/ground_plane_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 87% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_base_lidar_calibrator.py index 8c21e3bf..b73d27b7 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 91% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/mapping_based_lidar_lidar_calibrator.py index bfa6408a..be36cb88 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py similarity index 88% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py index 2747b11a..aae648ac 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_pnp_calibrator.py @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 87% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1/tag_based_sfm_base_lidar_calibrator.py index 0cadd8c5..dbafd6f6 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/__init__.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/__init__.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/__init__.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 88% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_pnp_calibrator.py index 22fc6ea9..5df81c75 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -16,11 +16,11 @@ from typing import Dict -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 similarity index 93% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_15/tag_based_sfm_base_lidars_cameras_calibrator.py index 95168db7..53c73cc3 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_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 @@ -18,11 +18,11 @@ from typing import Dict from typing import List -from new_extrinsic_calibration_manager.calibrator_base import CalibratorBase -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.types import FramePair 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( diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py similarity index 98% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py index 18cf402a..165c02ef 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/ros_interface.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py @@ -29,7 +29,7 @@ class RosInterface(Node): def __init__(self): - super().__init__("new_extrinsic_calibration_manager") + super().__init__("sensor_calibration_manager") self.lock = threading.RLock() self.ros_executor = None diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py similarity index 93% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py index 68855f80..5078fde7 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py @@ -42,17 +42,15 @@ from launch.actions.set_launch_configuration import SetLaunchConfiguration from launch.frontend import Parser from launch.launch_description import LaunchDescription -from new_extrinsic_calibration_manager.calibration_manager_model import CalibratorManagerModel -from new_extrinsic_calibration_manager.calibrator_base import CalibratorState -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry -import new_extrinsic_calibration_manager.calibrators # noqa: F401 Force imports -from new_extrinsic_calibration_manager.ros_interface import RosInterface -from new_extrinsic_calibration_manager.views.calibrator_selector_view import CalibrationSelectorView -from new_extrinsic_calibration_manager.views.launcher_configuration_view import ( - LauncherConfigurationView, -) -from new_extrinsic_calibration_manager.views.tf_view import TfView 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 NewExtrinsicCalibrationManager(QMainWindow): @@ -146,7 +144,7 @@ def launch_calibrators( # Execute the launcher argument_list = [f"{k}:={v}" for k, v in launch_argument_dict.items()] - package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") + package_share_directory = get_package_share_directory("sensor_calibration_manager") launcher_path = ( package_share_directory + "/launch/" diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/types.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/types.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/utils.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/calibrator_selector_view.py similarity index 97% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/views/calibrator_selector_view.py index 9086d6c0..8a9451a0 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/calibrator_selector_view.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/calibrator_selector_view.py @@ -22,7 +22,7 @@ from PySide2.QtWidgets import QPushButton from PySide2.QtWidgets import QVBoxLayout from PySide2.QtWidgets import QWidget -from new_extrinsic_calibration_manager.calibrator_registry import CalibratorRegistry +from sensor_calibration_manager.calibrator_registry import CalibratorRegistry class CalibrationSelectorView(QWidget): diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/launcher_configuration_view.py similarity index 98% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/views/launcher_configuration_view.py index f38ac4d5..58032448 100644 --- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/launcher_configuration_view.py @@ -61,7 +61,7 @@ def __init__(self, project_name, calibrator_name): self.required_arguments_dict = {} self.arguments_widgets_dict = {} - package_share_directory = get_package_share_directory("new_extrinsic_calibration_manager") + package_share_directory = get_package_share_directory("sensor_calibration_manager") launcher_path = ( package_share_directory + "/launch/" diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/tf_view.py rename to sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py 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/new_extrinsic_calibration_manager/setup.py b/sensor/sensor_calibration_manager/setup.py similarity index 82% rename from sensor/new_extrinsic_calibration_manager/setup.py rename to sensor/sensor_calibration_manager/setup.py index a54e0f47..d6010f63 100644 --- a/sensor/new_extrinsic_calibration_manager/setup.py +++ b/sensor/sensor_calibration_manager/setup.py @@ -2,7 +2,7 @@ from setuptools import setup -package_name = "new_extrinsic_calibration_manager" +package_name = "sensor_calibration_manager" data_files = [ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -40,12 +40,12 @@ def package_files(data_files, directory_list): zip_safe=True, maintainer="Kenzo Lobos Tsunekawa", maintainer_email="kenzo.lobos@tier4.jp", - description="TODO: Package description", - license="TODO: License declaration", + description="the sensor_calibration_manager", + license="Apache License 2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ - "new_extrinsic_calibration_manager = new_extrinsic_calibration_manager.new_extrinsic_calibration_manager:main" + "sensor_calibration_manager = sensor_calibration_manager.sensor_calibration_manager:main" ], }, ) diff --git a/sensor/new_extrinsic_calibration_manager/test/test_pep257.py b/sensor/sensor_calibration_manager/test/test_pep257.py similarity index 100% rename from sensor/new_extrinsic_calibration_manager/test/test_pep257.py rename to sensor/sensor_calibration_manager/test/test_pep257.py 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_pnp_calibrator/CMakeLists.txt b/sensor/tag_based_pnp_calibrator/CMakeLists.txt similarity index 65% rename from sensor/extrinsic_tag_based_pnp_calibrator/CMakeLists.txt rename to sensor/tag_based_pnp_calibrator/CMakeLists.txt index 45aacddf..237134f3 100644 --- a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator) +project(tag_based_pnp_calibrator) find_package(autoware_cmake REQUIRED) find_package(OpenCV REQUIRED) @@ -12,15 +12,15 @@ ament_export_include_directories( ${OpenCV_INCLUDE_DIRS} ) -ament_auto_add_executable(extrinsic_tag_based_pnp_calibrator +ament_auto_add_executable(tag_based_pnp_calibrator src/brute_force_matcher.cpp src/calibration_estimator.cpp - src/extrinsic_tag_based_pnp_calibrator.cpp + src/tag_based_pnp_calibrator.cpp src/tag_calibrator_visualizer.cpp src/main.cpp ) -target_link_libraries(extrinsic_tag_based_pnp_calibrator +target_link_libraries(tag_based_pnp_calibrator ${OpenCV_LIBS} ) diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp similarity index 86% rename from sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp index ff9384f9..2ec2157c 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/brute_force_matcher.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ -#define EXTRINSIC_TAG_BASED_PNP_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 @@ -35,4 +35,4 @@ 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_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__BRUTE_FORCE_MATCHER_HPP_ diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp similarity index 96% rename from sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp index 93befcce..e7754aeb 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/calibration_estimator.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ -#define EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#ifndef TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#define TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ #include #include @@ -165,4 +165,4 @@ class CalibrationEstimator cv::Matx31d hypothesis_translation_vector_, observation_translation_vector_; }; -#endif // EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__CALIBRATION_ESTIMATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp similarity index 92% rename from sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp index 057869e9..e2761a7e 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/extrinsic_tag_based_pnp_calibrator.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__EXTRINSIC_TAG_BASED_PNP_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__EXTRINSIC_TAG_BASED_PNP_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 @@ -142,4 +142,4 @@ class ExtrinsicTagBasedPNPCalibrator : public rclcpp::Node bool got_initial_transform; }; -#endif // EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__EXTRINSIC_TAG_BASED_PNP_CALIBRATOR_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__TAG_BASED_PNP_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp similarity index 93% rename from sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp rename to sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp index c4d06ecb..deb31008 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/include/extrinsic_tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ -#define EXTRINSIC_TAG_BASED_PNP_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 @@ -123,4 +123,4 @@ class TagCalibratorVisualizer cv::Affine3d camera_base_transform_; }; -#endif // EXTRINSIC_TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ +#endif // TAG_BASED_PNP_CALIBRATOR__TAG_CALIBRATOR_VISUALIZER_HPP_ diff --git a/sensor/extrinsic_tag_based_pnp_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_pnp_calibrator/launch/apriltag_16h5.launch.py rename to sensor/tag_based_pnp_calibrator/launch/apriltag_16h5.launch.py diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml similarity index 93% rename from sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml rename to sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml index e4d71be1..330f900d 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml +++ b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -17,13 +17,13 @@ - + - + diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/package.xml b/sensor/tag_based_pnp_calibrator/package.xml similarity index 90% rename from sensor/extrinsic_tag_based_pnp_calibrator/package.xml rename to sensor/tag_based_pnp_calibrator/package.xml index 39d56139..dff93dcf 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/package.xml +++ b/sensor/tag_based_pnp_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_tag_based_pnp_calibrator + tag_based_pnp_calibrator 0.0.1 - The extrinsic_tag_based_pnp_calibrator package + The tag_based_pnp_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz b/sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz similarity index 100% rename from sensor/extrinsic_tag_based_pnp_calibrator/rviz/default_profile.rviz rename to sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp b/sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp similarity index 99% rename from sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp rename to sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp index c62cbe67..2d4a4e8f 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/brute_force_matcher.cpp +++ b/sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp similarity index 99% rename from sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp rename to sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp index 9dbaceb1..431101c4 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/calibration_estimator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include #include #include +#include +#include #include #include diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/main.cpp b/sensor/tag_based_pnp_calibrator/src/main.cpp similarity index 92% rename from sensor/extrinsic_tag_based_pnp_calibrator/src/main.cpp rename to sensor/tag_based_pnp_calibrator/src/main.cpp index 330a6de0..bdb0c3a5 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/main.cpp +++ b/sensor/tag_based_pnp_calibrator/src/main.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp similarity index 99% rename from sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp rename to sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp index b5e4e38a..16f46246 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/extrinsic_tag_based_pnp_calibrator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include +#include #include #include @@ -25,7 +25,7 @@ #include ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::NodeOptions & options) -: Node("extrinsic_tag_based_pnp_calibrator_node", options), +: Node("tag_based_pnp_calibrator_node", options), tf_broadcaster_(this), request_received_(false), got_initial_transform(false) diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp similarity index 99% rename from sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp rename to sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index d0199824..bf9fc9a2 100644 --- a/sensor/extrinsic_tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/CMakeLists.txt b/sensor/tag_based_sfm_calibrator/CMakeLists.txt similarity index 85% rename from sensor/extrinsic_tag_based_sfm_calibrator/CMakeLists.txt rename to sensor/tag_based_sfm_calibrator/CMakeLists.txt index 2a070721..7cf42b59 100644 --- a/sensor/extrinsic_tag_based_sfm_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_sfm_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_sfm_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_sfm_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_sfm_calibrator src/visualization.cpp ) -target_link_libraries(extrinsic_tag_based_sfm_calibrator +target_link_libraries(tag_based_sfm_calibrator ${Boost_LIBRARIES} ${OpenCV_LIBS} apriltag::apriltag diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml b/sensor/tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml similarity index 100% rename from sensor/extrinsic_tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml rename to sensor/tag_based_sfm_calibrator/config/omiya_calibration_room_2023.param.yaml diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detection.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detection.hpp similarity index 89% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detection.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detection.hpp index 4f171030..95caca41 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detection.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detection.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { struct LidartagDetection @@ -100,6 +100,6 @@ using ApriltagGridDetections = std::vector; using GroupedApriltagDetections = std::map; using GroupedApriltagGridDetections = std::map; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTION_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detector.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detector.hpp similarity index 85% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detector.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detector.hpp index 28f9a2c0..136de60b 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/apriltag_detector.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/apriltag_detector.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTOR_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { class ApriltagDetector @@ -73,6 +73,6 @@ class ApriltagDetector static std::unordered_map tag_destroy_fn_map; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__APRILTAG_DETECTOR_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_scene_extractor.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_scene_extractor.hpp similarity index 86% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_scene_extractor.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_scene_extractor.hpp index 04b94740..2caa1301 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_scene_extractor.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_scene_extractor.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { class CalibrationSceneExtractor @@ -94,6 +94,6 @@ class CalibrationSceneExtractor bool debug_; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CALIBRATION_SCENE_EXTRACTOR_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_types.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_types.hpp similarity index 88% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_types.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_types.hpp index cae6dbad..2c4dfc7d 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/calibration_types.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/calibration_types.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { struct CalibrationData @@ -89,6 +89,6 @@ struct CalibrationData std::shared_ptr optimized_right_wheel_tag_pose; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CALIBRATION_TYPES_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/calibration_problem.hpp similarity index 95% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/calibration_problem.hpp index 7f953efa..85ef097b 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/calibration_problem.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/calibration_problem.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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,7 +26,7 @@ #include #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { class CalibrationProblem @@ -253,6 +253,6 @@ class CalibrationProblem std::array shared_intrinsics_opt; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__CALIBRATION_PROBLEM_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/camera_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp similarity index 97% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/camera_residual.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp index 76da5d61..cfe64dc6 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/camera_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { struct CameraResidual : public SensorResidual @@ -495,6 +495,6 @@ struct CameraResidual : public SensorResidual bool is_ground_tag_; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__CAMERA_RESIDUAL_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/lidar_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp similarity index 95% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/lidar_residual.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp index 41e34381..a1b0ad5b 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/lidar_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { struct LidarResidual : public SensorResidual @@ -289,6 +289,6 @@ struct LidarResidual : public SensorResidual Eigen::Matrix3d tag_centric_rotation_; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__LIDAR_RESIDUAL_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/sensor_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/sensor_residual.hpp similarity index 86% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/sensor_residual.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/sensor_residual.hpp index b955acf7..4c4942f1 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/ceres/sensor_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/sensor_residual.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { struct SensorResidual @@ -68,6 +68,6 @@ struct SensorResidual static constexpr int NUM_CORNERS = 4; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__CERES__SENSOR_RESIDUAL_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp similarity index 67% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp index 549d489e..c2729369 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/apriltag_calibrator.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_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_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__APRILTAG_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp similarity index 67% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp index ea0800b7..c29591ff 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/chessboard_calibrator.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { class ChessboardBasedCalibrator : public IntrinsicsCalibrator @@ -49,6 +49,6 @@ class ChessboardBasedCalibrator : public IntrinsicsCalibrator int cols_; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__CHESSBOARD_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp similarity index 79% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp index 0d916b43..db733674 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/intrinsics_calibration/intrinsics_calibrator.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { class IntrinsicsCalibrator @@ -69,6 +69,6 @@ class IntrinsicsCalibrator bool debug_; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__INTRINSICS_CALIBRATION__INTRINSICS_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/math.hpp similarity index 88% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/math.hpp index 3131de7c..4dc73a88 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/math.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/math.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { /* @@ -99,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_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/scene_types.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/scene_types.hpp similarity index 80% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/scene_types.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/scene_types.hpp index 72db10ae..c2153d70 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/scene_types.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/scene_types.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { struct ExternalCameraFrame @@ -58,6 +58,6 @@ struct CalibrationScene std::vector external_camera_frames; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__SCENE_TYPES_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/serialization.hpp similarity index 84% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/serialization.hpp index 451366d9..53cef818 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/serialization.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/serialization.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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 @@ -144,7 +144,7 @@ void serialize(Archive & ar, cv::Affine3<_Tp> & pose, [[maybe_unused]] const uns template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::ApriltagDetection & detection, + Archive & ar, tag_based_sfm_calibrator::ApriltagDetection & detection, [[maybe_unused]] const unsigned int version) { ar & detection.family; @@ -159,7 +159,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::ApriltagGridDetection & detection, + Archive & ar, tag_based_sfm_calibrator::ApriltagGridDetection & detection, [[maybe_unused]] const unsigned int version) { ar & detection.cols; @@ -177,7 +177,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::LidartagDetection & detection, + Archive & ar, tag_based_sfm_calibrator::LidartagDetection & detection, [[maybe_unused]] const unsigned int version) { ar & detection.id; @@ -189,7 +189,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::ExternalCameraFrame & frame, + Archive & ar, tag_based_sfm_calibrator::ExternalCameraFrame & frame, [[maybe_unused]] const unsigned int version) { ar & frame.image_filename; @@ -198,8 +198,7 @@ void serialize( template void serialize( - Archive & ar, - extrinsic_tag_based_sfm_calibrator::SingleCalibrationLidarDetections & lidar_detections, + Archive & ar, tag_based_sfm_calibrator::SingleCalibrationLidarDetections & lidar_detections, [[maybe_unused]] const unsigned int version) { ar & lidar_detections.calibration_frame; @@ -209,8 +208,7 @@ void serialize( template void serialize( - Archive & ar, - extrinsic_tag_based_sfm_calibrator::SingleCalibrationCameraDetections & camera_detections, + Archive & ar, tag_based_sfm_calibrator::SingleCalibrationCameraDetections & camera_detections, [[maybe_unused]] const unsigned int version) { ar & camera_detections.calibration_frame; @@ -221,7 +219,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::CalibrationScene & scene, + Archive & ar, tag_based_sfm_calibrator::CalibrationScene & scene, [[maybe_unused]] const unsigned int version) { ar & scene.calibration_cameras_detections; @@ -231,8 +229,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::UID & uid, - [[maybe_unused]] const unsigned int version) + Archive & ar, tag_based_sfm_calibrator::UID & uid, [[maybe_unused]] const unsigned int version) { ar & uid.type; ar & uid.sensor_type; @@ -246,7 +243,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::IntrinsicParameters & intrinsics, + Archive & ar, tag_based_sfm_calibrator::IntrinsicParameters & intrinsics, [[maybe_unused]] const unsigned int version) { ar & intrinsics.size; @@ -257,7 +254,7 @@ void serialize( template void serialize( - Archive & ar, extrinsic_tag_based_sfm_calibrator::CalibrationData & data, + Archive & ar, tag_based_sfm_calibrator::CalibrationData & data, [[maybe_unused]] const unsigned int version) { ar & data.scenes; @@ -294,4 +291,4 @@ void serialize( } // namespace serialization } // namespace boost -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__SERIALIZATION_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp similarity index 95% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp index 7f6f5531..642eb8a8 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__EXTRINSIC_TAG_BASED_SFM_CALIBRATOR_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__EXTRINSIC_TAG_BASED_SFM_CALIBRATOR_HPP_ - -#include -#include -#include -#include -#include +#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 @@ -49,7 +49,7 @@ #include #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node @@ -345,6 +345,6 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node std::vector precomputed_colors_; }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__EXTRINSIC_TAG_BASED_SFM_CALIBRATOR_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__TAG_BASED_SFM_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/types.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/types.hpp similarity index 94% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/types.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/types.hpp index d8523e23..fbadb1a2 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/types.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/types.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ +#ifndef TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ +#define TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ #include #include @@ -26,7 +26,7 @@ #include #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { struct ApriltagDetectorParameters @@ -231,15 +231,15 @@ struct UID // types }; -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator namespace std { template <> -struct hash +struct hash { - std::size_t operator()(const extrinsic_tag_based_sfm_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_SFM_CALIBRATOR__TYPES_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__TYPES_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/visualization.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/visualization.hpp similarity index 92% rename from sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/visualization.hpp rename to sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/visualization.hpp index e2094e23..5377689e 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/include/extrinsic_tag_based_sfm_calibrator/visualization.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/visualization.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { /*! @@ -132,6 +132,6 @@ void drawAxes( void drawAxes( cv::Mat & img, const ApriltagDetection & detection, const IntrinsicParameters & intrinsics); -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ +#endif // TAG_BASED_SFM_CALIBRATOR__VISUALIZATION_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.py b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.py similarity index 100% rename from sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.py rename to sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.py diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml similarity index 94% rename from sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml rename to sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml index 84a2d1e4..9b78865e 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml +++ b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml b/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml similarity index 85% rename from sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml rename to sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml index 858514bc..c4e6fb47 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/launch/calibrator.launch.xml +++ b/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml @@ -2,11 +2,11 @@ - + - + @@ -158,7 +158,7 @@ - + @@ -185,7 +185,7 @@ - + @@ -201,63 +201,63 @@ - + - + - + - + - + - + - + - + - + @@ -265,7 +265,7 @@ - + @@ -273,7 +273,7 @@ - + @@ -281,7 +281,7 @@ - + @@ -289,7 +289,7 @@ - + @@ -297,7 +297,7 @@ - + @@ -305,7 +305,7 @@ - + diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml b/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml similarity index 100% rename from sensor/extrinsic_tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml rename to sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/package.xml b/sensor/tag_based_sfm_calibrator/package.xml similarity index 92% rename from sensor/extrinsic_tag_based_sfm_calibrator/package.xml rename to sensor/tag_based_sfm_calibrator/package.xml index b2d8d0c1..39ab36fc 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/package.xml +++ b/sensor/tag_based_sfm_calibrator/package.xml @@ -1,9 +1,9 @@ - extrinsic_tag_based_sfm_calibrator + tag_based_sfm_calibrator 0.0.1 - The extrinsic_tag_based_sfm_calibrator package + The tag_based_sfm_calibrator package Kenzo Lobos Tsunekawa BSD diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz b/sensor/tag_based_sfm_calibrator/rviz/default.rviz similarity index 100% rename from sensor/extrinsic_tag_based_sfm_calibrator/rviz/default.rviz rename to sensor/tag_based_sfm_calibrator/rviz/default.rviz diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py b/sensor/tag_based_sfm_calibrator/scripts/calibrator_ui_node.py similarity index 90% rename from sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py rename to sensor/tag_based_sfm_calibrator/scripts/calibrator_ui_node.py index 10a99c0d..f451627e 100755 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/scripts/calibrator_ui_node.py +++ b/sensor/tag_based_sfm_calibrator/scripts/calibrator_ui_node.py @@ -19,9 +19,9 @@ import sys from PySide2.QtWidgets import QApplication -from extrinsic_marker_radar_lidar_calibrator import CalibratorUI -from extrinsic_marker_radar_lidar_calibrator import RosInterface import rclpy +from tag_based_sfm_calibrator import CalibratorUI +from tag_based_sfm_calibrator import RosInterface def main(args=None): diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp b/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp similarity index 98% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp rename to sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp index 5f6a5625..57a6c24b 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detection.cpp +++ b/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp @@ -14,16 +14,16 @@ #include #include -#include -#include #include #include #include +#include +#include #include #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { LidartagDetection LidartagDetection::fromLidartagDetectionMsg( @@ -355,4 +355,4 @@ double ApriltagGridDetection::detectionDiagonalRatio() const (rows * cols); } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detector.cpp b/sensor/tag_based_sfm_calibrator/src/apriltag_detector.cpp similarity index 98% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detector.cpp rename to sensor/tag_based_sfm_calibrator/src/apriltag_detector.cpp index 556cc1e6..5b1ba283 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/apriltag_detector.cpp +++ b/sensor/tag_based_sfm_calibrator/src/apriltag_detector.cpp @@ -14,10 +14,10 @@ #include #include -#include -#include #include #include +#include +#include #include #include @@ -26,7 +26,7 @@ #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { std::unordered_map @@ -333,4 +333,4 @@ GroupedApriltagGridDetections ApriltagDetector::detect(const cv::Mat & cv_img) c return grid_detections_map; } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp b/sensor/tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp similarity index 96% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp rename to sensor/tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp index 349f32a9..5546bace 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp +++ b/sensor/tag_based_sfm_calibrator/src/calibration_scene_extractor.cpp @@ -14,20 +14,20 @@ #include #include -#include -#include #include #include #include #include #include #include +#include +#include #include #include #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { void CalibrationSceneExtractor::setCalibrationSensorIntrinsics(IntrinsicParameters & intrinsics) @@ -173,4 +173,4 @@ GroupedApriltagGridDetections CalibrationSceneExtractor::detect( return detector.detect(undistorted_img); } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp similarity index 99% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp rename to sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp index a97bd8d2..63dc61e6 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp +++ b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp @@ -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 @@ -35,7 +35,7 @@ #include #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { void CalibrationProblem::setOptimizeIntrinsics(bool ba_optimize_intrinsics) @@ -1221,4 +1221,4 @@ void CalibrationProblem::printCalibrationResults() } } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp similarity index 96% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp index a292ac54..2c80a782 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/apriltag_calibrator.cpp @@ -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_sfm_calibrator +namespace tag_based_sfm_calibrator { void ApriltagBasedCalibrator::extractCalibrationPoints() @@ -128,4 +128,4 @@ void ApriltagBasedCalibrator::writeDebugImages(const IntrinsicParameters & intri } } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp similarity index 95% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp index 6eb6e589..76323a7c 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/chessboard_calibrator.cpp @@ -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_sfm_calibrator +namespace tag_based_sfm_calibrator { void ChessboardBasedCalibrator::extractCalibrationPoints() @@ -112,4 +112,4 @@ void ChessboardBasedCalibrator::writeDebugImages(const IntrinsicParameters & int } } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp similarity index 94% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp index b2faedb2..6a17bd25 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/intrinsics_calibration/intrinsics_calibrator.cpp @@ -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_sfm_calibrator +namespace tag_based_sfm_calibrator { void IntrinsicsCalibrator::setCalibrationImageFiles( @@ -103,4 +103,4 @@ bool IntrinsicsCalibrator::calibrate(IntrinsicParameters & intrinsics) return true; } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_marker_radar_lidar_calibrator/src/main.cpp b/sensor/tag_based_sfm_calibrator/src/main.cpp similarity index 73% rename from sensor/extrinsic_marker_radar_lidar_calibrator/src/main.cpp rename to sensor/tag_based_sfm_calibrator/src/main.cpp index 254cd810..532414bd 100644 --- a/sensor/extrinsic_marker_radar_lidar_calibrator/src/main.cpp +++ b/sensor/tag_based_sfm_calibrator/src/main.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include + +#include int main(int argc, char ** argv) { @@ -21,9 +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_sfm_calibrator/src/math.cpp b/sensor/tag_based_sfm_calibrator/src/math.cpp similarity index 97% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp rename to sensor/tag_based_sfm_calibrator/src/math.cpp index c865f961..e0c2a393 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/math.cpp +++ b/sensor/tag_based_sfm_calibrator/src/math.cpp @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ -#define EXTRINSIC_TAG_BASED_SFM_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_sfm_calibrator +namespace tag_based_sfm_calibrator { /* @@ -493,6 +493,6 @@ void estimateInitialPoses( } } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator -#endif // EXTRINSIC_TAG_BASED_SFM_CALIBRATOR__MATH_HPP_ +#endif // tag_based_sfm_calibrator__MATH_HPP_ diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp similarity index 98% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp rename to sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp index fc989b8d..7ecb8889 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/extrinsic_tag_based_sfm_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp @@ -13,20 +13,20 @@ // 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 @@ -44,12 +44,12 @@ #include #include -namespace extrinsic_tag_based_sfm_calibrator +namespace tag_based_sfm_calibrator { ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( const rclcpp::NodeOptions & options) -: Node("extrinsic_tag_based_sfm_calibrator_node", options), +: Node("tag_based_sfm_calibrator_node", options), tf_broadcaster_(this), calibration_done_(false), data_(std::make_shared()) @@ -1569,4 +1569,4 @@ bool ExtrinsicTagBasedBaseCalibrator::saveDatabaseCallback( return true; } -} // namespace extrinsic_tag_based_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/src/visualization.cpp b/sensor/tag_based_sfm_calibrator/src/visualization.cpp similarity index 98% rename from sensor/extrinsic_tag_based_sfm_calibrator/src/visualization.cpp rename to sensor/tag_based_sfm_calibrator/src/visualization.cpp index 6a06e2fc..fa5be7d5 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/src/visualization.cpp +++ b/sensor/tag_based_sfm_calibrator/src/visualization.cpp @@ -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_sfm_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_sfm_calibrator +} // namespace tag_based_sfm_calibrator diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/__init__.py b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/__init__.py similarity index 100% rename from sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/__init__.py rename to sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/__init__.py diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/calibrator_ui.py similarity index 100% rename from sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/calibrator_ui.py rename to sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/calibrator_ui.py diff --git a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/ros_interface.py similarity index 99% rename from sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py rename to sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/ros_interface.py index f18459b7..a1dbe09e 100644 --- a/sensor/extrinsic_tag_based_sfm_calibrator/extrinsic_tag_based_sfm_calibrator/ros_interface.py +++ b/sensor/tag_based_sfm_calibrator/tag_based_sfm_calibrator/ros_interface.py @@ -88,7 +88,7 @@ def __call__(self, files_list): class RosInterface(Node): def __init__(self): - super().__init__("extrinsic_tag_based_sfm_calibrator") + super().__init__("tag_based_sfm_calibrator") self.ros_context = None self.ros_executor = None From 4ce9f7df1507da28e586458e76cd8c748b64ae17 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 09:49:51 +0900 Subject: [PATCH 22/49] chore: deleted deprecated mesasges Signed-off-by: Kenzo Lobos-Tsunekawa --- common/tier4_calibration_msgs/CMakeLists.txt | 2 -- .../srv/ExtrinsicCalibrationManager.srv | 6 ------ common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv | 6 ------ 3 files changed, 14 deletions(-) delete mode 100644 common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv delete mode 100644 common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt index ed46d69b..24753d21 100644 --- a/common/tier4_calibration_msgs/CMakeLists.txt +++ b/common/tier4_calibration_msgs/CMakeLists.txt @@ -26,8 +26,6 @@ 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/NewExtrinsicCalibrator.srv" 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 deleted file mode 100644 index c8e0a95b..00000000 --- a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv +++ /dev/null @@ -1,6 +0,0 @@ -geometry_msgs/Pose initial_pose ---- -geometry_msgs/Pose result_pose -bool success -float32 score -sensor_msgs/PointCloud2 debug_pointcloud From b12027364bcdc54abea59b06c2dbdef8d5138db3 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 11:40:41 +0900 Subject: [PATCH 23/49] chore: fixed extra mispells found in ci/cd Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 3 +++ common/tier4_calibration_views/CMakeLists.txt | 15 --------------- .../tier4_calibration_views/image_view.py | 16 ++++++++-------- sensor/docs/how_to_extrinsic_interactive.md | 2 +- sensor/docs/how_to_extrinsic_tag_based.md | 2 +- .../{reprojerror.svg => reprojection_error.svg} | 0 .../package.xml | 1 - sensor/mapping_based_calibrator/CMakeLists.txt | 1 + sensor/mapping_based_calibrator/package.xml | 1 - .../rviz/default.rviz | 2 +- ...pping_based_lidar_lidar_calibrator.launch.xml | 2 +- .../rviz/default_profile.rviz | 2 +- .../omiya_calibration_room_2023.param.yaml | 2 +- .../launch/calibrator.launch.xml | 3 --- 14 files changed, 18 insertions(+), 34 deletions(-) rename sensor/docs/images/camera-lidar/{reprojerror.svg => reprojection_error.svg} (100%) diff --git a/.cspell.json b/.cspell.json index 99c308a7..a9d2e7df 100644 --- a/.cspell.json +++ b/.cspell.json @@ -10,6 +10,7 @@ "astype", "auxiliar", "axisd", + "beforementioned", "calib", "cmap", "coeffs", @@ -51,6 +52,8 @@ "neighbours", "ncols", "nrows", + "omiya", + "overfits", "pandar", "permutate", "pixmap", diff --git a/common/tier4_calibration_views/CMakeLists.txt b/common/tier4_calibration_views/CMakeLists.txt index 029104b5..95b1e5ab 100644 --- a/common/tier4_calibration_views/CMakeLists.txt +++ b/common/tier4_calibration_views/CMakeLists.txt @@ -5,30 +5,15 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) find_package(autoware_cmake REQUIRED) -#find_package(sensor_msgs REQUIRED) -#find_package(nav_msgs REQUIRED) -#find_package(geometry_msgs REQUIRED) autoware_package() ament_python_install_package(${PROJECT_NAME}) -#if(BUILD_TESTING) -# find_package(rclpy REQUIRED) -# find_package(ament_cmake_nose REQUIRED) - -# ament_add_nose_test(pointclouds test/test_pointclouds.py) -# ament_add_nose_test(images test/test_images.py) -# ament_add_nose_test(occupancygrids test/test_occupancygrids.py) -# ament_add_nose_test(geometry test/test_geometry.py) -# ament_add_nose_test(quaternions test/test_quat.py) -#endif() - 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/tier4_calibration_views/image_view.py b/common/tier4_calibration_views/tier4_calibration_views/image_view.py index 976e4c07..8f452178 100644 --- a/common/tier4_calibration_views/tier4_calibration_views/image_view.py +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view.py @@ -484,8 +484,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": @@ -494,10 +494,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 @@ -514,7 +514,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() @@ -598,7 +598,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 @@ -669,7 +669,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 @@ -776,7 +776,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, diff --git a/sensor/docs/how_to_extrinsic_interactive.md b/sensor/docs/how_to_extrinsic_interactive.md index 5c22af0b..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
diff --git a/sensor/docs/how_to_extrinsic_tag_based.md b/sensor/docs/how_to_extrinsic_tag_based.md index 6a55e8fb..4e631d33 100644 --- a/sensor/docs/how_to_extrinsic_tag_based.md +++ b/sensor/docs/how_to_extrinsic_tag_based.md @@ -131,7 +131,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 5.
- +
Fig 5. Reprojection error
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/interactive_camera_lidar_calibrator/package.xml b/sensor/interactive_camera_lidar_calibrator/package.xml index 3e381a86..0e1223a8 100644 --- a/sensor/interactive_camera_lidar_calibrator/package.xml +++ b/sensor/interactive_camera_lidar_calibrator/package.xml @@ -10,7 +10,6 @@ python3-matplotlib python3-pyside2.qtquick python3-transforms3d - ros2_numpy ros2launch tier4_calibration_msgs tier4_calibration_views diff --git a/sensor/mapping_based_calibrator/CMakeLists.txt b/sensor/mapping_based_calibrator/CMakeLists.txt index cb61be46..1b8d02f8 100644 --- a/sensor/mapping_based_calibrator/CMakeLists.txt +++ b/sensor/mapping_based_calibrator/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) 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") diff --git a/sensor/mapping_based_calibrator/package.xml b/sensor/mapping_based_calibrator/package.xml index 9dbb03da..fc6149da 100644 --- a/sensor/mapping_based_calibrator/package.xml +++ b/sensor/mapping_based_calibrator/package.xml @@ -20,7 +20,6 @@ kalman_filter libboost-filesystem libboost-serialization - libg2o libpcl-all-dev nav_msgs ndt_omp diff --git a/sensor/marker_radar_lidar_calibrator/rviz/default.rviz b/sensor/marker_radar_lidar_calibrator/rviz/default.rviz index da6a00ed..bdb9608d 100644 --- a/sensor/marker_radar_lidar_calibrator/rviz/default.rviz +++ b/sensor/marker_radar_lidar_calibrator/rviz/default.rviz @@ -124,7 +124,7 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: lidar_forground_pointcloud + Name: lidar_foreground_pointcloud Position Transformer: XYZ Selectable: true Size (Pixels): 3 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 index 4fef1ff4..26b57743 100644 --- 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 @@ -8,7 +8,7 @@ - + diff --git a/sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz b/sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz index fcaa0b8f..9b8fc094 100644 --- a/sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz +++ b/sensor/tag_based_pnp_calibrator/rviz/default_profile.rviz @@ -566,7 +566,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /lidartag/intesection_markers + Value: /lidartag/intersection_markers Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false 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 index 572012ad..83d869f7 100644 --- 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 @@ -79,6 +79,6 @@ max_homography_error: 0.5 quad_decimate: 1.0 quad_sigma: 0.0 - nthreads: 12 + nthreads: 12 # cSpell:ignore nthreads debug: false refine_edges: true diff --git a/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml b/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml index c4e6fb47..b7b10c2d 100644 --- a/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml +++ b/sensor/tag_based_sfm_calibrator/launch/calibrator.launch.xml @@ -151,10 +151,7 @@ - - From 79ce6462303a84b8efe13f68dd2ef14db5cb6398 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 11:46:01 +0900 Subject: [PATCH 24/49] chore: more spell fixes Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 1 + sensor/docs/how_to_extrinsic_tag_based.md | 10 +++++----- .../{tagbased-1.jpg => tag_based-1.jpg} | Bin .../{tagbased-2.jpg => tag_based-2.jpg} | Bin 4 files changed, 6 insertions(+), 5 deletions(-) rename sensor/docs/images/camera-lidar/{tagbased-1.jpg => tag_based-1.jpg} (100%) rename sensor/docs/images/camera-lidar/{tagbased-2.jpg => tag_based-2.jpg} (100%) diff --git a/.cspell.json b/.cspell.json index a9d2e7df..4cb33a5a 100644 --- a/.cspell.json +++ b/.cspell.json @@ -40,6 +40,7 @@ "intrinsics", "kalman", "keyframes", + "libceres", "lidars", "lidartag", "lidartags", diff --git a/sensor/docs/how_to_extrinsic_tag_based.md b/sensor/docs/how_to_extrinsic_tag_based.md index 4e631d33..08273b6f 100644 --- a/sensor/docs/how_to_extrinsic_tag_based.md +++ b/sensor/docs/how_to_extrinsic_tag_based.md @@ -135,7 +135,7 @@ The difference between a point in the image (pimage), and the project
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/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 From 025c46edb839f87ebb3c22aaf69bdb1a94c07b06 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 12:00:01 +0900 Subject: [PATCH 25/49] fix: fixed compile error (eigen vs. opencv) Signed-off-by: Kenzo Lobos-Tsunekawa --- sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp index 16f46246..99b8c0a4 100644 --- a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include // note: this header must come before #include #include #include From 6d9c30a22b0aadf835f971eb357453e6f4faf691 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 13:17:44 +0900 Subject: [PATCH 26/49] chore: applied the fix to another file Signed-off-by: Kenzo Lobos-Tsunekawa --- sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp index 431101c4..bea31c35 100644 --- a/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include // note: this header must come before #include #include #include From 5b96f653cf05c55e3aa2588ec4b4c3ad46628029 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 19 Jan 2024 14:19:10 +0900 Subject: [PATCH 27/49] chore: fixed dependencies for ci cd Signed-off-by: Kenzo Lobos-Tsunekawa --- sensor/sensor_calibration_manager/package.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sensor/sensor_calibration_manager/package.xml b/sensor/sensor_calibration_manager/package.xml index 5335b216..b9bceb7b 100644 --- a/sensor/sensor_calibration_manager/package.xml +++ b/sensor/sensor_calibration_manager/package.xml @@ -7,10 +7,10 @@ Kenzo Lobos Tsunekawa Apache License 2.0 - python3-pyside2.qtquick - python3-transforms3d - ros2launch - tier4_calibration_msgs + python3-pyside2.qtquick + python3-transforms3d + ros2launch + tier4_calibration_msgs ament_python From b3f8e901796eba8a8e4e049e39f6071d21494f4b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 22 Jan 2024 14:21:33 +0900 Subject: [PATCH 28/49] fix: typo in launcher (thx vivid) Signed-off-by: Kenzo Lobos-Tsunekawa --- .../launch/rdv/tag_based_sfm_base_lidars_calibrator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index eba58f68..21dea311 100644 --- 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 @@ -9,7 +9,7 @@ - + From aca65439a72b0d8bb178083e9f5d89cc64a27856 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 25 Jan 2024 14:00:24 +0900 Subject: [PATCH 29/49] fix: forgot to set the initial solution after the refactoring in the ground plane calibrator Signed-off-by: Kenzo Lobos-Tsunekawa --- sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp index e0df215b..1f13b372 100644 --- a/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp +++ b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp @@ -206,6 +206,8 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( } // 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_); From fc7fb7e4303ba9d679416271f74de5066e5fc5eb Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 6 Feb 2024 16:20:02 +0900 Subject: [PATCH 30/49] chore: added explanations regarding the coordinate systems Signed-off-by: Kenzo Lobos-Tsunekawa --- .../tier4_calibration_views/image_view.py | 5 +++++ .../interactive_camera_lidar_calibrator/image_view.py | 4 ++++ .../mapping_based_calibrator/src/base_lidar_calibrator.cpp | 1 + sensor/mapping_based_calibrator/src/camera_calibrator.cpp | 1 + .../src/marker_radar_lidar_calibrator.cpp | 4 ++++ .../src/tag_calibrator_visualizer.cpp | 5 +++++ .../tag_based_sfm_calibrator/ceres/camera_residual.hpp | 1 + .../tag_based_sfm_calibrator/ceres/lidar_residual.hpp | 4 ++-- .../src/ceres/calibration_problem.cpp | 1 + 9 files changed, 24 insertions(+), 2 deletions(-) diff --git a/common/tier4_calibration_views/tier4_calibration_views/image_view.py b/common/tier4_calibration_views/tier4_calibration_views/image_view.py index 8f452178..fb2bf8d4 100644 --- a/common/tier4_calibration_views/tier4_calibration_views/image_view.py +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view.py @@ -454,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, @@ -580,8 +581,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, @@ -653,6 +656,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( @@ -762,6 +766,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, 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 index 84dbf1eb..896c458e 100644 --- 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 @@ -454,6 +454,7 @@ def draw_pointcloud(self, painter): ): 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, @@ -579,6 +580,7 @@ def draw_calibration_points(self, painter): 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( @@ -654,6 +656,7 @@ def draw_external_calibration_points(self, painter): 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, @@ -761,6 +764,7 @@ def draw_current_point(self, painter): ): 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, diff --git a/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp b/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp index 66d1a27e..aa328a27 100644 --- a/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/base_lidar_calibrator.cpp @@ -133,6 +133,7 @@ void BaseLidarCalibrator::publishResults( 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()); diff --git a/sensor/mapping_based_calibrator/src/camera_calibrator.cpp b/sensor/mapping_based_calibrator/src/camera_calibrator.cpp index a554852d..1d392abb 100644 --- a/sensor/mapping_based_calibrator/src/camera_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/camera_calibrator.cpp @@ -179,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), diff --git a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index 27dbb422..cf1b5d0a 100644 --- a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -1174,6 +1174,7 @@ std::tuple< ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta() { // Define two sets of 2D points (just 3D points with z=0) + // Note: pcs=paralell cordinate 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()); @@ -1251,6 +1252,7 @@ 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=paralell cordinate system rcs=radar coordinate system // Estimate full transformation using SVD pcl::registration::TransformationEstimationSVD estimator; Eigen::Matrix4f full_radar_to_radar_parallel_transformation; @@ -1403,6 +1405,7 @@ void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( pcl::PointCloud::Ptr lidar_points_pcs, pcl::PointCloud::Ptr radar_points_rcs) { + // Note: pcs=paralell cordinate system rcs=radar coordinate system int tracks_size = static_cast(converged_tracks_.size()); if (tracks_size <= 3) return; @@ -1527,6 +1530,7 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() output_metrics_.clear(); + // Note: pcs=paralell cordinate 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); diff --git a/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index bf9fc9a2..fe6d659d 100644 --- a/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -137,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_; @@ -208,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; @@ -315,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; @@ -481,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; @@ -582,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; diff --git a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp index cfe64dc6..56405b30 100644 --- a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp @@ -122,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]; diff --git a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp index a1b0ad5b..389aee6a 100644 --- a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/lidar_residual.hpp @@ -125,8 +125,8 @@ 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_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 = diff --git a/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp index 63dc61e6..29a9503d 100644 --- a/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp +++ b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp @@ -957,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]}; From 899c59361bc66bd2367c1e86f6b9ef0643dea314 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 6 Feb 2024 16:24:11 +0900 Subject: [PATCH 31/49] chore: fixed typo in launcher Signed-off-by: Kenzo Lobos-Tsunekawa --- .../launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index b94a8a4c..bbc67b99 100644 --- 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 @@ -8,7 +8,7 @@ - + From b88971be7abe7f9a7a7a0553d895533e944780a8 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 6 Feb 2024 19:41:13 +0900 Subject: [PATCH 32/49] chore: updated documentation and added missing marker in rviz profile Signed-off-by: Kenzo Lobos-Tsunekawa --- sensor/mapping_based_calibrator/src/utils.cpp | 11 +++++------ .../rviz/default.rviz | 14 +++++++++++++- .../src/marker_radar_lidar_calibrator.cpp | 8 ++++---- 3 files changed, 22 insertions(+), 11 deletions(-) diff --git a/sensor/mapping_based_calibrator/src/utils.cpp b/sensor/mapping_based_calibrator/src/utils.cpp index 9a3fd26a..68c63125 100644 --- a/sensor/mapping_based_calibrator/src/utils.cpp +++ b/sensor/mapping_based_calibrator/src/utils.cpp @@ -110,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 rem = poseInterpolationBase(te, 0, dt, Eigen::Matrix4f::Identity(), dm); - - return m * rem; + return m * poseInterpolationBase(t_rem, 0, dt, Eigen::Matrix4f::Identity(), dm); } template diff --git a/sensor/marker_radar_lidar_calibrator/rviz/default.rviz b/sensor/marker_radar_lidar_calibrator/rviz/default.rviz index bdb9608d..90edc3bf 100644 --- a/sensor/marker_radar_lidar_calibrator/rviz/default.rviz +++ b/sensor/marker_radar_lidar_calibrator/rviz/default.rviz @@ -256,7 +256,19 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: " /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 diff --git a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index cf1b5d0a..2dcb67d7 100644 --- a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -1174,7 +1174,7 @@ std::tuple< ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta() { // Define two sets of 2D points (just 3D points with z=0) - // Note: pcs=paralell cordinate system rcs=radar coordinate system + // 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()); @@ -1252,7 +1252,7 @@ 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=paralell cordinate system rcs=radar coordinate system + // Note: pcs=parallel coordinate system rcs=radar coordinate system // Estimate full transformation using SVD pcl::registration::TransformationEstimationSVD estimator; Eigen::Matrix4f full_radar_to_radar_parallel_transformation; @@ -1405,7 +1405,7 @@ void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( pcl::PointCloud::Ptr lidar_points_pcs, pcl::PointCloud::Ptr radar_points_rcs) { - // Note: pcs=paralell cordinate system rcs=radar coordinate system + // Note: pcs=parallel coordinate system rcs=radar coordinate system int tracks_size = static_cast(converged_tracks_.size()); if (tracks_size <= 3) return; @@ -1530,7 +1530,7 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() output_metrics_.clear(); - // Note: pcs=paralell cordinate system rcs=radar coordinate system + // 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); From ca14dce9fc9efd40e5021d428000396cb1382644 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 26 Feb 2024 20:29:25 +0900 Subject: [PATCH 33/49] chore: forgot to ass the mapping base-lidar for the default project Signed-off-by: Kenzo Lobos-Tsunekawa --- ...ing_based_base_lidar_calibrator.launch.xml | 78 +++++++++++++++++++ .../calibrators/default_project/__init__.py | 2 + .../mapping_based_base_lidar_calibrator.py | 42 ++++++++++ 3 files changed, 122 insertions(+) create mode 100644 sensor/sensor_calibration_manager/launch/default_project/mapping_based_base_lidar_calibrator.launch.xml create mode 100644 sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/mapping_based_base_lidar_calibrator.py 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/sensor_calibration_manager/calibrators/default_project/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py index c09d5c9b..6736494d 100644 --- 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 @@ -1,5 +1,6 @@ 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 .tag_based_pnp_calibrator import TagBasedPNPCalibrator from .tag_based_sfm_base_lidar_calibrator import TagBasedSfmBaseLidarCalibrator @@ -9,6 +10,7 @@ __all__ = [ "GroundPlaneCalibrator", "LidarLidar2DCalibrator", + "MappingBasedBaseLidarCalibrator", "MappingBasedLidarLidarCalibrator", "TagBasedPNPCalibrator", "TagBasedSfmBaseLidarCalibrator", 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) + ], + ) From 7109aa3bfdeff036d4d59637a73c80f2f44fe482 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 18 Mar 2024 17:07:17 +0900 Subject: [PATCH 34/49] chore: deleted the new part of the service Signed-off-by: Kenzo Lobos-Tsunekawa --- common/tier4_calibration_msgs/CMakeLists.txt | 2 +- ...ewExtrinsicCalibrator.srv => ExtrinsicCalibrator.srv} | 0 .../ground_plane_calibrator/ground_plane_calibrator.hpp | 8 ++++---- .../src/ground_plane_calibrator.cpp | 7 +++---- .../interactive_camera_lidar_calibrator/ros_interface.py | 6 +++--- .../lidar_to_lidar_2d_calibrator.hpp | 8 ++++---- .../src/lidar_to_lidar_2d_calibrator.cpp | 7 +++---- .../mapping_based_calibrator.hpp | 8 ++++---- .../src/mapping_based_calibrator.cpp | 7 +++---- .../marker_radar_lidar_calibrator.hpp | 8 ++++---- .../src/marker_radar_lidar_calibrator.cpp | 7 +++---- .../sensor_calibration_manager/calibrator_wrapper.py | 4 ++-- .../sensor_calibration_manager/ros_interface.py | 6 +++--- .../tag_based_pnp_calibrator.hpp | 8 ++++---- .../src/tag_based_pnp_calibrator.cpp | 7 +++---- .../tag_based_sfm_calibrator.hpp | 9 ++++----- .../src/tag_based_sfm_calibrator.cpp | 8 +++----- 17 files changed, 51 insertions(+), 59 deletions(-) rename common/tier4_calibration_msgs/srv/{NewExtrinsicCalibrator.srv => ExtrinsicCalibrator.srv} (100%) diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt index 24753d21..4c03fb4d 100644 --- a/common/tier4_calibration_msgs/CMakeLists.txt +++ b/common/tier4_calibration_msgs/CMakeLists.txt @@ -28,7 +28,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/CalibrationDatabase.srv" "srv/Frame.srv" "srv/IntrinsicsOptimizer.srv" - "srv/NewExtrinsicCalibrator.srv" + "srv/ExtrinsicCalibrator.srv" DEPENDENCIES std_msgs sensor_msgs diff --git a/common/tier4_calibration_msgs/srv/NewExtrinsicCalibrator.srv b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv similarity index 100% rename from common/tier4_calibration_msgs/srv/NewExtrinsicCalibrator.srv rename to common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv diff --git a/sensor/ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp b/sensor/ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp index f8aa59bb..44ac2406 100644 --- a/sensor/ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp +++ b/sensor/ground_plane_calibrator/include/ground_plane_calibrator/ground_plane_calibrator.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include @@ -82,8 +82,8 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node * @param response A vector of calibration results */ void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); /*! * ROS pointcloud callback @@ -168,7 +168,7 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr pointcloud_sub_; - rclcpp::Service::SharedPtr service_server_; + rclcpp::Service::SharedPtr service_server_; // Threading, sync, and result std::mutex mutex_; diff --git a/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp index 1f13b372..63d74df5 100644 --- a/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp +++ b/sensor/ground_plane_calibrator/src/ground_plane_calibrator.cpp @@ -95,7 +95,7 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod // The service server runs in a dedicated thread srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - service_server_ = this->create_service( + service_server_ = this->create_service( "extrinsic_calibration", std::bind( &ExtrinsicGroundPlaneCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -115,10 +115,9 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod } void ExtrinsicGroundPlaneCalibrator::requestReceivedCallback( - [[maybe_unused]] const std::shared_ptr< - tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) + 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; 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 index 58dd12c8..a34f3a7c 100644 --- 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 @@ -28,7 +28,7 @@ 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 NewExtrinsicCalibrator +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 @@ -70,7 +70,7 @@ def __init__(self): if self.use_calibration_api: self.service_callback_group = MutuallyExclusiveCallbackGroup() self.calibration_api_service_server = self.create_service( - NewExtrinsicCalibrator, + ExtrinsicCalibrator, "extrinsic_calibration", self.calibration_api_service_callback, callback_group=self.service_callback_group, @@ -84,7 +84,7 @@ def send_calibration_api_result(self, calibration_error): self.calibration_error = calibration_error def calibration_api_service_callback( - self, request: NewExtrinsicCalibrator.Request, response: NewExtrinsicCalibrator.Response + self, request: ExtrinsicCalibrator.Request, response: ExtrinsicCalibrator.Response ): # Notify the UI that a request was received self.calibration_api_request_received_callback() 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 index 365d429a..2f806136 100644 --- 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 @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -77,8 +77,8 @@ class LidarToLidar2DCalibrator : public rclcpp::Node * @param response A vector of calibration results */ void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); /*! * Source pointcloud callback @@ -174,7 +174,7 @@ class LidarToLidar2DCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr source_pointcloud_sub_; rclcpp::Subscription::SharedPtr target_pointcloud_sub_; - rclcpp::Service::SharedPtr service_server_; + rclcpp::Service::SharedPtr service_server_; // Threading std::mutex mutex_; diff --git a/sensor/lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp b/sensor/lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp index 14f4b803..5381753b 100644 --- a/sensor/lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp +++ b/sensor/lidar_to_lidar_2d_calibrator/src/lidar_to_lidar_2d_calibrator.cpp @@ -103,7 +103,7 @@ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & o // 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( + service_server_ = this->create_service( "extrinsic_calibration", std::bind( &LidarToLidar2DCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -140,10 +140,9 @@ LidarToLidar2DCalibrator::LidarToLidar2DCalibrator(const rclcpp::NodeOptions & o } void LidarToLidar2DCalibrator::requestReceivedCallback( - [[maybe_unused]] const std::shared_ptr< - tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr response) { using std::chrono_literals::operator""s; diff --git a/sensor/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp index b7625446..89904eec 100644 --- a/sensor/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp +++ b/sensor/mapping_based_calibrator/include/mapping_based_calibrator/mapping_based_calibrator.hpp @@ -32,8 +32,8 @@ #include #include #include +#include #include -#include #include #include @@ -67,8 +67,8 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node * @param response A vector of calibration results */ void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); /*! * Message callback for detected objects @@ -116,7 +116,7 @@ class ExtrinsicMappingBasedCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr predicted_objects_sub_; - rclcpp::Service::SharedPtr service_server_; + 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_; diff --git a/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp b/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp index 07293c2a..0013db34 100644 --- a/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp @@ -323,7 +323,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - service_server_ = this->create_service( + service_server_ = this->create_service( "extrinsic_calibration", std::bind( &ExtrinsicMappingBasedCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -507,10 +507,9 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicMappingBasedCalibrator::paramC } void ExtrinsicMappingBasedCalibrator::requestReceivedCallback( - [[maybe_unused]] const std::shared_ptr< - tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr response) { using std::chrono_literals::operator""s; diff --git a/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp index d0a02b58..a23764cb 100644 --- a/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include @@ -63,8 +63,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node protected: void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); void timerCallback(); @@ -214,7 +214,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr lidar_sub_; rclcpp::Subscription::SharedPtr radar_sub_; - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr calibration_request_server_; rclcpp::Service::SharedPtr background_model_service_server_; rclcpp::Service::SharedPtr tracking_service_server_; diff --git a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index 2dcb67d7..ab9ed95d 100644 --- a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -261,7 +261,7 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( std::bind(&ExtrinsicReflectorBasedCalibrator::paramCallback, this, std::placeholders::_1)); calibration_request_server_ = - this->create_service( + this->create_service( "extrinsic_calibration", std::bind( &ExtrinsicReflectorBasedCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -281,10 +281,9 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( } void ExtrinsicReflectorBasedCalibrator::requestReceivedCallback( - [[maybe_unused]] const std::shared_ptr< - tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr response) { using std::chrono_literals::operator""s; diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py index 3b8eb70b..7817cc97 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py @@ -28,7 +28,7 @@ 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 NewExtrinsicCalibrator +from tier4_calibration_msgs.srv import ExtrinsicCalibrator class CalibratorServiceWrapper(QObject): @@ -160,7 +160,7 @@ def is_available(self) -> bool: def finished(self): return all(self.finished_list) - def result_ros_callback(self, result: NewExtrinsicCalibrator.Response): + def result_ros_callback(self, result: ExtrinsicCalibrator.Response): logging.debug(f"{threading.get_ident() }: result_ros_callback") self.result_signal.emit(result.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 index 165c02ef..e6f49c41 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/ros_interface.py @@ -24,7 +24,7 @@ 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 NewExtrinsicCalibrator +from tier4_calibration_msgs.srv import ExtrinsicCalibrator class RosInterface(Node): @@ -110,7 +110,7 @@ def timer_callback(self): def register_calibration_service(self, service_name, result_callback, status_callback): with self.lock: self.calibration_services_dict[service_name] = self.create_client( - NewExtrinsicCalibrator, service_name + ExtrinsicCalibrator, service_name ) self.calibration_result_callback_dict[service_name] = result_callback @@ -119,7 +119,7 @@ def register_calibration_service(self, service_name, result_callback, status_cal def call_calibration_service(self, service_name): with self.lock: - req = NewExtrinsicCalibrator.Request() + 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 diff --git a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp index e2761a7e..4eba5e61 100644 --- a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -66,8 +66,8 @@ class ExtrinsicTagBasedPNPCalibrator : public rclcpp::Node void clickedPointCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_msg); void requestReceivedCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); void tfTimerCallback(); void manualCalibrationTimerCallback(); @@ -117,7 +117,7 @@ class ExtrinsicTagBasedPNPCalibrator : public rclcpp::Node rclcpp::TimerBase::SharedPtr calib_timer_; rclcpp::TimerBase::SharedPtr tf_timer_; - rclcpp::Service::SharedPtr service_server_; + rclcpp::Service::SharedPtr service_server_; // Threading, sync, and result bool request_received_; diff --git a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp index 99b8c0a4..a5029e9b 100644 --- a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp @@ -146,7 +146,7 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // initialize service server - service_server_ = this->create_service( + service_server_ = this->create_service( "extrinsic_calibration", std::bind( &ExtrinsicTagBasedPNPCalibrator::requestReceivedCallback, this, std::placeholders::_1, @@ -276,10 +276,9 @@ void ExtrinsicTagBasedPNPCalibrator::cameraInfoCallback( } void ExtrinsicTagBasedPNPCalibrator::requestReceivedCallback( - [[maybe_unused]] const std::shared_ptr< - tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr response) { using std::chrono_literals::operator""s; RCLCPP_INFO(this->get_logger(), "Received calibration request"); diff --git a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp index 642eb8a8..d6915859 100644 --- a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/tag_based_sfm_calibrator.hpp @@ -31,9 +31,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -64,8 +64,8 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node * @param response the calibration response */ void calibrationRequestCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); /*! * Callback method for the image of the calibration cameras @@ -233,8 +233,7 @@ class ExtrinsicTagBasedBaseCalibrator : public rclcpp::Node // Calibration API related services rclcpp::CallbackGroup::SharedPtr calibration_api_srv_group_; - rclcpp::Service::SharedPtr - calibration_api_srv_; + rclcpp::Service::SharedPtr calibration_api_srv_; // Scene related services rclcpp::Service::SharedPtr diff --git a/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp index 7ecb8889..83dfb2e4 100644 --- a/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp @@ -293,7 +293,7 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( // The service servers runs in a dedicated threads since they are blocking calibration_api_srv_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - calibration_api_srv_ = this->create_service( + calibration_api_srv_ = this->create_service( "/extrinsic_calibration", std::bind( &ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback, this, std::placeholders::_1, @@ -377,11 +377,9 @@ ExtrinsicTagBasedBaseCalibrator::ExtrinsicTagBasedBaseCalibrator( } void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( - [[maybe_unused]] const std::shared_ptr< - tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Request> + [[maybe_unused]] const std::shared_ptr request, - [[maybe_unused]] const std::shared_ptr< - tier4_calibration_msgs::srv::NewExtrinsicCalibrator::Response> + [[maybe_unused]] const std::shared_ptr response) { using std::chrono_literals::operator""s; From 3f5684b669f5a730609bac1af8eeba749b942bfe Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 19 Mar 2024 13:48:45 +0900 Subject: [PATCH 35/49] feat: added the radar lidar calibrator to the default project Signed-off-by: Kenzo Lobos-Tsunekawa --- .../marker_radar_lidar_calibrator.launch.xml | 22 +++++++ .../calibrators/default_project/__init__.py | 2 + .../marker_radar_lidar_calibrator.py | 60 +++++++++++++++++++ 3 files changed, 84 insertions(+) create mode 100644 sensor/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml create mode 100644 sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py 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/sensor_calibration_manager/calibrators/default_project/__init__.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/__init__.py index 6736494d..530b5460 100644 --- 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 @@ -2,6 +2,7 @@ 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 @@ -12,6 +13,7 @@ "LidarLidar2DCalibrator", "MappingBasedBaseLidarCalibrator", "MappingBasedLidarLidarCalibrator", + "MarkerRadarLidarCalibrator", "TagBasedPNPCalibrator", "TagBasedSfmBaseLidarCalibrator", "TagBasedSfmBaseLidarsCalibrator", 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 From a23cea234f232fbd7c9b4a31c2976518f4e83918 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 19 Mar 2024 14:19:16 +0900 Subject: [PATCH 36/49] chore: replaced all instances of transl -> translation and updated the copyrights Signed-off-by: Kenzo Lobos-Tsunekawa --- .../joint_icp_extended.hpp | 2 +- .../joint_icp_extended_impl.hpp | 2 +- .../voxel_grid_triplets.hpp | 2 +- .../voxel_grid_triplets_impl.hpp | 2 +- .../src/joint_icp_extended.cpp | 2 +- .../src/voxel_grid_triplets.cpp | 2 +- .../tier4_tag_utils/apriltag_filter.hpp | 8 +- .../tier4_tag_utils/apriltag_hypothesis.hpp | 18 ++-- .../tier4_tag_utils/lidartag_filter.hpp | 10 +-- .../tier4_tag_utils/lidartag_hypothesis.hpp | 22 ++--- .../tier4_tag_utils/src/apriltag_filter.cpp | 14 ++-- .../src/apriltag_hypothesis.cpp | 38 +++++---- .../tier4_tag_utils/src/lidartag_filter.cpp | 17 ++-- .../src/lidartag_hypothesis.cpp | 82 ++++++++++--------- .../include/ground_plane_calibrator/utils.hpp | 2 +- sensor/ground_plane_calibrator/src/main.cpp | 2 +- .../lidar_to_lidar_2d_calibrator/src/main.cpp | 2 +- .../src/filters/best_frames_filter.cpp | 2 +- .../src/filters/dynamics_filter.cpp | 2 +- .../src/filters/lost_state_filter.cpp | 2 +- .../src/filters/object_detection_filter.cpp | 2 +- sensor/mapping_based_calibrator/src/main.cpp | 2 +- .../brute_force_matcher.hpp | 2 +- .../calibration_estimator.hpp | 35 ++++---- .../tag_based_pnp_calibrator.hpp | 2 +- .../tag_calibrator_visualizer.hpp | 10 +-- .../launch/calibrator.launch.xml | 20 ++--- .../src/brute_force_matcher.cpp | 2 +- .../src/calibration_estimator.cpp | 76 ++++++++--------- sensor/tag_based_pnp_calibrator/src/main.cpp | 2 +- .../src/tag_based_pnp_calibrator.cpp | 34 ++++---- .../src/tag_calibrator_visualizer.cpp | 16 ++-- .../launch/apriltag_detector.launch.xml | 8 +- .../launch/lidartag_detector.launch.xml | 8 +- 34 files changed, 235 insertions(+), 217 deletions(-) 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 9ca0077a..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. 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 da833b1f..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. 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 fc90da0f..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. 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_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 2342aa68..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. @@ -58,10 +58,10 @@ class ApriltagHypothesis 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: @@ -69,17 +69,17 @@ class ApriltagHypothesis void initKalman(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_; 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..bb7e492b 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,12 +46,12 @@ class LidartagFilter : public rclcpp::Node // Parameters double max_no_observation_time_; double new_hypothesis_distance_; - double new_hypothesis_transl_; + double new_hypothesis_translation_; double new_hypothesis_rot_; - double measurement_noise_transl_; + double measurement_noise_translation_; double measurement_noise_rot_; - double process_noise_transl_; - double process_noise_transl_dot_; + double process_noise_translation_; + double process_noise_translation_dot_; double process_noise_rot_; double process_noise_rot_dot_; 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 f2637a38..32c5f032 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. @@ -57,10 +57,10 @@ class LidartagHypothesis void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); void setMaxConvergenceThreshold( - double transl, double translation_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); + 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 rot, double rot_dot); protected: void reset(); @@ -81,23 +81,23 @@ class LidartagHypothesis cv::Matx31d rot2euler(const cv::Matx33d & rotation_matrix); cv::Matx33d euler2rot(const cv::Matx31d & euler); - double convergence_transl_; - double convergence_transl_dot_; + double convergence_translation_; + double convergence_translation_dot_; double convergence_rot_; double convergence_rot_dot_; - double new_hypothesis_transl_; + double new_hypothesis_translation_; double new_hypothesis_rot_; 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_translation_; + double process_noise_translation_dot_; double process_noise_rot_; double process_noise_rot_dot_; - double measurement_noise_transl_; + double measurement_noise_translation_; double measurement_noise_rot_; // General variables diff --git a/common/tier4_tag_utils/src/apriltag_filter.cpp b/common/tier4_tag_utils/src/apriltag_filter.cpp index 91c1eeca..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. @@ -36,9 +36,9 @@ 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"); @@ -144,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 22b0dde7..2421e451 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. @@ -43,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; @@ -184,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; @@ -201,18 +201,27 @@ 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) { - new_hypothesis_transl_ = max_transl; + new_hypothesis_translation_ = max_transl; } 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; } @@ -222,19 +231,20 @@ void ApriltagHypothesis::initKalman(const std::vector & corners) 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)); diff --git a/common/tier4_tag_utils/src/lidartag_filter.cpp b/common/tier4_tag_utils/src/lidartag_filter.cpp index ee460ab3..be8adac9 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. @@ -31,12 +31,12 @@ 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_translation_ = this->declare_parameter("new_hypothesis_translation"); new_hypothesis_rot_ = this->declare_parameter("new_hypothesis_rot"); - measurement_noise_transl_ = this->declare_parameter("measurement_noise_transl"); + measurement_noise_translation_ = this->declare_parameter("measurement_noise_translation"); 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_translation_ = this->declare_parameter("process_noise_translation"); + process_noise_translation_dot_ = this->declare_parameter("process_noise_translation_dot"); process_noise_rot_ = this->declare_parameter("process_noise_rot"); process_noise_rot_dot_ = this->declare_parameter("process_noise_rot_dot"); @@ -99,10 +99,11 @@ void LidartagFilter::updateHypothesis( 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_rot_); + h.setNewHypothesisThreshold(new_hypothesis_translation_, new_hypothesis_rot_); 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_rot_, + process_noise_rot_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 90a8703e..873f3535 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_rot_ || dt > max_no_observation_time_) { first_observation_timestamp_ = stamp; filtered_translation_vector_ = pose_translation; @@ -166,10 +166,10 @@ 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); + return std::sqrt(max_translation_cov); } double LidartagHypothesis::getRotCov() const @@ -193,18 +193,19 @@ 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 = 0.0; + double max_translation_dot_cov = 0.0; double max_rot_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_translation_cov) > convergence_translation_ || + std::sqrt(max_translation_dot_cov) > convergence_translation_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_dot_cov) > convergence_rot_dot_ || + getSpeed() > convergence_translation_dot_) { return false; } @@ -227,33 +228,33 @@ void LidartagHypothesis::setMinConvergenceTime(double convergence_time) } void LidartagHypothesis::setMaxConvergenceThreshold( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double translation_dot, double rot, double rot_dot) { - convergence_transl_ = transl; - convergence_transl_dot_ = transl_dot; + convergence_translation_ = translation; + convergence_translation_dot_ = translation_dot; convergence_rot_ = rot; convergence_rot_dot_ = rot_dot; } -void LidartagHypothesis::setNewHypothesisThreshold(double max_transl, double max_rot) +void LidartagHypothesis::setNewHypothesisThreshold(double max_translation, double max_rot) { - new_hypothesis_transl_ = max_transl; + new_hypothesis_translation_ = max_translation; new_hypothesis_rot_ = max_rot; } void LidartagHypothesis::setMaxNoObservationTime(double time) { max_no_observation_time_ = time; } -void LidartagHypothesis::setMeasurementNoise(double transl, double rot) +void LidartagHypothesis::setMeasurementNoise(double translation, double rot) { - measurement_noise_transl_ = transl; + measurement_noise_translation_ = translation; measurement_noise_rot_ = rot; } void LidartagHypothesis::setProcessNoise( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double translation_dot, double rot, double rot_dot) { - process_noise_transl_ = transl; - process_noise_transl_dot_ = transl_dot; + process_noise_translation_ = translation; + process_noise_translation_dot_ = translation_dot; process_noise_rot_ = rot; process_noise_rot_dot_ = rot_dot; } @@ -263,26 +264,27 @@ void LidartagHypothesis::initKalman( { kalman_filter_.init(6, 6, 0, CV_64F); - const double process_cov_transl = process_noise_transl_ * process_noise_transl_; + const double process_cov_translation = process_noise_translation_ * process_noise_translation_; const double process_cov_rot = process_noise_rot_ * process_noise_rot_; 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(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_rot; kalman_filter_.processNoiseCov.at(4, 4) = process_cov_rot; kalman_filter_.processNoiseCov.at(5, 5) = process_cov_rot; - const double measurement_cov_transl = measurement_noise_transl_ * measurement_noise_transl_; + const double measurement_cov_translation = + measurement_noise_translation_ * measurement_noise_translation_; const double measurement_cov_rot = measurement_noise_rot_ * measurement_noise_rot_; 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(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_rot; kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rot; kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rot; @@ -300,19 +302,20 @@ 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_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_rot = process_noise_rot_ * process_noise_rot_; const double process_cov_rot_dot = process_noise_rot_dot_ * process_noise_rot_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(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_rot; kalman_filter_.processNoiseCov.at(7, 7) = process_cov_rot; kalman_filter_.processNoiseCov.at(8, 8) = process_cov_rot; @@ -320,14 +323,15 @@ void LidartagHypothesis::initConstantVelocityKalman( 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_translation = + measurement_noise_translation_ * measurement_noise_translation_; const double measurement_cov_rot = measurement_noise_rot_ * measurement_noise_rot_; 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(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_rot; kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rot; kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rot; diff --git a/sensor/ground_plane_calibrator/include/ground_plane_calibrator/utils.hpp b/sensor/ground_plane_calibrator/include/ground_plane_calibrator/utils.hpp index bb92db14..fa2ecb10 100644 --- a/sensor/ground_plane_calibrator/include/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. diff --git a/sensor/ground_plane_calibrator/src/main.cpp b/sensor/ground_plane_calibrator/src/main.cpp index 08af84a5..f5b62376 100644 --- a/sensor/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. diff --git a/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp b/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp index 258b1290..7700f07d 100644 --- a/sensor/lidar_to_lidar_2d_calibrator/src/main.cpp +++ b/sensor/lidar_to_lidar_2d_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. diff --git a/sensor/mapping_based_calibrator/src/filters/best_frames_filter.cpp b/sensor/mapping_based_calibrator/src/filters/best_frames_filter.cpp index 6e1c6704..8a563f33 100644 --- a/sensor/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. diff --git a/sensor/mapping_based_calibrator/src/filters/dynamics_filter.cpp b/sensor/mapping_based_calibrator/src/filters/dynamics_filter.cpp index e1419555..20185f8a 100644 --- a/sensor/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. diff --git a/sensor/mapping_based_calibrator/src/filters/lost_state_filter.cpp b/sensor/mapping_based_calibrator/src/filters/lost_state_filter.cpp index 0765f335..deefbd07 100644 --- a/sensor/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. diff --git a/sensor/mapping_based_calibrator/src/filters/object_detection_filter.cpp b/sensor/mapping_based_calibrator/src/filters/object_detection_filter.cpp index 0d7abc4d..9fc7a6af 100644 --- a/sensor/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. diff --git a/sensor/mapping_based_calibrator/src/main.cpp b/sensor/mapping_based_calibrator/src/main.cpp index 8d81a21a..b0ab0a01 100644 --- a/sensor/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. diff --git a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/brute_force_matcher.hpp index 2ec2157c..53c4a1bd 100644 --- a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_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. diff --git a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp index e7754aeb..f92c35a2 100644 --- a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_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. @@ -79,15 +79,16 @@ class CalibrationEstimator void setTagSizes(std::vector & tag_id, std::vector & tag_sizes); void setLidartagMaxConvergenceThreshold( - double transl, double transl_dot, double angle, double angle_dot); + double translation, double translation_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); + void setLidartagProcessNoise( + double translation, double translation_dot, double rot, double rot_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; @@ -125,25 +126,25 @@ class CalibrationEstimator double new_hypothesis_distance_; // Lidartag estimation parameters - double lidartag_convergence_transl_; - double lidartag_convergence_transl_dot_; + double lidartag_convergence_translation_; + double lidartag_convergence_translation_dot_; double lidartag_convergence_rot_; double lidartag_convergence_rot_dot_; - double lidartag_new_hypothesis_transl_; + double lidartag_new_hypothesis_translation_; double lidartag_new_hypothesis_rot_; - double lidartag_process_noise_transl_; - double lidartag_process_noise_transl_dot_; + double lidartag_process_noise_translation_; + double lidartag_process_noise_translation_dot_; double lidartag_process_noise_rot_; double lidartag_process_noise_rot_dot_; - double lidartag_measurement_noise_transl_; + double lidartag_measurement_noise_translation_; double lidartag_measurement_noise_rot_; // 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_; diff --git a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_calibrator.hpp index 4eba5e61..aa266f9e 100644 --- a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_based_pnp_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. diff --git a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp index deb31008..d22dbd29 100644 --- a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_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. @@ -74,7 +74,7 @@ class TagCalibratorVisualizer void setMaxNoObservationTime(double time); void setLidartagMaxConvergenceThreshold( double transl, double transl_dot, double angle, double angle_dot); - void setApriltagMaxConvergenceThreshold(double transl); + 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_translation_; + double lidartag_convergence_translation_dot_; double lidartag_convergence_rot_; double lidartag_convergence_rot_dot_; - double apriltag_convergence_transl_; + double apriltag_convergence_translation_; bool valid_base_lidar_transform_; bool valid_camera_lidar_transform_; diff --git a/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml index 330f900d..8ba647d7 100644 --- a/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml +++ b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -47,24 +47,24 @@ - - + + - + - + - - + + - - + + - - + + diff --git a/sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp b/sensor/tag_based_pnp_calibrator/src/brute_force_matcher.cpp index 2d4a4e8f..16c6d415 100644 --- a/sensor/tag_based_pnp_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. diff --git a/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp index bea31c35..54055d54 100644 --- a/sensor/tag_based_pnp_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. @@ -30,22 +30,22 @@ CalibrationEstimator::CalibrationEstimator() : 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_translation_(0.05), + lidartag_convergence_translation_dot_(0.001), lidartag_convergence_rot_(), lidartag_convergence_rot_dot_(0.0001), - lidartag_new_hypothesis_transl_(0.2), + lidartag_new_hypothesis_translation_(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_translation_(0.005), + lidartag_process_noise_translation_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_translation_(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), + 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) { @@ -81,12 +81,12 @@ void CalibrationEstimator::update( // 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); @@ -138,13 +138,15 @@ void CalibrationEstimator::update( 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_rot_, lidartag_convergence_rot_dot_); + new_h->setMeasurementNoise( + lidartag_measurement_noise_translation_, lidartag_measurement_noise_rot_); + new_h->setNewHypothesisThreshold( + lidartag_new_hypothesis_translation_, lidartag_new_hypothesis_rot_); 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_rot_, lidartag_process_noise_rot_dot_); new_h->update(translation_cv, rotation_cv, detection.size, stamp); // 2) Compare with converged hypotheses @@ -686,53 +688,53 @@ void CalibrationEstimator::setTagSizes( } void CalibrationEstimator::setLidartagMaxConvergenceThreshold( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double transl_dot, double rot, double rot_dot) { - lidartag_convergence_transl_ = transl; - lidartag_convergence_transl_dot_ = transl_dot; + lidartag_convergence_translation_ = translation; + lidartag_convergence_translation_dot_ = transl_dot; lidartag_convergence_rot_ = CV_PI * rot / 180.0; lidartag_convergence_rot_dot_ = CV_PI * rot_dot / 180.0; } void CalibrationEstimator::setLidartagNewHypothesisThreshold(double max_transl, double max_rot) { - lidartag_new_hypothesis_transl_ = max_transl; + lidartag_new_hypothesis_translation_ = max_transl; lidartag_new_hypothesis_rot_ = CV_PI * max_rot / 180.0; } -void CalibrationEstimator::setLidartagMeasurementNoise(double transl, double rot) +void CalibrationEstimator::setLidartagMeasurementNoise(double translation, double rot) { - lidartag_measurement_noise_transl_ = transl; + lidartag_measurement_noise_translation_ = translation; lidartag_measurement_noise_rot_ = CV_PI * rot / 180.0; } void CalibrationEstimator::setLidartagProcessNoise( - double transl, double transl_dot, double rot, double rot_dot) + double translation, double translation_dot, double rot, double rot_dot) { - lidartag_process_noise_transl_ = transl; - lidartag_process_noise_transl_dot_ = transl_dot; + lidartag_process_noise_translation_ = translation; + lidartag_process_noise_translation_dot_ = translation_dot; lidartag_process_noise_rot_ = CV_PI * rot / 180.0; lidartag_process_noise_rot_dot_ = CV_PI * rot_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) { - apriltag_new_hypothesis_transl_ = max_transl; + apriltag_new_hypothesis_translation_ = max_transl; } -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_; } diff --git a/sensor/tag_based_pnp_calibrator/src/main.cpp b/sensor/tag_based_pnp_calibrator/src/main.cpp index bdb0c3a5..9be49c1e 100644 --- a/sensor/tag_based_pnp_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. diff --git a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp index a5029e9b..e94a0106 100644 --- a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_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. @@ -56,37 +56,37 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod 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"); + 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_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"); + this->declare_parameter("lidartag_new_hypothesis_translation"); double lidartag_new_hypothesis_rot = this->declare_parameter("lidartag_new_hypothesis_rot"); double lidartag_measurement_noise_transl = - this->declare_parameter("lidartag_measurement_noise_transl"); + this->declare_parameter("lidartag_measurement_noise_translation"); 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"); + 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_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"); + this->declare_parameter("apriltag_max_convergence_translation"); double apriltag_new_hypothesis_transl = - this->declare_parameter("apriltag_new_hypothesis_transl"); + this->declare_parameter("apriltag_new_hypothesis_translation"); double apriltag_measurement_noise_transl = - this->declare_parameter("apriltag_measurement_noise_transl"); + this->declare_parameter("apriltag_measurement_noise_translation"); double apriltag_process_noise_transl = - this->declare_parameter("apriltag_process_noise_transl"); + this->declare_parameter("apriltag_process_noise_translation"); camera_info_sub_ = this->create_subscription( "camera_info", rclcpp::QoS(1).best_effort(), @@ -120,15 +120,15 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod estimator_.setTagSizes(tag_ids, tag_sizes); estimator_.setLidartagMaxConvergenceThreshold( - lidartag_max_convergence_transl, lidartag_max_convergence_transl_dot, + lidartag_max_convergence_transl, lidartag_max_convergence_translation_dot, lidartag_max_convergence_rot, lidartag_max_convergence_rot_dot); estimator_.setLidartagNewHypothesisThreshold( lidartag_new_hypothesis_transl, lidartag_new_hypothesis_rot); estimator_.setLidartagMeasurementNoise( lidartag_measurement_noise_transl, lidartag_measurement_noise_rot); estimator_.setLidartagProcessNoise( - lidartag_process_noise_transl, lidartag_process_noise_transl_dot, lidartag_process_noise_rot, - lidartag_process_noise_rot_dot); + lidartag_process_noise_transl, lidartag_process_noise_translation_dot, + lidartag_process_noise_rot, lidartag_process_noise_rot_dot); estimator_.setApriltagMaxConvergenceThreshold(apriltag_max_convergence_transl); estimator_.setApriltagNewHypothesisThreshold(apriltag_new_hypothesis_transl); @@ -161,7 +161,7 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod 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_transl, lidartag_max_convergence_translation_dot, lidartag_max_convergence_rot, lidartag_max_convergence_rot_dot); visualizer_->setApriltagMaxConvergenceThreshold(apriltag_max_convergence_transl); } diff --git a/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index fe6d659d..aae19ace 100644 --- a/sensor/tag_based_pnp_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. @@ -440,11 +440,11 @@ 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) + + to_string_with_precision(lidartag_convergence_translation_, 3) + "\nrot_cov=" + to_string_with_precision(rot_cov, 3) + "/" + to_string_with_precision(lidartag_convergence_rot_, 3) + "\nspeed=" + to_string_with_precision(speed, 3) + "/" + - to_string_with_precision(lidartag_convergence_transl_dot_, 3) + + to_string_with_precision(lidartag_convergence_translation_dot_, 3) + to_string_with_precision(lidartag_convergence_rot_dot_, 3); text_marker.pose.position.x = center_base(0); @@ -903,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 rot, double rot_dot) { - lidartag_convergence_transl_ = transl; - lidartag_convergence_transl_dot_ = transl_dot; + lidartag_convergence_translation_ = translation; + lidartag_convergence_translation_dot_ = translation_dot; lidartag_convergence_rot_ = CV_PI * rot / 180.0; lidartag_convergence_rot_dot_ = CV_PI * rot_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/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml index 9b78865e..5a5a9708 100644 --- a/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml +++ b/sensor/tag_based_sfm_calibrator/launch/apriltag_detector.launch.xml @@ -34,11 +34,11 @@ - - + + - - + + diff --git a/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml b/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml index 01630092..e922aeef 100644 --- a/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml +++ b/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml @@ -18,12 +18,12 @@ - + - + - - + + From 56bd2f0b42352f2255c57cbab928dbe97b18e1bc Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 19 Mar 2024 14:56:21 +0900 Subject: [PATCH 37/49] chore: replaced rot -> rotation and missing transl -> translation Signed-off-by: Kenzo Lobos-Tsunekawa --- .../tier4_calibration_views/utils.py | 4 +- .../tier4_tag_utils/lidartag_filter.hpp | 8 +- .../tier4_tag_utils/lidartag_hypothesis.hpp | 15 ++-- .../src/apriltag_hypothesis.cpp | 4 +- .../tier4_tag_utils/src/lidartag_filter.cpp | 16 ++-- .../src/lidartag_hypothesis.cpp | 74 ++++++++-------- .../src/calibration_mapper.cpp | 23 ++--- .../sensor_calibration_manager/utils.py | 4 +- .../calibration_estimator.hpp | 18 ++-- .../tag_calibrator_visualizer.hpp | 6 +- .../launch/calibrator.launch.xml | 12 +-- .../src/calibration_estimator.cpp | 51 +++++------ .../src/tag_based_pnp_calibrator.cpp | 87 ++++++++++--------- .../src/tag_calibrator_visualizer.cpp | 14 +-- .../ceres/camera_residual.hpp | 61 ++++++------- .../launch/lidartag_detector.launch.xml | 8 +- .../src/apriltag_detection.cpp | 8 +- .../src/ceres/calibration_problem.cpp | 30 +++---- .../src/tag_based_sfm_calibrator.cpp | 8 +- 19 files changed, 230 insertions(+), 221 deletions(-) diff --git a/common/tier4_calibration_views/tier4_calibration_views/utils.py b/common/tier4_calibration_views/tier4_calibration_views/utils.py index efd98887..81f4a5fb 100644 --- a/common/tier4_calibration_views/tier4_calibration_views/utils.py +++ b/common/tier4_calibration_views/tier4_calibration_views/utils.py @@ -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 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 bb7e492b..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 @@ -47,13 +47,13 @@ class LidartagFilter : public rclcpp::Node double max_no_observation_time_; double new_hypothesis_distance_; double new_hypothesis_translation_; - double new_hypothesis_rot_; + double new_hypothesis_rotation_; double measurement_noise_translation_; - double measurement_noise_rot_; + double measurement_noise_rotation_; double process_noise_translation_; double process_noise_translation_dot_; - double process_noise_rot_; - double process_noise_rot_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 32c5f032..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 @@ -60,7 +60,8 @@ class LidartagHypothesis 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 rot, double rot_dot); + void setProcessNoise( + double translation, double translation_dot, double rotation, double rotation_dot); protected: void reset(); @@ -83,10 +84,10 @@ class LidartagHypothesis double convergence_translation_; double convergence_translation_dot_; - double convergence_rot_; - double convergence_rot_dot_; + double convergence_rotation_; + double convergence_rotation_dot_; double new_hypothesis_translation_; - double new_hypothesis_rot_; + double new_hypothesis_rotation_; double min_convergence_time_; double max_no_observation_time_; @@ -94,11 +95,11 @@ class LidartagHypothesis cv::KalmanFilter kalman_filter_; double process_noise_translation_; double process_noise_translation_dot_; - double process_noise_rot_; - double process_noise_rot_dot_; + double process_noise_rotation_; + double process_noise_rotation_dot_; double measurement_noise_translation_; - double measurement_noise_rot_; + double measurement_noise_rotation_; // General variables int id_; diff --git a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp index 2421e451..a149e192 100644 --- a/common/tier4_tag_utils/src/apriltag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/apriltag_hypothesis.cpp @@ -206,9 +206,9 @@ void ApriltagHypothesis::setMaxConvergenceThreshold(double translation) convergence_translation_ = translation; } -void ApriltagHypothesis::setNewHypothesisThreshold(double max_transl) +void ApriltagHypothesis::setNewHypothesisThreshold(double max_translation) { - new_hypothesis_translation_ = max_transl; + new_hypothesis_translation_ = max_translation; } void ApriltagHypothesis::setMaxNoObservationTime(double time) { max_no_observation_time_ = time; } diff --git a/common/tier4_tag_utils/src/lidartag_filter.cpp b/common/tier4_tag_utils/src/lidartag_filter.cpp index be8adac9..207e0d29 100644 --- a/common/tier4_tag_utils/src/lidartag_filter.cpp +++ b/common/tier4_tag_utils/src/lidartag_filter.cpp @@ -32,13 +32,13 @@ LidartagFilter::LidartagFilter(const rclcpp::NodeOptions & options) new_hypothesis_distance_ = this->declare_parameter("new_hypothesis_distance"); new_hypothesis_translation_ = this->declare_parameter("new_hypothesis_translation"); - new_hypothesis_rot_ = this->declare_parameter("new_hypothesis_rot"); + new_hypothesis_rotation_ = this->declare_parameter("new_hypothesis_rotation"); measurement_noise_translation_ = this->declare_parameter("measurement_noise_translation"); - measurement_noise_rot_ = this->declare_parameter("measurement_noise_rot"); + 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_rot_ = this->declare_parameter("process_noise_rot"); - process_noise_rot_dot_ = this->declare_parameter("process_noise_rot_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, @@ -99,11 +99,11 @@ void LidartagFilter::updateHypothesis( h.setMinConvergenceTime(std::numeric_limits::max()); h.setMaxConvergenceThreshold(0.0, 0.0, 0.0, 0.0); - h.setMeasurementNoise(measurement_noise_translation_, measurement_noise_rot_); - h.setNewHypothesisThreshold(new_hypothesis_translation_, new_hypothesis_rot_); + h.setMeasurementNoise(measurement_noise_translation_, measurement_noise_rotation_); + h.setNewHypothesisThreshold(new_hypothesis_translation_, new_hypothesis_rotation_); h.setProcessNoise( - process_noise_translation_, process_noise_translation_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 873f3535..4b2e5cfb 100644 --- a/common/tier4_tag_utils/src/lidartag_hypothesis.cpp +++ b/common/tier4_tag_utils/src/lidartag_hypothesis.cpp @@ -48,7 +48,7 @@ bool LidartagHypothesis::update( initKalman(pose_translation, pose_rotation); return true; } else if ( - trans_diff > new_hypothesis_translation_ || 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; @@ -175,8 +175,9 @@ double LidartagHypothesis::getTransCov() const double LidartagHypothesis::getRotCov() const { const cv::Mat & cov = kalman_filter_.errorCovPost; - double max_rot_cov = std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); - 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::getSpeed() const { return estimated_speed_; } @@ -198,13 +199,14 @@ bool LidartagHypothesis::converged() const double max_translation_dot_cov = 0.0; - double max_rot_cov = std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); + double max_rotation_cov = + std::max({cov.at(3, 3), cov.at(4, 4), cov.at(5, 5)}); if ( std::sqrt(max_translation_cov) > convergence_translation_ || std::sqrt(max_translation_dot_cov) > convergence_translation_dot_ || - std::sqrt(max_rot_cov) > convergence_rot_ || - std::sqrt(max_translation_dot_cov) > convergence_rot_dot_ || + std::sqrt(max_rotation_cov) > convergence_rotation_ || + std::sqrt(max_translation_dot_cov) > convergence_rotation_dot_ || getSpeed() > convergence_translation_dot_) { return false; } @@ -228,35 +230,35 @@ void LidartagHypothesis::setMinConvergenceTime(double convergence_time) } void LidartagHypothesis::setMaxConvergenceThreshold( - double translation, double translation_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { convergence_translation_ = translation; convergence_translation_dot_ = translation_dot; - convergence_rot_ = rot; - convergence_rot_dot_ = rot_dot; + convergence_rotation_ = rotation; + convergence_rotation_dot_ = rotation_dot; } -void LidartagHypothesis::setNewHypothesisThreshold(double max_translation, double max_rot) +void LidartagHypothesis::setNewHypothesisThreshold(double max_translation, double max_rotation) { new_hypothesis_translation_ = max_translation; - new_hypothesis_rot_ = max_rot; + new_hypothesis_rotation_ = max_rotation; } void LidartagHypothesis::setMaxNoObservationTime(double time) { max_no_observation_time_ = time; } -void LidartagHypothesis::setMeasurementNoise(double translation, double rot) +void LidartagHypothesis::setMeasurementNoise(double translation, double rotation) { measurement_noise_translation_ = translation; - measurement_noise_rot_ = rot; + measurement_noise_rotation_ = rotation; } void LidartagHypothesis::setProcessNoise( - double translation, double translation_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { process_noise_translation_ = translation; process_noise_translation_dot_ = translation_dot; - process_noise_rot_ = rot; - process_noise_rot_dot_ = rot_dot; + process_noise_rotation_ = rotation; + process_noise_rotation_dot_ = rotation_dot; } void LidartagHypothesis::initKalman( @@ -265,29 +267,29 @@ void LidartagHypothesis::initKalman( kalman_filter_.init(6, 6, 0, CV_64F); const double process_cov_translation = process_noise_translation_ * process_noise_translation_; - const double process_cov_rot = process_noise_rot_ * process_noise_rot_; + 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_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_rot; - kalman_filter_.processNoiseCov.at(4, 4) = process_cov_rot; - kalman_filter_.processNoiseCov.at(5, 5) = process_cov_rot; + 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_translation = measurement_noise_translation_ * measurement_noise_translation_; - const double measurement_cov_rot = measurement_noise_rot_ * measurement_noise_rot_; + 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_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_rot; - kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rot; - kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rot; + 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)); @@ -305,8 +307,8 @@ void LidartagHypothesis::initConstantVelocityKalman( 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_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_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)); @@ -316,25 +318,25 @@ void LidartagHypothesis::initConstantVelocityKalman( 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_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; + 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_rot = measurement_noise_rot_ * measurement_noise_rot_; + 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_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_rot; - kalman_filter_.measurementNoiseCov.at(4, 4) = measurement_cov_rot; - kalman_filter_.measurementNoiseCov.at(5, 5) = measurement_cov_rot; + 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)); diff --git a/sensor/mapping_based_calibrator/src/calibration_mapper.cpp b/sensor/mapping_based_calibrator/src/calibration_mapper.cpp index ff4f5b98..1ee056dc 100644 --- a/sensor/mapping_based_calibrator/src/calibration_mapper.cpp +++ b/sensor/mapping_based_calibrator/src/calibration_mapper.cpp @@ -442,27 +442,28 @@ void CalibrationMapper::checkKeyframeLost(Frame::Ptr keyframe) 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 = delta_pose2.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 = + 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) " "threshold=%.2f (deg)", - trans_angle_diff, rot_angle_diff, parameters_->lost_frame_max_angle_diff_); + translation_angle_diff, rotation_angle_diff, parameters_->lost_frame_max_angle_diff_); return; } @@ -482,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() - @@ -491,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; } diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py index 53121c62..c79e0835 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/utils.py @@ -23,9 +23,9 @@ def tf_message_to_transform_matrix(msg): transform_matrix = np.eye(4) q = msg.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.translation.x transform_matrix[1, 3] = msg.translation.y transform_matrix[2, 3] = msg.translation.z diff --git a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp index f92c35a2..8447d96a 100644 --- a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp @@ -80,10 +80,10 @@ class CalibrationEstimator void setLidartagMaxConvergenceThreshold( double translation, double translation_dot, double angle, double angle_dot); - void setLidartagNewHypothesisThreshold(double transl, double angle); - void setLidartagMeasurementNoise(double transl, double angle); + void setLidartagNewHypothesisThreshold(double translation, double angle); + void setLidartagMeasurementNoise(double translation, double angle); void setLidartagProcessNoise( - double translation, double translation_dot, double rot, double rot_dot); + double translation, double translation_dot, double rotation, double rotation_dot); void setApriltagMaxConvergenceThreshold(double translation); void setApriltagNewHypothesisThreshold(double translation); @@ -128,17 +128,17 @@ class CalibrationEstimator // Lidartag estimation parameters double lidartag_convergence_translation_; double lidartag_convergence_translation_dot_; - double lidartag_convergence_rot_; - double lidartag_convergence_rot_dot_; + double lidartag_convergence_rotation_; + double lidartag_convergence_rotation_dot_; double lidartag_new_hypothesis_translation_; - double lidartag_new_hypothesis_rot_; + double lidartag_new_hypothesis_rotation_; double lidartag_process_noise_translation_; double lidartag_process_noise_translation_dot_; - double lidartag_process_noise_rot_; - double lidartag_process_noise_rot_dot_; + double lidartag_process_noise_rotation_; + double lidartag_process_noise_rotation_dot_; double lidartag_measurement_noise_translation_; - double lidartag_measurement_noise_rot_; + double lidartag_measurement_noise_rotation_; // Apriltag estimation parameters double apriltag_convergence_translation_; diff --git a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp index d22dbd29..b3a376ce 100644 --- a/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp +++ b/sensor/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp @@ -73,7 +73,7 @@ class TagCalibratorVisualizer void setMinConvergenceTime(double convergence_time); void setMaxNoObservationTime(double time); void setLidartagMaxConvergenceThreshold( - double transl, double transl_dot, double angle, double angle_dot); + double translation, double transl_dot, double angle, double angle_dot); void setApriltagMaxConvergenceThreshold(double translation); private: @@ -96,8 +96,8 @@ class TagCalibratorVisualizer double max_no_observation_time_; double lidartag_convergence_translation_; double lidartag_convergence_translation_dot_; - double lidartag_convergence_rot_; - double lidartag_convergence_rot_dot_; + double lidartag_convergence_rotation_; + double lidartag_convergence_rotation_dot_; double apriltag_convergence_translation_; bool valid_base_lidar_transform_; diff --git a/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml index 8ba647d7..4cec786a 100644 --- a/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml +++ b/sensor/tag_based_pnp_calibrator/launch/calibrator.launch.xml @@ -49,16 +49,16 @@ - - + + - + - + - - + + diff --git a/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp index 54055d54..ca6e4be6 100644 --- a/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/calibration_estimator.cpp @@ -32,16 +32,16 @@ CalibrationEstimator::CalibrationEstimator() max_no_observation_time_(2.0), lidartag_convergence_translation_(0.05), lidartag_convergence_translation_dot_(0.001), - lidartag_convergence_rot_(), - lidartag_convergence_rot_dot_(0.0001), + lidartag_convergence_rotation_(), + lidartag_convergence_rotation_dot_(0.0001), lidartag_new_hypothesis_translation_(0.2), - lidartag_new_hypothesis_rot_(CV_PI * 45 / 180.0), + 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_rot_(CV_PI * 0.5 / 180.0), - lidartag_process_noise_rot_dot_(CV_PI * 0.5 / 180.0), + 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_rot_(CV_PI * 5 / 180.0), + 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), @@ -139,14 +139,14 @@ void CalibrationEstimator::update( new_h->setMaxConvergenceThreshold( lidartag_convergence_translation_, lidartag_convergence_translation_dot_, - lidartag_convergence_rot_, lidartag_convergence_rot_dot_); + lidartag_convergence_rotation_, lidartag_convergence_rotation_dot_); new_h->setMeasurementNoise( - lidartag_measurement_noise_translation_, lidartag_measurement_noise_rot_); + lidartag_measurement_noise_translation_, lidartag_measurement_noise_rotation_); new_h->setNewHypothesisThreshold( - lidartag_new_hypothesis_translation_, lidartag_new_hypothesis_rot_); + lidartag_new_hypothesis_translation_, lidartag_new_hypothesis_rotation_); new_h->setProcessNoise( lidartag_process_noise_translation_, lidartag_process_noise_translation_dot_, - lidartag_process_noise_rot_, lidartag_process_noise_rot_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 @@ -507,12 +507,12 @@ 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::computeCrossValidationReprojectionError( @@ -688,33 +688,34 @@ void CalibrationEstimator::setTagSizes( } void CalibrationEstimator::setLidartagMaxConvergenceThreshold( - double translation, double transl_dot, double rot, double rot_dot) + double translation, double transl_dot, double rotation, double rotation_dot) { lidartag_convergence_translation_ = translation; lidartag_convergence_translation_dot_ = transl_dot; - lidartag_convergence_rot_ = CV_PI * rot / 180.0; - lidartag_convergence_rot_dot_ = CV_PI * rot_dot / 180.0; + 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_translation_ = 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 translation, double rot) +void CalibrationEstimator::setLidartagMeasurementNoise(double translation, double rotation) { lidartag_measurement_noise_translation_ = translation; - lidartag_measurement_noise_rot_ = CV_PI * rot / 180.0; + lidartag_measurement_noise_rotation_ = CV_PI * rotation / 180.0; } void CalibrationEstimator::setLidartagProcessNoise( - double translation, double translation_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { lidartag_process_noise_translation_ = translation; lidartag_process_noise_translation_dot_ = translation_dot; - lidartag_process_noise_rot_ = CV_PI * rot / 180.0; - lidartag_process_noise_rot_dot_ = CV_PI * rot_dot / 180.0; + lidartag_process_noise_rotation_ = CV_PI * rotation / 180.0; + lidartag_process_noise_rotation_dot_ = CV_PI * rotation_dot / 180.0; } void CalibrationEstimator::setApriltagMaxConvergenceThreshold(double translation) @@ -722,9 +723,9 @@ void CalibrationEstimator::setApriltagMaxConvergenceThreshold(double translation apriltag_convergence_translation_ = translation; } -void CalibrationEstimator::setApriltagNewHypothesisThreshold(double max_transl) +void CalibrationEstimator::setApriltagNewHypothesisThreshold(double max_translation) { - apriltag_new_hypothesis_translation_ = max_transl; + apriltag_new_hypothesis_translation_ = max_translation; } void CalibrationEstimator::setApriltagMeasurementNoise(double translation) diff --git a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp index e94a0106..b46997ad 100644 --- a/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_based_pnp_calibrator.cpp @@ -55,37 +55,38 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod std::vector tag_ids = this->declare_parameter>("tag_ids"); std::vector tag_sizes = this->declare_parameter>("tag_sizes"); - double lidartag_max_convergence_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_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 = + 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_rot = - this->declare_parameter("lidartag_new_hypothesis_rot"); - double lidartag_measurement_noise_transl = + 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_rot = - this->declare_parameter("lidartag_measurement_noise_rot"); - double lidartag_process_noise_transl = + 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_rot = this->declare_parameter("lidartag_process_noise_rot"); - double lidartag_process_noise_rot_dot = - this->declare_parameter("lidartag_process_noise_rot_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_transl = + double apriltag_max_convergence_translation = this->declare_parameter("apriltag_max_convergence_translation"); - double apriltag_new_hypothesis_transl = + double apriltag_new_hypothesis_translation = this->declare_parameter("apriltag_new_hypothesis_translation"); - double apriltag_measurement_noise_transl = + double apriltag_measurement_noise_translation = this->declare_parameter("apriltag_measurement_noise_translation"); - double apriltag_process_noise_transl = + double apriltag_process_noise_translation = this->declare_parameter("apriltag_process_noise_translation"); camera_info_sub_ = this->create_subscription( @@ -120,20 +121,20 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod estimator_.setTagSizes(tag_ids, tag_sizes); estimator_.setLidartagMaxConvergenceThreshold( - lidartag_max_convergence_transl, lidartag_max_convergence_translation_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_translation_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); tf_timer_ = rclcpp::create_timer( this, get_clock(), std::chrono::duration(1.0 / calib_rate_), @@ -161,9 +162,9 @@ ExtrinsicTagBasedPNPCalibrator::ExtrinsicTagBasedPNPCalibrator(const rclcpp::Nod visualizer_->setMinConvergenceTime(min_convergence_time); visualizer_->setMaxNoObservationTime(max_no_observation_time); visualizer_->setLidartagMaxConvergenceThreshold( - lidartag_max_convergence_transl, lidartag_max_convergence_translation_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 ExtrinsicTagBasedPNPCalibrator::lidarTagDetectionsCallback( @@ -333,17 +334,17 @@ void ExtrinsicTagBasedPNPCalibrator::tfTimerCallback() 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) { @@ -380,27 +381,27 @@ void ExtrinsicTagBasedPNPCalibrator::automaticCalibrationTimerCallback() if (estimator_.calibrate()) { // Visualization - cv::Matx33d initial_rot_matrix; + cv::Matx33d initial_rotation_matrix; cv::Matx31d initial_trans_vector; - auto [current_trans_vector, current_rot_matrix] = estimator_.getCurrentPose(); - auto [filtered_trans_vector, filtered_rot_matrix] = estimator_.getFilteredPose(); + 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; diff --git a/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp index aae19ace..fbde860b 100644 --- a/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp +++ b/sensor/tag_based_pnp_calibrator/src/tag_calibrator_visualizer.cpp @@ -424,7 +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 rotation_cov = h->getRotCov(); const double speed = h->getSpeed(); cv::Matx31d center_base(center.x, center.y, center.z); @@ -441,11 +441,11 @@ void TagCalibratorVisualizer::drawCalibrationStatusText( "/" + to_string_with_precision(max_no_observation_time_) + "\ntrans_cov=" + to_string_with_precision(trans_cov, 3) + "/" + to_string_with_precision(lidartag_convergence_translation_, 3) + - "\nrot_cov=" + to_string_with_precision(rot_cov, 3) + "/" + - to_string_with_precision(lidartag_convergence_rot_, 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_translation_dot_, 3) + - to_string_with_precision(lidartag_convergence_rot_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); @@ -903,12 +903,12 @@ void TagCalibratorVisualizer::setMaxNoObservationTime(double time) } void TagCalibratorVisualizer::setLidartagMaxConvergenceThreshold( - double translation, double translation_dot, double rot, double rot_dot) + double translation, double translation_dot, double rotation, double rotation_dot) { lidartag_convergence_translation_ = translation; lidartag_convergence_translation_dot_ = translation_dot; - lidartag_convergence_rot_ = CV_PI * rot / 180.0; - lidartag_convergence_rot_dot_ = CV_PI * rot_dot / 180.0; + lidartag_convergence_rotation_ = CV_PI * rotation / 180.0; + lidartag_convergence_rotation_dot_ = CV_PI * rotation_dot / 180.0; } void TagCalibratorVisualizer::setApriltagMaxConvergenceThreshold(double translation) diff --git a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp index 56405b30..5f183bc6 100644 --- a/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp +++ b/sensor/tag_based_sfm_calibrator/include/tag_based_sfm_calibrator/ceres/camera_residual.hpp @@ -37,14 +37,14 @@ struct CameraResidual : public SensorResidual const ApriltagDetection & detection, const std::array & fixed_camera_pose_inv, const std::array & - fixed_tag_rot_z, // cSpell:ignore SHRD - bool fix_camera_pose, bool fix_tag_rot_z, bool optimize_intrinsics, bool is_ground_tag) + 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) { @@ -237,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; @@ -254,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 @@ -267,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); @@ -276,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); @@ -324,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); } } @@ -379,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< @@ -430,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, @@ -489,9 +492,9 @@ 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_; }; diff --git a/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml b/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml index e922aeef..02c0f144 100644 --- a/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml +++ b/sensor/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml @@ -19,13 +19,13 @@ - + - + - - + + diff --git a/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp b/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp index 57a6c24b..cbdc8b73 100644 --- a/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp +++ b/sensor/tag_based_sfm_calibrator/src/apriltag_detection.cpp @@ -39,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; @@ -51,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; diff --git a/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp index 29a9503d..b5fe2a58 100644 --- a/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp +++ b/sensor/tag_based_sfm_calibrator/src/ceres/calibration_problem.cpp @@ -89,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( @@ -1092,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(); @@ -1196,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() diff --git a/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp index 83dfb2e4..cdd38ab6 100644 --- a/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp +++ b/sensor/tag_based_sfm_calibrator/src/tag_based_sfm_calibrator.cpp @@ -453,18 +453,18 @@ void ExtrinsicTagBasedBaseCalibrator::calibrationRequestCallback( // Display the correction in calibration 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( From 424a726f5e0f3c09d72b629ddea39b00bbab2ab1 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 19 Mar 2024 15:03:48 +0900 Subject: [PATCH 38/49] chore: code explanation Signed-off-by: Kenzo Lobos-Tsunekawa --- .../mapping_based_calibrator/src/mapping_based_calibrator.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp b/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp index 0013db34..c34d22f5 100644 --- a/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp +++ b/sensor/mapping_based_calibrator/src/mapping_based_calibrator.cpp @@ -279,7 +279,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( keyframe_path_pub, keyframe_markers_pub, rosbag2_pause_client_, rosbag2_resume_client_, tf_buffer_); - // Set up lidar calibrators + // 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]; @@ -296,6 +296,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( target_markers_pub); } + // 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]; @@ -312,6 +313,7 @@ ExtrinsicMappingBasedCalibrator::ExtrinsicMappingBasedCalibrator( 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 = From d747ebeca10cb6b1da0ab549327df12630f4a9d2 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 19 Mar 2024 15:07:30 +0900 Subject: [PATCH 39/49] chore: removed redundant pass Signed-off-by: Kenzo Lobos-Tsunekawa --- .../tier4_calibration_views/image_view.py | 1 - 1 file changed, 1 deletion(-) diff --git a/common/tier4_calibration_views/tier4_calibration_views/image_view.py b/common/tier4_calibration_views/tier4_calibration_views/image_view.py index fb2bf8d4..52257ab8 100644 --- a/common/tier4_calibration_views/tier4_calibration_views/image_view.py +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view.py @@ -528,7 +528,6 @@ def draw_pointcloud(self, painter): raise NotImplementedError except Exception as e: logging.error(e) - pass line_pen = QPen() line_pen.setWidth(2) From 0d4bf8b4580d1f4b267f686f505f08f5039ea587 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 19 Mar 2024 15:10:16 +0900 Subject: [PATCH 40/49] chore: typo Signed-off-by: Kenzo Lobos-Tsunekawa --- .../src/marker_radar_lidar_calibrator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index ab9ed95d..0c81978c 100644 --- a/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp +++ b/sensor/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -1260,7 +1260,7 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation( Eigen::Isometry3d calibrated_2d_radar_to_radar_parallel_transformation( full_radar_to_radar_parallel_transformation.cast()); - // Check that is is actually a 2D transformation + // 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 = From 125d52e7f137ce760e9b459d8e98fabafc37acb3 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 19 Mar 2024 15:12:16 +0900 Subject: [PATCH 41/49] chore: added explanation of a variable in the header file Signed-off-by: Kenzo Lobos-Tsunekawa --- .../marker_radar_lidar_calibrator.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp index a23764cb..fd44ee99 100644 --- a/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp +++ b/sensor/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp @@ -149,7 +149,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node struct Parameters { - std::string radar_parallel_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; From e3e7d988efecf04cc1c882292a33c8dbcc9407dd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 19 Mar 2024 06:24:50 +0000 Subject: [PATCH 42/49] ci(pre-commit): autofix --- .../views/launcher_configuration_view.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index 58032448..da4de64f 100644 --- 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 @@ -218,9 +218,9 @@ def check_argument_status(self, text=None): def on_click(self): args_dict: Dict[str, str] = { - arg_name: args_widget.text() - if hasattr(args_widget, "text") - else args_widget.currentText() + 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() } From f462c2806775ff64075821638dd042347849d53e Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 2 Apr 2024 11:19:09 +0900 Subject: [PATCH 43/49] chore: attempt to fix pep257 Signed-off-by: Kenzo Lobos-Tsunekawa --- .../sensor_calibration_manager/calibrator_registry.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py index 6d13c3b6..714b6fa4 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py @@ -39,13 +39,14 @@ def getProjectCalibrators(cls, project_name) -> List: @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. + """ + 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: - The Executor class itself. TODO: write correct + wrapper """ def inner_wrapper(wrapped_class: CalibratorBase) -> CalibratorBase: @@ -61,7 +62,8 @@ def inner_wrapper(wrapped_class: CalibratorBase) -> CalibratorBase: @classmethod def create_calibrator(cls, project_name: str, calibrator_name: str, **kwargs) -> CalibratorBase: - """Create the executor using a factory pattern. + """ + 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 From c0f9ad5f490cac6b9a5b72bb56b27b549566fadb Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 2 Apr 2024 11:28:33 +0900 Subject: [PATCH 44/49] chore: fixing ci/cd and standalone installation due to changes in autoware Signed-off-by: Kenzo Lobos-Tsunekawa --- build_depends.repos | 4 ++++ 1 file changed, 4 insertions(+) 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 From 715fee71e7d1a26b05d6d59885f07cba1ed36559 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 2 Apr 2024 11:52:57 +0900 Subject: [PATCH 45/49] chore: more pep stuff Signed-off-by: Kenzo Lobos-Tsunekawa --- .../sensor_calibration_manager/calibrator_registry.py | 1 + 1 file changed, 1 insertion(+) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py index 714b6fa4..d30daa9e 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py @@ -71,6 +71,7 @@ def create_calibrator(cls, project_name: str, calibrator_name: str, **kwargs) -> Args: name (str): The name of the executor to create. + Returns: An instance of the executor that is created. """ From a4458085a27ecbf491677eb15240558800ac39a3 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 2 Apr 2024 12:34:26 +0900 Subject: [PATCH 46/49] chore: fized pep Signed-off-by: Kenzo Lobos-Tsunekawa --- .../sensor_calibration_manager/calibrator_registry.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py index d30daa9e..d076f241 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_registry.py @@ -45,8 +45,11 @@ def register_calibrator(cls, project_name: str, calibrator_name: str) -> Callabl Args: project_name (str): The name of the calibration project. calibrator_name (str): The name of the calibrator. - Returns: + + Returns + ------- wrapper + """ def inner_wrapper(wrapped_class: CalibratorBase) -> CalibratorBase: @@ -72,8 +75,10 @@ def create_calibrator(cls, project_name: str, calibrator_name: str, **kwargs) -> Args: name (str): The name of the executor to create. - Returns: + 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( From 20a5aa7272ea258110eb5619e0ba1b771fbc1f58 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 2 Apr 2024 13:49:08 +0900 Subject: [PATCH 47/49] chore: deleted redundant pass statements Signed-off-by: Kenzo Lobos-Tsunekawa --- .../tier4_calibration_views/image_view_ui.py | 1 - .../interactive_camera_lidar_calibrator/calibrator.py | 1 - .../interactive_calibrator.py | 2 -- .../board_detectors/apriltag_grid_detector.py | 1 - .../board_detectors/chessboard_detector.py | 1 - .../board_detectors/dotboard_detector.py | 1 - .../intrinsic_camera_calibrator/data_collector.py | 1 - .../intrinsic_camera_calibrator/data_sources/data_source.py | 1 - .../data_sources/video_file_data_source.py | 1 - .../sensor_calibration_manager/calibrator_base.py | 1 - .../sensor_calibration_manager/calibrator_wrapper.py | 1 - .../sensor_calibration_manager/sensor_calibration_manager.py | 1 - 12 files changed, 13 deletions(-) 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 index a38d96c2..9d2c2d5b 100644 --- a/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py +++ b/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py @@ -357,7 +357,6 @@ 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() diff --git a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py index cf10f8ba..c9d51bba 100644 --- a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py +++ b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/calibrator.py @@ -37,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 diff --git a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py index 57fb3eac..51e5d972 100644 --- a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py +++ b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py @@ -366,7 +366,6 @@ def save_calibration_callback(self): 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( @@ -407,7 +406,6 @@ def load_calibration_callback(self): 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: 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 6ae864f6..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 @@ -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/chessboard_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py index 2de50074..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 @@ -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 761ba22c..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 @@ -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/data_collector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index a4136c8e..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 @@ -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 f26dbbea..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 @@ -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/video_file_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/video_file_data_source.py index c96d6f14..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 @@ -21,4 +21,3 @@ class VideoFileDataSource(DataSource): def __init__(): super().__init__() - pass diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py index b9e90f8f..3a06a771 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py @@ -60,7 +60,6 @@ def __init__(self, ros_interface: RosInterface): def init(): logging.debug("CalibratorBase: Calibrator init?") - pass def on_check_tf_timer(self): logging.debug("CalibratorBase: on_check_tf_timer") diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py index 7817cc97..8696e030 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py @@ -166,7 +166,6 @@ def result_ros_callback(self, result: ExtrinsicCalibrator.Response): def status_ros_callback(self, status: bool): self.status_signal.emit(status) - pass def get_data(self, index) -> list: if not self.service_called: 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 index 5078fde7..e4890b9f 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py @@ -194,7 +194,6 @@ def launch_calibrators( self.calibrators_view.setFixedWidth(800) self.ros_interface.spin() - pass def on_calibrator_state_changed(self, state: CalibratorState): text_dict = { From d33e95d7e61a77d673e024ad7e89a2608d32e4a5 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 3 Apr 2024 15:33:06 +0900 Subject: [PATCH 48/49] chore: updated old name Signed-off-by: Kenzo Lobos-Tsunekawa --- .../sensor_calibration_manager/sensor_calibration_manager.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index e4890b9f..2a298736 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py @@ -53,7 +53,7 @@ from sensor_calibration_manager.views.tf_view import TfView -class NewExtrinsicCalibrationManager(QMainWindow): +class SensorCalibrationManager(QMainWindow): tfs_graph_signal = Signal(object) def __init__(self): @@ -301,7 +301,7 @@ def main(args=None): try: signal.signal(signal.SIGINT, sigint_handler) - calibration_manager = NewExtrinsicCalibrationManager() # noqa: F841 + calibration_manager = SensorCalibrationManager() # noqa: F841 sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): From 45453cfff40144d04f022c1b3632290b6d2ed8ca Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 9 Apr 2024 14:44:58 +0900 Subject: [PATCH 49/49] chore: updated misleading names Signed-off-by: Kenzo Lobos-Tsunekawa --- .../calibrators/rdv/tag_based_pnp_calibrator.py | 4 ++-- .../calibrators/x2/tag_based_pnp_calibrator.py | 4 ++-- .../calibrators/xx1/tag_based_pnp_calibrator.py | 4 ++-- .../calibrators/xx1_15/tag_based_pnp_calibrator.py | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) 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 index 5734d19b..18a8952b 100644 --- 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 @@ -44,7 +44,7 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - camera_to_lidar_transform = calibration_transforms[ + optical_link_to_lidar_transform = calibration_transforms[ f"{self.camera_name}/camera_optical_link" ]["pandar_top"] @@ -58,7 +58,7 @@ def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): sensor_kit_camera_link_transform = np.linalg.inv( camera_to_optical_link_transform - @ camera_to_lidar_transform + @ optical_link_to_lidar_transform @ np.linalg.inv(sensor_kit_to_lidar_transform) ) 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 index c3c589d0..e0857724 100644 --- 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 @@ -65,7 +65,7 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - camera_to_lidar_transform = calibration_transforms[ + optical_link_to_lidar_transform = calibration_transforms[ f"{self.camera_name}/camera_optical_link" ][self.lidar_frame] @@ -79,7 +79,7 @@ def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): sensor_kit_camera_link_transform = np.linalg.inv( camera_to_optical_link_transform - @ camera_to_lidar_transform + @ optical_link_to_lidar_transform @ np.linalg.inv(sensor_kit_to_lidar_transform) ) 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 index aae648ac..5098df1d 100644 --- 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 @@ -44,7 +44,7 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - camera_to_lidar_transform = calibration_transforms[ + 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( @@ -55,7 +55,7 @@ def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): ) sensor_kit_camera_link_transform = np.linalg.inv( camera_to_optical_link_transform - @ camera_to_lidar_transform + @ optical_link_to_lidar_transform @ np.linalg.inv(sensor_kit_to_lidar_transform) ) 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 index 5df81c75..f69841c2 100644 --- 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 @@ -44,7 +44,7 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - camera_to_lidar_transform = calibration_transforms[ + 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( @@ -55,7 +55,7 @@ def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): ) sensor_kit_camera_link_transform = np.linalg.inv( camera_to_optical_link_transform - @ camera_to_lidar_transform + @ optical_link_to_lidar_transform @ np.linalg.inv(sensor_kit_to_lidar_transform) )