From 62957b28334fe72bb1b581187bcbebbad084a8d7 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 28 Sep 2023 18:54:56 +0900 Subject: [PATCH] feat(extrinsic_ground_plane_calibrator): redesign, generalization, and improvement of the calibrator (#97) * Refactored/improved some parts of the ground plane calibrator and made it usable for the xx1/omiya combination Signed-off-by: Kenzo Lobos-Tsunekawa * Changing default parameters for the review * Added launchers for the x2 Signed-off-by: Kenzo Lobos-Tsunekawa * Fixed typo Signed-off-by: Kenzo Lobos-Tsunekawa * Fixed typos in the launchers Signed-off-by: Kenzo Lobos-Tsunekawa * Fixed one last typo Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa --- .../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_xx1/ground_plane.launch.xml | 11 +- .../ground_plane_sensor_kit.launch.xml | 74 --- .../aip_xx1/ground_plane_sensors.launch.xml | 44 +- .../launch/calibration.launch.xml | 4 +- .../CMakeLists.txt | 3 - .../extrinsic_ground_plane_calibrator.hpp | 125 ++++- .../utils.hpp | 46 ++ .../launch/calibrator.launch.xml | 32 +- .../rviz/x2.rviz | 399 +++++++++++++++ .../rviz/xx1.rviz | 352 +++++++++++++ .../src/extrinsic_ground_plane_calibrator.cpp | 484 ++++++++++++------ 15 files changed, 1507 insertions(+), 283 deletions(-) create mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane.launch.xml create mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_front_unit.launch.xml create mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_rear_unit.launch.xml create mode 100644 sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_top_unit.launch.xml delete mode 100644 sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensor_kit.launch.xml create mode 100644 sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/utils.hpp create mode 100644 sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz create mode 100644 sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz 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 new file mode 100644 index 00000000..98b334df --- /dev/null +++ b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane.launch.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..b1cf21e3 --- /dev/null +++ b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_front_unit.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..ccf5efc8 --- /dev/null +++ b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_rear_unit.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..01bf0f4f --- /dev/null +++ b/sensor/extrinsic_calibration_manager/launch/aip_x2/ground_plane_top_unit.launch.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 index 224153af..c750f831 100644 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane.launch.xml +++ b/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane.launch.xml @@ -4,16 +4,9 @@ - - + + - - diff --git a/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensor_kit.launch.xml b/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensor_kit.launch.xml deleted file mode 100644 index 74cd0aa7..00000000 --- a/sensor/extrinsic_calibration_manager/launch/aip_xx1/ground_plane_sensor_kit.launch.xml +++ /dev/null @@ -1,74 +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 index e5235451..2d37f3e2 100644 --- 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 @@ -5,6 +5,9 @@ + + + @@ -18,17 +21,48 @@ + [sensor_kit_base_link]"/> - + - + - - + + + + + + + + + + + + + + + + + + + + + diff --git a/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml b/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml index b6334d67..daa3c276 100644 --- a/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml +++ b/sensor/extrinsic_calibration_manager/launch/calibration.launch.xml @@ -113,9 +113,7 @@ - - - + diff --git a/sensor/extrinsic_ground_plane_calibrator/CMakeLists.txt b/sensor/extrinsic_ground_plane_calibrator/CMakeLists.txt index 9c67bc76..ea62c3eb 100644 --- a/sensor/extrinsic_ground_plane_calibrator/CMakeLists.txt +++ b/sensor/extrinsic_ground_plane_calibrator/CMakeLists.txt @@ -3,13 +3,11 @@ cmake_minimum_required(VERSION 3.5) project(extrinsic_ground_plane_calibrator) find_package(autoware_cmake REQUIRED) -find_package(OpenCV REQUIRED) # TODO: consider removing this one later autoware_package() ament_export_include_directories( include - ${OpenCV_INCLUDE_DIRS} ) # COMPILE THE SOURCE @@ -20,7 +18,6 @@ ament_auto_add_executable(extrinsic_ground_plane_calibrator ) target_link_libraries(extrinsic_ground_plane_calibrator - ${OpenCV_LIBS} ) ament_auto_package( 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 428ff164..fde4a276 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 @@ -15,10 +15,10 @@ #ifndef EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ #define EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ -#define PCL_NO_PRECOMPILE // We require this macro to use the PCL templates with velodyne PCs +#define PCL_NO_PRECOMPILE #include +#include #include -#include #include #include @@ -58,34 +58,122 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node const std::shared_ptr request, const std::shared_ptr response); + /*! + * ROS pointcloud callback + * @param[in] pc the input pointcloud + */ void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr pc); + /*! + * Checks that all the needed tfs are available + * @retval wether or not all the needed tfs are available + */ bool checkInitialTransforms(); + /*! + * Extracts the ground plane from a pointcloud + * @param[in] pointcloud the input pointcloud + * @param[in] model the estimated ground plane model + * @param[in] inliers the inliers of the current estimated model + * @retval wether or not th calibration plane was found + */ bool extractGroundPlane( pcl::PointCloud::Ptr & pointcloud, Eigen::Vector4d & model, pcl::PointCloud::Ptr & inliers); + /*! + * Computes the fitting error of an estimated model and the initial one + * @param[in] estimated_model the estimated model + * @param[in] inliers the inliers of the current estimated model + */ void evaluateModels( const Eigen::Vector4d & estimated_model, pcl::PointCloud::Ptr inliers) const; + /*! + * Filter individual calibration plane estimations and accumulate the inliers for a final + * regression + * @param[in] estimated_model the estimated model + * @param[in] inliers the inliers of the current estimated model + */ + void filterCalibration( + const Eigen::Vector4d & estimated_model, pcl::PointCloud::Ptr inliers); + + /*! + * Creates a visualization of a calibration plane + * @param[in] ground_plane_model the calibration plane + */ void visualizeCalibration(const Eigen::Vector4d & ground_plane_model); + + /*! + * Creates a visualization of a calibration plane + * @param[in] name the name of the calibration plane + * @param[in] model the model of the calibration plane + * @param[out] makers the output markers of the calibration plane + */ void visualizePlaneModel( const std::string & name, Eigen::Vector4d model, visualization_msgs::msg::MarkerArray & makers); + + /*! + * Creates a visualization of a calibration plane + * @param[in] name the name of the calibration plane + * @param[in] lidar_base_pose the pose of the calibration plane + * @param[out] makers the output markers of the calibration plane + */ void visualizePlaneModel( const std::string & name, const Eigen::Isometry3d & lidar_base_pose, visualization_msgs::msg::MarkerArray & makers); + /*! + * Publishes tfs related to the calibration process + * @param[in] ground_plane_model the current calibration plane + */ void publishTf(const Eigen::Vector4d & ground_plane_model); + /*! + * Computes a plane model given a pose. + * The normal of the plane is given by the z-axis of the rotation of the pose + * @param[in] pointcloud Point cloud to crop + * @param[in] max_range Range to crop the pointcloud to + * @retval the plane model + */ Eigen::Vector4d poseToPlaneModel(const Eigen::Isometry3d & pose) const; + + /*! + * Compute a pose from a plane model a*x + b*y +c*z +d = 0 + * The pose lies has its origin on the z-projection of the plane + * @param[in] model Point cloud to crop + * @retval the plane pose + */ Eigen::Isometry3d modelPlaneToPose(const Eigen::Vector4d & model) const; + /*! + * Refine a lidar-base pose given an estimated ground plane + * Projects the initial base lidar pose into the ground plane. + * @param[in] base_lidar_pose Initial base lidar pose + * @param[in] ground_plane_model ground plane model + * @retval the refined base lidar pose + */ + Eigen::Isometry3d refineBaseLidarPose( + const Eigen::Isometry3d & base_lidar_pose, const Eigen::Vector4d & model) const; + + /*! + * Removes the point that are consistent with an input plane from the pointcloud + * @param[in] input_pointcloud the pointcloud to filter + * @param[in] outlier_model the model that represents the outliers + * @param[in] outlier_tolerance the tolerance with which a point is still considered an outlier + * @retval the refined base lidar pose + */ + pcl::PointCloud::Ptr removeOutliers( + pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, + double outlier_tolerance) const; + // Parameters + // We perform base-lidar pose estimation but the output are frames in between + // base -> parent -> child -> lidar std::string base_frame_; - std::string 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 actual lidar tf) + std::string parent_frame_; + std::string child_frame_; + double marker_size_; bool use_crop_box_filter_; double crop_box_min_x_; @@ -94,6 +182,8 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node 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_; @@ -104,13 +194,13 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node bool broadcast_calibration_tf_; bool filter_estimations_; double initial_angle_cov_; - double initial_z_cov_; + double initial_translation_cov_; double angle_measurement_cov_; double angle_process_cov_; - double z_measurement_cov_; - double z_process_cov_; + double translation_measurement_cov_; + double translation_process_cov_; double angle_convergence_threshold_; - double z_convergence_threshold_; + double translation_convergence_threshold_; // ROS Interface tf2_ros::StaticTransformBroadcaster tf_broadcaster_; @@ -140,15 +230,16 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node Eigen::Isometry3d initial_base_to_lidar_eigen_; // Other tfs to calculate the complete chain. There are constant for our purposes - geometry_msgs::msg::Transform base_to_sensor_kit_msg_; - tf2::Transform base_to_sensor_kit_tf2_; - Eigen::Isometry3d base_to_sensor_kit_eigen_; + geometry_msgs::msg::Transform base_to_parent_msg_; + tf2::Transform base_to_parent_tf2_; + Eigen::Isometry3d base_to_parent_eigen_; - geometry_msgs::msg::Transform lidar_base_to_lidar_msg_; - tf2::Transform lidar_base_to_lidar_tf2_; - Eigen::Isometry3d lidar_base_to_lidar_eigen_; + 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_calibration_msg_; + geometry_msgs::msg::Pose output_parent_to_child_msg_; + Eigen::Isometry3d output_parent_to_child_eigen_; bool got_initial_transform_; bool broadcast_tf_; @@ -157,6 +248,8 @@ class ExtrinsicGroundPlaneCalibrator : public rclcpp::Node // Filtering KalmanFilter kalman_filter_; bool first_observation_; + RingBuffer::Ptr> inlier_observations_; + std::vector outlier_models_; }; #endif // EXTRINSIC_GROUND_PLANE_CALIBRATOR__EXTRINSIC_GROUND_PLANE_CALIBRATOR_HPP_ diff --git a/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/utils.hpp b/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/utils.hpp new file mode 100644 index 00000000..166ef753 --- /dev/null +++ b/sensor/extrinsic_ground_plane_calibrator/include/extrinsic_ground_plane_calibrator/utils.hpp @@ -0,0 +1,46 @@ +// 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_GROUND_PLANE_CALIBRATOR__UTILS_HPP_ +#define EXTRINSIC_GROUND_PLANE_CALIBRATOR__UTILS_HPP_ + +#include + +template +class RingBuffer +{ +public: + void setMaxSize(const int max_size) { max_size_ = max_size; } + + void add(const T & element) + { + if (max_size_ == 0) { + return; + } else if (current_index_ == data_.size()) { + data_.push_back(element); + } else { + data_[current_index_] = element; + } + + current_index_ = (current_index_ + 1) % max_size_; + } + + const std::vector & get() const { return data_; } + + std::vector data_; + std::size_t current_index_{0}; + int max_size_{0}; +}; + +#endif // EXTRINSIC_GROUND_PLANE_CALIBRATOR__UTILS_HPP_ diff --git a/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml index eeb2adee..06b74ee7 100644 --- a/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml +++ b/sensor/extrinsic_ground_plane_calibrator/launch/calibrator.launch.xml @@ -6,6 +6,17 @@ + + + + + + + + + + + @@ -13,9 +24,9 @@ - + - + @@ -24,21 +35,30 @@ - + - + - + - + + + + + + + + + + diff --git a/sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz b/sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz new file mode 100644 index 00000000..2a2e109a --- /dev/null +++ b/sensor/extrinsic_ground_plane_calibrator/rviz/x2.rviz @@ -0,0 +1,399 @@ +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/rviz/xx1.rviz b/sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz new file mode 100644 index 00000000..e7ad85a9 --- /dev/null +++ b/sensor/extrinsic_ground_plane_calibrator/rviz/xx1.rviz @@ -0,0 +1,352 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /top1/Topic1 + - /initial_base_link1 + - /top_inliers1 + - /top_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: 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: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + 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: /sensors/base_link/sensor_kit_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: 1.294999599456787 + Position: + X: -6.224511623382568 + Y: 0.10581792891025543 + Z: 25.368898391723633 + Target Frame: + Value: FPS (rviz_default_plugins) + Yaw: 0.051838066428899765 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000023b0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e00000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1846 + X: 74 + 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 3122c23b..118f4eef 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 @@ -34,8 +34,6 @@ #include -#define UNUSED(x) (void)x; - ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::NodeOptions & options) : Node("extrinsic_ground_plane_calibrator_node", options), tf_broadcaster_(this), @@ -47,8 +45,8 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod 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"); + parent_frame_ = this->declare_parameter("parent_frame"); + child_frame_ = this->declare_parameter("child_frame"); marker_size_ = this->declare_parameter("marker_size", 20.0); @@ -60,6 +58,8 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod 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); @@ -70,28 +70,33 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod verbose_ = this->declare_parameter("verbose", false); broadcast_calibration_tf_ = this->declare_parameter("broadcast_calibration_tf", false); filter_estimations_ = this->declare_parameter("filter_estimations", true); + int ring_buffer_size = this->declare_parameter("ring_buffer_size", 100); + + inlier_observations_.setMaxSize(ring_buffer_size); initial_angle_cov_ = this->declare_parameter("initial_angle_cov", 5.0); - initial_z_cov_ = this->declare_parameter("initial_z_cov", 0.05); + initial_translation_cov_ = this->declare_parameter("initial_translation_cov", 0.05); angle_measurement_cov_ = this->declare_parameter("angle_measurement_cov", 0.5); angle_process_cov_ = this->declare_parameter("angle_process_cov", 0.1); - z_measurement_cov_ = this->declare_parameter("z_measurement_cov", 0.005); - z_process_cov_ = this->declare_parameter("z_process_cov", 0.001); + translation_measurement_cov_ = + this->declare_parameter("translation_measurement_cov", 0.005); + translation_process_cov_ = this->declare_parameter("translation_process_cov", 0.001); angle_convergence_threshold_ = this->declare_parameter("angle_convergence_threshold", 0.0); - z_convergence_threshold_ = this->declare_parameter("z_convergence_threshold", 0.0); + translation_convergence_threshold_ = + this->declare_parameter("translation_convergence_threshold", 0.0); initial_angle_cov_ = std::pow(initial_angle_cov_ * M_PI_2 / 180.0, 2); - initial_z_cov_ = std::pow(initial_z_cov_, 2); + initial_translation_cov_ = std::pow(initial_translation_cov_, 2); angle_measurement_cov_ = std::pow(angle_measurement_cov_ * M_PI_2 / 180.0, 2); angle_process_cov_ = std::pow(angle_process_cov_ * M_PI_2 / 180.0, 2); - z_measurement_cov_ = std::pow(z_measurement_cov_, 2); - z_process_cov_ = std::pow(z_process_cov_, 2); + translation_measurement_cov_ = std::pow(translation_measurement_cov_, 2); + translation_process_cov_ = std::pow(translation_process_cov_, 2); angle_convergence_threshold_ = std::pow(angle_convergence_threshold_ * M_PI_2 / 180.0, 2); - z_convergence_threshold_ = std::pow(z_convergence_threshold_, 2); + translation_convergence_threshold_ = std::pow(translation_convergence_threshold_, 2); markers_pub_ = this->create_publisher("markers", 10); inliers_pub_ = this->create_publisher("inliers", 10); @@ -111,21 +116,24 @@ ExtrinsicGroundPlaneCalibrator::ExtrinsicGroundPlaneCalibrator(const rclcpp::Nod rmw_qos_profile_services_default, srv_callback_group_); // Initialize the filter - kalman_filter_.setA(Eigen::DiagonalMatrix(1.0, 1.0, 1.0)); - kalman_filter_.setB(Eigen::DiagonalMatrix(0.0, 0.0, 0.0)); - kalman_filter_.setC(Eigen::DiagonalMatrix(1.0, 1.0, 1.0)); - kalman_filter_.setQ(Eigen::DiagonalMatrix( - angle_measurement_cov_, angle_measurement_cov_, z_measurement_cov_)); - kalman_filter_.setR( - Eigen::DiagonalMatrix(angle_process_cov_, angle_process_cov_, z_process_cov_)); + kalman_filter_.setA(Eigen::DiagonalMatrix(1.0, 1.0, 1.0, 1.0, 1.0, 1.0)); + kalman_filter_.setB(Eigen::DiagonalMatrix(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + kalman_filter_.setC(Eigen::DiagonalMatrix(1.0, 1.0, 1.0, 1.0, 1.0, 1.0)); + kalman_filter_.setQ(Eigen::DiagonalMatrix( + angle_measurement_cov_, angle_measurement_cov_, angle_measurement_cov_, + translation_measurement_cov_, translation_measurement_cov_, translation_measurement_cov_)); + kalman_filter_.setR(Eigen::DiagonalMatrix( + angle_process_cov_, angle_process_cov_, angle_process_cov_, translation_process_cov_, + translation_process_cov_, translation_process_cov_)); } void ExtrinsicGroundPlaneCalibrator::requestReceivedCallback( - const std::shared_ptr request, + __attribute__((unused)) + const std::shared_ptr + request, const std::shared_ptr response) { // This tool uses several tfs, so for consistency we take the initial calibration using lookups - UNUSED(request); using std::chrono_literals::operator""s; // Loop until the calibration finishes @@ -141,7 +149,11 @@ void ExtrinsicGroundPlaneCalibrator::requestReceivedCallback( } response->success = true; - response->result_pose = output_calibration_msg_; + response->result_pose = output_parent_to_child_msg_; + + RCLCPP_INFO(this->get_logger(), "Calibration result sent"); + + pointcloud_sub_.reset(); } void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( @@ -151,7 +163,7 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( header_ = msg->header; // Make sure we have all the required initial tfs - if (!checkInitialTransforms()) { + if (!checkInitialTransforms() || calibration_done_) { return; } @@ -160,6 +172,18 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( pcl::PointCloud::Ptr inliers_pointcloud(new pcl::PointCloud); pcl::fromROSMsg(*msg, *pointcloud); + // Filter the pointcloud using previous outlier models + if (remove_outliers_) { + std::size_t original_num_points = pointcloud->size(); + for (const auto & outlier_model : outlier_models_) { + pointcloud = removeOutliers(pointcloud, outlier_model, remove_outlier_tolerance_); + } + + RCLCPP_INFO( + this->get_logger(), "Outlier plane removal: %lu -> %lu points", original_num_points, + pointcloud->size()); + } + // Extract the ground plane model Eigen::Vector4d ground_plane_model; if (!extractGroundPlane(pointcloud, ground_plane_model, inliers_pointcloud)) { @@ -177,6 +201,8 @@ void ExtrinsicGroundPlaneCalibrator::pointCloudCallback( // Create markers to visualize the calibration visualizeCalibration(ground_plane_model); + filterCalibration(ground_plane_model, inliers_pointcloud); + // Obtain the final output tf and publish the lidar -> ground tfs to evaluate the calibration publishTf(ground_plane_model); } @@ -201,29 +227,17 @@ bool ExtrinsicGroundPlaneCalibrator::checkInitialTransforms() 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_sensor_kit_msg_ = - tf_buffer_->lookupTransform(base_frame_, sensor_kit_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_); + base_to_parent_msg_ = + tf_buffer_->lookupTransform(base_frame_, parent_frame_, t, timeout).transform; - const auto & base_to_sensor_kit_rpy = - tier4_autoware_utils::getRPY(base_to_sensor_kit_msg_.rotation); + fromMsg(base_to_parent_msg_, base_to_parent_tf2_); + base_to_parent_eigen_ = tf2::transformToEigen(base_to_parent_msg_); - if (base_to_sensor_kit_rpy.x != 0.0 || base_to_sensor_kit_rpy.y != 0.0) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "This method assumes that the base and the sensor kit are parallel. RPY=" - << base_to_sensor_kit_rpy.x << ", " << base_to_sensor_kit_rpy.y << ", " - << base_to_sensor_kit_rpy.z); - return false; - } - - lidar_base_to_lidar_msg_ = - tf_buffer_->lookupTransform(lidar_base_frame_, lidar_frame_, t, timeout).transform; + child_to_lidar_msg_ = + tf_buffer_->lookupTransform(child_frame_, lidar_frame_, t, timeout).transform; - fromMsg(lidar_base_to_lidar_msg_, lidar_base_to_lidar_tf2_); - lidar_base_to_lidar_eigen_ = tf2::transformToEigen(lidar_base_to_lidar_msg_); + fromMsg(child_to_lidar_msg_, child_to_lidar_tf2_); + child_to_lidar_eigen_ = tf2::transformToEigen(child_to_lidar_msg_); got_initial_transform_ = true; } catch (tf2::TransformException & ex) { @@ -264,7 +278,7 @@ bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( if (verbose_) { RCLCPP_INFO( - this->get_logger(), "Rough plane normal. x=%.2f, y=%.2f, z=%.2f", rough_normal.x(), + this->get_logger(), "Rough plane normal. x=%.3f, y=%.3f, z=%.3f", rough_normal.x(), rough_normal.y(), rough_normal.z()); } @@ -299,25 +313,25 @@ bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( 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_) { - model = Eigen::Vector4d( - coefficients->values[0], coefficients->values[1], coefficients->values[2], - coefficients->values[3]); - if (verbose_) { RCLCPP_INFO( - this->get_logger(), "Plane found: inliers=%ld (%.2f)", inliers->indices.size(), + this->get_logger(), "Plane found: inliers=%ld (%.3f)", inliers->indices.size(), inlier_percentage); RCLCPP_INFO( - this->get_logger(), "Plane model. a=%.2f, b=%.2f, c=%.2f, d=%.2f", model(0), model(1), + 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: %.2f / %.2f", cos_distance, max_cos_distance_); + this->get_logger(), "Cos distance: %.3f / %.3f", cos_distance, max_cos_distance_); } // Extract the ground plane inliers @@ -326,6 +340,37 @@ bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( extract.setNegative(false); extract.filter(*inliers_pointcloud); return true; + } else { + if (remove_outliers_) { + bool accept = true; + + for (const auto & outlier_model : outlier_models_) { + Eigen::Vector3f outlier_normal(outlier_model.x(), outlier_model.y(), outlier_model.z()); + float cos_distance = 1.0 - std::abs(outlier_normal.dot(normal)); + + if ( + cos_distance < max_cos_distance_ && + std::abs(outlier_model.w() - model.w()) < remove_outlier_tolerance_) { + accept = false; + } + } + + if (accept) { + outlier_models_.push_back(model); + RCLCPP_INFO( + this->get_logger(), "New outlier model: a=%.3f, b=%.3f, c=%.3f, d=%.3f", model(0), + model(1), model(2), model(3)); + } + } + + if (verbose_) { + RCLCPP_INFO( + this->get_logger(), + "Iteration failed. model: a=%.3f, b=%.3f, c=%.3f, d=%.3f inliers=%lu inlier " + "percentage=%.2f cos_distance=%.2f", + model(0), model(1), model(2), model(3), inliers->indices.size(), inlier_percentage, + cos_distance); + } } // Extract the inliers from the pointcloud (the detected plane was not the ground plane) @@ -339,7 +384,6 @@ bool ExtrinsicGroundPlaneCalibrator::extractGroundPlane( iteration_cloud->swap(next_cloud); iteration_size = iteration_cloud->height * iteration_cloud->width; } - return false; } @@ -369,6 +413,161 @@ void ExtrinsicGroundPlaneCalibrator::evaluateModels( RCLCPP_INFO(this->get_logger(), "Estimated calibration error: %3f m", estimated_model_error); } +void ExtrinsicGroundPlaneCalibrator::filterCalibration( + const Eigen::Vector4d & ground_plane_model, pcl::PointCloud::Ptr inliers) +{ + // Eigen::Isometry3d lidar_to_ground_pose = modelPlaneToPose(ground_plane_model); + // Eigen::Isometry3d ground_to_lidar_pose = lidar_to_ground_pose.inverse(); + + Eigen::Isometry3d estimated_base_to_lidar_pose = + refineBaseLidarPose(initial_base_to_lidar_eigen_, ground_plane_model); + + Eigen::Isometry3d estimated_parent_to_child_eigen = base_to_parent_eigen_.inverse() * + estimated_base_to_lidar_pose * + child_to_lidar_eigen_.inverse(); + + Eigen::Isometry3d initial_parent_to_child_eigen = base_to_parent_eigen_.inverse() * + initial_base_to_lidar_eigen_ * + child_to_lidar_eigen_.inverse(); + + Eigen::Vector3d estimated_translation; + auto estimated_rpy = + tier4_autoware_utils::getRPY(tf2::toMsg(estimated_parent_to_child_eigen).orientation); + auto initial_rpy = + tier4_autoware_utils::getRPY(tf2::toMsg(initial_parent_to_child_eigen).orientation); + + if (verbose_) { + RCLCPP_INFO( + this->get_logger(), "Initial parent->child euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", + initial_rpy.x, initial_rpy.y, initial_rpy.z); + RCLCPP_INFO( + this->get_logger(), "Estimated parent->child euler angles: roll=%.3f, pitch=%.3f, yaw=%.3f", + estimated_rpy.x, estimated_rpy.y, estimated_rpy.z); + + RCLCPP_INFO( + this->get_logger(), "Initial parent->child translation: x=%.3f, y=%.3f, z=%.3f", + initial_parent_to_child_eigen.translation().x(), + initial_parent_to_child_eigen.translation().y(), + initial_parent_to_child_eigen.translation().z()); + RCLCPP_INFO( + this->get_logger(), "Estimated parent->child translation: x=%.3f, y=%.3f, z=%.3f", + estimated_parent_to_child_eigen.translation().x(), + estimated_parent_to_child_eigen.translation().y(), + estimated_parent_to_child_eigen.translation().z()); + } + + // Optional filtering + if (filter_estimations_) { + Eigen::Vector x( + estimated_rpy.x, estimated_rpy.y, estimated_rpy.z, + estimated_parent_to_child_eigen.translation().x(), + estimated_parent_to_child_eigen.translation().y(), + estimated_parent_to_child_eigen.translation().z()); + Eigen::DiagonalMatrix p0( + initial_angle_cov_, initial_angle_cov_, initial_angle_cov_, initial_translation_cov_, + initial_translation_cov_, initial_translation_cov_); + + if (first_observation_) { + kalman_filter_.init(x, p0); + first_observation_ = false; + } else { + kalman_filter_.update(x); + } + + estimated_rpy.x = kalman_filter_.getXelement(0); + estimated_rpy.y = kalman_filter_.getXelement(1); + estimated_rpy.z = kalman_filter_.getXelement(2); + estimated_translation.x() = kalman_filter_.getXelement(3); + estimated_translation.y() = kalman_filter_.getXelement(4); + estimated_translation.z() = kalman_filter_.getXelement(5); + } + + // By detecting the ground plane and fabricating a pose arbitrarily, the x, y, and yaw do not hold + // real meaning, so we instead just use the ones from the initial calibration + geometry_msgs::msg::Quaternion estimated_orientation_msg = + tier4_autoware_utils::createQuaternionFromRPY(estimated_rpy.x, estimated_rpy.y, initial_rpy.z); + Eigen::Quaterniond estimated_orientation_eigen; + + tf2::fromMsg(estimated_orientation_msg, estimated_orientation_eigen); + + output_parent_to_child_eigen_.linear() = estimated_orientation_eigen.toRotationMatrix(); + output_parent_to_child_eigen_.translation() = estimated_translation; + + // We perform basic filtering on the estimated angles + { + std::unique_lock lock(mutex_); + output_parent_to_child_msg_ = tf2::toMsg(output_parent_to_child_eigen_); + + if (filter_estimations_) { + Eigen::MatrixXd p; + kalman_filter_.getP(p); + Eigen::VectorXd diag = p.diagonal(); + std::array thresholds{ + angle_convergence_threshold_, angle_convergence_threshold_, + angle_convergence_threshold_, translation_convergence_threshold_, + translation_convergence_threshold_, translation_convergence_threshold_}; + inlier_observations_.add(inliers); + + bool converged = true; + for (std::size_t index = 0; index < thresholds.size(); index++) { + converged &= diag(index) < thresholds[index]; + } + + RCLCPP_INFO( + this->get_logger(), "Filter cov: roll=%.2e, pitch=%.2e yaw=%.2e, x=%.2e, y=%.2e, z=%.2e", + diag(0), diag(1), diag(2), diag(3), diag(4), diag(5)); + + RCLCPP_INFO( + this->get_logger(), "Convergence thresh: angle=%.2e, translation=%.2e", + angle_convergence_threshold_, translation_convergence_threshold_); + + if (!converged) { + return; + } + + pcl::PointCloud::Ptr augmented_inliers(new pcl::PointCloud); + + for (const auto & inliers : inlier_observations_.get()) { + *augmented_inliers += *inliers; + } + + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr final_inliers(new pcl::PointIndices); + pcl::SACSegmentation seg; + pcl::ExtractIndices extract; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(10 * max_inlier_distance_); + seg.setMaxIterations(max_iterations_); + + seg.setInputCloud(augmented_inliers); + seg.segment(*final_inliers, *coefficients); + Eigen::Vector4d output_model( + coefficients->values[0], coefficients->values[1], coefficients->values[2], + coefficients->values[3]); + + RCLCPP_INFO( + this->get_logger(), + "Final model: a=%.3f, b=%.3f, c=%.3f, d=%.3f final inliers=%lu total.percentage=%.2f", + output_model(0), output_model(1), output_model(2), output_model(3), + final_inliers->indices.size(), + 100.f * final_inliers->indices.size() / augmented_inliers->size()); + + Eigen::Isometry3d output_base_to_lidar_pose = + refineBaseLidarPose(initial_base_to_lidar_eigen_, output_model); + + output_parent_to_child_eigen_ = base_to_parent_eigen_.inverse() * output_base_to_lidar_pose * + child_to_lidar_eigen_.inverse(); + output_parent_to_child_msg_ = tf2::toMsg(output_parent_to_child_eigen_); + + calibration_done_ = true; + } else { + calibration_done_ = true; + } + } +} + void ExtrinsicGroundPlaneCalibrator::visualizeCalibration( const Eigen::Vector4d & estimated_ground_model) { @@ -482,130 +681,32 @@ void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_pl tf_broadcaster_.sendTransform(initial_lidar_to_base_msg); Eigen::Isometry3d raw_lidar_to_base_eigen = modelPlaneToPose(ground_plane_model); - Eigen::Isometry3d raw_base_to_lidar_eigen = raw_lidar_to_base_eigen.inverse(); // The ground_plane_raw tf is only assures us that it lies on the ground plane, but its yaw is // arbitrary, and the position in the plane is obtained by projecting the lidar origin in the // plane geometry_msgs::msg::TransformStamped raw_lidar_to_base_msg = tf2::eigenToTransform(raw_lidar_to_base_eigen); + raw_lidar_to_base_msg.header.stamp = header_.stamp; raw_lidar_to_base_msg.header.frame_id = lidar_frame_; raw_lidar_to_base_msg.child_frame_id = lidar_frame_ + "_ground_plane_raw"; tf_broadcaster_.sendTransform(raw_lidar_to_base_msg); - Eigen::Isometry3d raw_sensor_kit_to_lidar_base_eigen = base_to_sensor_kit_eigen_.inverse() * - raw_base_to_lidar_eigen * - lidar_base_to_lidar_eigen_.inverse(); - - Eigen::Isometry3d initial_sensor_kit_to_lidar_base_eigen = base_to_sensor_kit_eigen_.inverse() * - initial_base_to_lidar_eigen_ * - lidar_base_to_lidar_eigen_.inverse(); - - auto estimated_rpy = - tier4_autoware_utils::getRPY(tf2::toMsg(raw_sensor_kit_to_lidar_base_eigen).orientation); - auto initial_rpy = - tier4_autoware_utils::getRPY(tf2::toMsg(initial_sensor_kit_to_lidar_base_eigen).orientation); - - if (verbose_) { - RCLCPP_INFO( - this->get_logger(), "Initial euler angles: roll=%.2f, pitch=%.2f, yaw=%.2f", initial_rpy.x, - initial_rpy.y, initial_rpy.z); - RCLCPP_INFO( - this->get_logger(), "Estimated euler angles: roll=%.2f, pitch=%.2f, yaw=%.2f", - estimated_rpy.x, estimated_rpy.y, estimated_rpy.z); - - RCLCPP_INFO( - this->get_logger(), "Initial translation: x=%.2f, y=%.2f, z=%.2f", - initial_sensor_kit_to_lidar_base_eigen.translation().x(), - initial_sensor_kit_to_lidar_base_eigen.translation().y(), - initial_sensor_kit_to_lidar_base_eigen.translation().z()); - RCLCPP_INFO( - this->get_logger(), "Estimated translation: x=%.2f, y=%.2f, z=%.2f", - raw_sensor_kit_to_lidar_base_eigen.translation().x(), - raw_sensor_kit_to_lidar_base_eigen.translation().y(), - raw_sensor_kit_to_lidar_base_eigen.translation().z()); - } - - double estimated_z = raw_sensor_kit_to_lidar_base_eigen.translation().z(); - - // Optional filtering - if (filter_estimations_) { - Eigen::Vector3d x(estimated_rpy.x, estimated_rpy.y, estimated_z); - Eigen::DiagonalMatrix p0(initial_angle_cov_, initial_angle_cov_, initial_z_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_z = kalman_filter_.getXelement(2); - } - - // 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::Pose output_sensor_kit_to_lidar_base_msg; - output_sensor_kit_to_lidar_base_msg.orientation = - tier4_autoware_utils::createQuaternionFromRPY(estimated_rpy.x, estimated_rpy.y, initial_rpy.z); - - output_sensor_kit_to_lidar_base_msg.position.x = - initial_sensor_kit_to_lidar_base_eigen.translation().x(); - output_sensor_kit_to_lidar_base_msg.position.y = - initial_sensor_kit_to_lidar_base_eigen.translation().y(); - output_sensor_kit_to_lidar_base_msg.position.z = estimated_z; - - Eigen::Isometry3d output_sensor_kit_to_lidar_base_eigen; - tf2::fromMsg(output_sensor_kit_to_lidar_base_msg, output_sensor_kit_to_lidar_base_eigen); - if (broadcast_calibration_tf_) { geometry_msgs::msg::TransformStamped output_tf_msg; - output_tf_msg.transform.rotation = output_sensor_kit_to_lidar_base_msg.orientation; - output_tf_msg.transform.translation.x = output_sensor_kit_to_lidar_base_msg.position.x; - output_tf_msg.transform.translation.y = output_sensor_kit_to_lidar_base_msg.position.y; - output_tf_msg.transform.translation.z = output_sensor_kit_to_lidar_base_msg.position.z; + 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 = sensor_kit_frame_; - output_tf_msg.child_frame_id = lidar_base_frame_; + output_tf_msg.header.frame_id = parent_frame_; + output_tf_msg.child_frame_id = child_frame_; tf_broadcaster_.sendTransform(output_tf_msg); } - // We perform basic filtering on the estimated angles - { - std::unique_lock lock(mutex_); - output_calibration_msg_ = tf2::toMsg(output_sensor_kit_to_lidar_base_eigen); - - if (filter_estimations_) { - Eigen::MatrixXd p; - kalman_filter_.getP(p); - double roll_cov = p(0, 0); - double pitch_cov = p(1, 1); - double z_cov = p(2, 2); - - if ( - roll_cov < angle_convergence_threshold_ && pitch_cov < angle_convergence_threshold_ && - z_cov < z_convergence_threshold_) { - calibration_done_ = true; - } - - RCLCPP_INFO( - this->get_logger(), "Filter cov: roll=%.2e, pitch=%.2e, z=%.2e", roll_cov, pitch_cov, - z_cov); - - RCLCPP_INFO( - this->get_logger(), "Convergence thresh: angle=%.2e, x=%.2e", angle_convergence_threshold_, - z_convergence_threshold_); - - } else { - calibration_done_ = true; - } - } - Eigen::Isometry3d output_base_to_lidar_eigen = - base_to_sensor_kit_eigen_ * output_sensor_kit_to_lidar_base_eigen * lidar_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 @@ -613,15 +714,8 @@ void ExtrinsicGroundPlaneCalibrator::publishTf(const Eigen::Vector4d & ground_pl 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"; + output_lidar_to_base_msg.child_frame_id = lidar_frame_ + "_ground_plane"; tf_broadcaster_.sendTransform(output_lidar_to_base_msg); - - // Test correctness of the output lidar -> base tf - Eigen::Vector4d output_model = poseToPlaneModel(output_base_to_lidar_eigen.inverse()); - - Eigen::Vector3d raw_model_normal( - ground_plane_model(0), ground_plane_model(1), ground_plane_model(2)); - Eigen::Vector3d output_model_normal(output_model(0), output_model(1), output_model(2)); } Eigen::Vector4d ExtrinsicGroundPlaneCalibrator::poseToPlaneModel( @@ -648,7 +742,7 @@ Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::modelPlaneToPose( Eigen::Vector3d x0 = -n * model(3); - // To create a real pose we need to invent a base + // To create a real pose we need to invent a basis Eigen::Vector3d base_x, base_y, base_z; base_z = n; @@ -678,3 +772,59 @@ Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::modelPlaneToPose( return pose; } + +Eigen::Isometry3d ExtrinsicGroundPlaneCalibrator::refineBaseLidarPose( + const Eigen::Isometry3d & base_lidar_pose, const Eigen::Vector4d & model) const +{ + const Eigen::Isometry3d lidar_base_pose = base_lidar_pose.inverse(); + const Eigen::Isometry3d lidar_ground_pose = modelPlaneToPose(model); + + const Eigen::Isometry3d ground_base = lidar_ground_pose.inverse() * lidar_base_pose; + + Eigen::Vector3d ground_base_projected_translation = ground_base.translation(); + ground_base_projected_translation.z() = 0; + + Eigen::Vector3d ground_base_projected_rotation_x = ground_base.rotation().col(0); + ground_base_projected_rotation_x.z() = 0.0; + ground_base_projected_rotation_x.normalize(); + + Eigen::Matrix3d ground_base_projected_rotation; + ground_base_projected_rotation.col(2) = Eigen::Vector3d(0.0, 0.0, 1.0); + ground_base_projected_rotation.col(0) = ground_base_projected_rotation_x; + ground_base_projected_rotation.col(1) = + ground_base_projected_rotation.col(2).cross(ground_base_projected_rotation.col(0)); + + Eigen::Isometry3d ground_base_projected; + ground_base_projected.translation() = ground_base_projected_translation; + ground_base_projected.linear() = ground_base_projected_rotation; + + return ground_base_projected.inverse() * lidar_ground_pose.inverse(); +} + +pcl::PointCloud::Ptr ExtrinsicGroundPlaneCalibrator::removeOutliers( + pcl::PointCloud::Ptr input_pointcloud, const Eigen::Vector4d & outlier_plane_model, + double outlier_tolerance) const +{ + pcl::ExtractIndices extract; + pcl::PointCloud::Ptr inliers(new pcl::PointCloud); + pcl::PointIndices::Ptr outliers(new pcl::PointIndices); + + for (std::size_t index = 0; index < input_pointcloud->size(); index++) { + const auto & p = input_pointcloud->points[index]; + double error = std::abs( + outlier_plane_model.x() * p.x + outlier_plane_model.y() * p.y + + outlier_plane_model.z() * p.z + outlier_plane_model.w()); + + if (error < outlier_tolerance) { + outliers->indices.push_back(index); + } + } + + // Extract the inliers from the pointcloud (the detected plane was not the ground plane) + extract.setInputCloud(input_pointcloud); + extract.setIndices(outliers); + extract.setNegative(true); + extract.filter(*inliers); + + return inliers; +}