diff --git a/CMakeLists.txt b/CMakeLists.txt index bb84576..79f4662 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,7 +71,8 @@ ament_auto_add_executable(lidartag_main src/tag16h5.cpp src/lidartag_rviz.cpp src/lidartag_cluster.cpp - src/lidartag_prune.cpp) + src/lidartag_prune.cpp + src/rectangle_estimator.cpp) target_link_libraries(lidartag_main ${PCL_LIBRARIES} @@ -81,10 +82,30 @@ target_link_libraries(lidartag_main Boost::thread ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() +#if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# +# include_directories( +# include +# ) +# +# # Unit tests +# set(TEST_SOURCES +# test/test_rectangle_estimation.cpp +# src/rectangle_estimator.cpp +# #test/test_pid.cpp +# ) +# +# #set(TEST_VELOCITY_CONTROLLER test_velocity_controller) +# ament_add_gtest(sometest_name ${TEST_SOURCES}) +# #target_link_libraries(${TEST_VELOCITY_CONTROLLER} velocity_controller_core) +# +# target_link_libraries(sometest_name +# ${PCL_LIBRARIES} +#) +# +#endif() + ament_auto_package( INSTALL_TO_SHARE diff --git a/include/KDTreeVectorOfVectorsAdaptor.h b/include/lidartag/KDTreeVectorOfVectorsAdaptor.hpp similarity index 100% rename from include/KDTreeVectorOfVectorsAdaptor.h rename to include/lidartag/KDTreeVectorOfVectorsAdaptor.hpp diff --git a/include/apriltag_utils.h b/include/lidartag/apriltag_utils.hpp similarity index 96% rename from include/apriltag_utils.h rename to include/lidartag/apriltag_utils.hpp index 783973a..405370e 100644 --- a/include/apriltag_utils.h +++ b/include/lidartag/apriltag_utils.hpp @@ -28,10 +28,10 @@ * WEBSITE: https://www.brucerobot.com/ */ -#ifndef APRILTAG_UTILS_H -#define APRILTAG_UTILS_H +#ifndef APRILTAG_UTILS_HPP +#define APRILTAG_UTILS_HPP -#include "lidartag.h" +#include namespace BipedAprilLab{ diff --git a/include/lidartag.h b/include/lidartag/lidartag.hpp similarity index 93% rename from include/lidartag.h rename to include/lidartag/lidartag.hpp index cba3cd4..3ff76c6 100644 --- a/include/lidartag.h +++ b/include/lidartag/lidartag.hpp @@ -29,8 +29,8 @@ * WEBSITE: https://www.brucerobot.com/ */ -#ifndef LIDARTAG_H -#define LIDARTAG_H +#ifndef LIDARTAG_HPP +#define LIDARTAG_HPP #include #include @@ -49,7 +49,6 @@ #include #include #include -//#include // threading #include @@ -75,11 +74,12 @@ #include #include #include -#include "thread_pool.h" -#include "types.h" -#include "utils.h" -#include "apriltag_utils.h" +#include +#include +#include +#include +#include namespace BipedLab { class LidarTag: public rclcpp::Node { @@ -95,7 +95,6 @@ class LidarTag: public rclcpp::Node { double ransac_threshold; int fine_cluster_threshold; // TODO: REPLACE WITH TAG PARAMETERS int filling_gap_max_index; // TODO: CHECK - int filling_max_points_threshold; // TODO: REMOVE double points_threshold_factor; // TODO: CHECK double distance_to_plane_threshold; double max_outlier_ratio; @@ -108,11 +107,22 @@ class LidarTag: public rclcpp::Node { int cluster_min_index; int cluster_max_points_size; int cluster_min_points_size; + double depth_bound; // Edge detection parameters + double min_rkhs_score; + bool optional_fix_cluster; + bool use_rectangle_model; + bool rectangle_model_use_ransac; + int rectangle_model_max_iterations; + float rectangle_model_max_error; + bool rectangle_fix_point_groups; + bool refine_cluster_with_intersections; bool debug_single_pointcloud; double debug_point_x; double debug_point_y; double debug_point_z; int debug_cluster_id; + int debug_ring_id; + int debug_scan_id; } params_; OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -166,6 +176,7 @@ class LidarTag: public rclcpp::Node { rclcpp::Publisher::SharedPtr edge3_pub_; rclcpp::Publisher::SharedPtr edge4_pub_; rclcpp::Publisher::SharedPtr boundary_pub_; + rclcpp::Publisher::SharedPtr initial_corners_pub_; rclcpp::Publisher::SharedPtr cluster_pub_; rclcpp::Publisher::SharedPtr payload_pub_; rclcpp::Publisher::SharedPtr payload3d_pub_; @@ -200,6 +211,7 @@ class LidarTag: public rclcpp::Node { rclcpp::Publisher::SharedPtr colored_cluster_buff_pub_; rclcpp::Publisher::SharedPtr ps_cluster_buff_pub_; rclcpp::Publisher::SharedPtr in_cluster_buff_pub_; + rclcpp::Publisher::SharedPtr ordered_pointcloud_markers_pub_; // Flag int point_cloud_received_; // check if a scan of point cloud has received or @@ -210,12 +222,8 @@ class LidarTag: public rclcpp::Node { std::queue point_cloud1_queue_; // LiDAR parameters - bool sensor_qos_; rclcpp::Time current_scan_time_; // store current time of the lidar scan - std::string pointcloud_topic_; // subscribe channel - std::string pub_frame_; // publish under what frame? - std::string lidartag_detections_topic_; - std::string corners_array_topic_; + std::string lidar_frame_; // publish under what frame? // Overall LiDAR system parameters LiDARSystem_t lidar_system_; int max_queue_size_; @@ -226,9 +234,6 @@ class LidarTag: public rclcpp::Node { int point_cloud_size_; std_msgs::msg::Header point_cloud_header_; - // Edge detection parameters - double depth_threshold_; - // If on-board processing is limited, limit range of points double distance_bound_; double distance_threshold_; @@ -284,6 +289,9 @@ class LidarTag: public rclcpp::Node { std::string outputs_path_; std::string library_path_; + // Corner estimation + std::shared_ptr rectangle_estimator_; + /* [Main loop] * main loop of process */ @@ -486,6 +494,25 @@ class LidarTag: public rclcpp::Node { */ void organizeDataPoints(ClusterFamily_t & t_cluster); + /* + * A function to estimate the four corners of a tag using 4x line RANSAC + */ + bool estimateCornersUsingLinesRANSAC(ClusterFamily_t & t_cluster, + pcl::PointCloud::Ptr & cloud1, pcl::PointCloud::Ptr & cloud2, + pcl::PointCloud::Ptr & cloud3, pcl::PointCloud::Ptr & cloud4, + Eigen::Vector3f & corner1, Eigen::Vector3f & corner2, + Eigen::Vector3f & corner3, Eigen::Vector3f & corner4); + + /* + * A function to estimate the four corners of a tag using 1x rectangle fiting with + * optional RANSAC + */ + bool estimateCornersUsingRectangleFitting(ClusterFamily_t & t_cluster, + pcl::PointCloud::Ptr & cloud1, pcl::PointCloud::Ptr & cloud2, + pcl::PointCloud::Ptr & cloud3, pcl::PointCloud::Ptr & cloud4, + Eigen::Vector3f & corner1, Eigen::Vector3f & corner2, + Eigen::Vector3f & corner3, Eigen::Vector3f & corner4); + /* [Edge points and principal axes] * A function to transform the edge points to the tag frame */ @@ -710,7 +737,8 @@ class LidarTag: public rclcpp::Node { pcl::PointCloud::Ptr t_edge3, pcl::PointCloud::Ptr t_edge4, pcl::PointCloud::Ptr t_boundary_pts, - visualization_msgs::msg::MarkerArray & t_marker_array); + pcl::PointCloud::Ptr t_initial_corners_pts, + visualization_msgs::msg::MarkerArray & t_marker_array); void plotIdealFrame(); @@ -804,6 +832,14 @@ class LidarTag: public rclcpp::Node { void displayClusterPointSize(const std::vector & cluster_buff); void displayClusterIndexNumber(const std::vector & cluster_buff); + void addOrderedPointcloudMarkers(std::vector> & ordered_buff); + visualization_msgs::msg::MarkerArray ordered_pointcloud_markers_; + + void fixClusters( + const std::vector> & edge_buff, + std::vector & cluster_buff); + + }; // GrizTag } // namespace BipedLab #endif diff --git a/include/nanoflann.hpp b/include/lidartag/nanoflann.hpp similarity index 100% rename from include/nanoflann.hpp rename to include/lidartag/nanoflann.hpp diff --git a/include/lidartag/rectangle_estimator.hpp b/include/lidartag/rectangle_estimator.hpp new file mode 100644 index 0000000..3a465fd --- /dev/null +++ b/include/lidartag/rectangle_estimator.hpp @@ -0,0 +1,90 @@ +// Copyright 2021 Tier 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 RECTANGLE_ESTIMATOR_HPP +#define RECTANGLE_ESTIMATOR_HPP + +#include +#include +#include + +class RectangleEstimator { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + RectangleEstimator(); + + void setInputPoints( + pcl::PointCloud::Ptr & points1, + pcl::PointCloud::Ptr & points2, + pcl::PointCloud::Ptr & points3, + pcl::PointCloud::Ptr & points4); + + bool estimate(); + bool estimate_ransac(); + bool estimate_imlp( + pcl::PointCloud::Ptr points1, + pcl::PointCloud::Ptr points2, + pcl::PointCloud::Ptr points3, + pcl::PointCloud::Ptr points4, + Eigen::Vector2d & n_out, + Eigen::Vector4d & c_out); + + std::vector getCorners(); + std::vector getCorners(Eigen::Vector2d & n, Eigen::Vector4d & c); + + Eigen::Vector2d getNCoefficients() const; + Eigen::Vector4d getCCoefficients() const; + + void setRANSAC(bool enable); + void setFilterByCoefficients(bool enable); + void setMaxIterations(int iterations); + void setInlierError(double error); + void setFixPointGroups(bool enabe); + + void preparePointsMatrix(); + double getModelErrorAndInliers(Eigen::Vector2d & n, Eigen::Vector4d & c, int & inliers, + std::vector & inliers1, std::vector & inliers2, + std::vector & inliers3, std::vector & inliers4, + Eigen::VectorXd & errors1, Eigen::VectorXd & errors2, + Eigen::VectorXd & errors3, Eigen::VectorXd & errors4, + bool add_inliers); + +protected: + + bool checkCoefficients() const; + void fixPointGroups(Eigen::VectorXd & errors1, Eigen::VectorXd & errors2, + Eigen::VectorXd & errors3, Eigen::VectorXd & errors4); + + bool use_ransac_; + bool filter_by_coefficients_; + int max_ransac_iterations_; + double max_error_; + bool fix_point_groups_; + + Eigen::MatrixXd augmented_matrix_; + Eigen::MatrixXd points_matrix_; + Eigen::Vector4d estimated_c; + Eigen::Vector2d estimated_n; + + pcl::PointCloud::Ptr points1_; + pcl::PointCloud::Ptr points2_; + pcl::PointCloud::Ptr points3_; + pcl::PointCloud::Ptr points4_; + + pcl::PointCloud points; + +}; + +#endif // RECTANGLE_ESTIMATOR_HPP diff --git a/include/tag16h5.h b/include/lidartag/tag16h5.hpp similarity index 98% rename from include/tag16h5.h rename to include/lidartag/tag16h5.hpp index 7f46298..3d83074 100644 --- a/include/tag16h5.h +++ b/include/lidartag/tag16h5.hpp @@ -32,7 +32,7 @@ #ifndef _TAG16H5 #define _TAG16H5 -#include "lidartag.h" +#include BipedLab::GrizTagFamily_t *tag16h5_create(); void tag16h5_destroy(BipedLab::GrizTagFamily_t *tf); diff --git a/include/tag49h14.h b/include/lidartag/tag49h14.hpp similarity index 98% rename from include/tag49h14.h rename to include/lidartag/tag49h14.hpp index f3e7c31..209e622 100644 --- a/include/tag49h14.h +++ b/include/lidartag/tag49h14.hpp @@ -31,7 +31,7 @@ #ifndef _TAG49H14 #define _TAG49H14 -#include "lidartag.h" +#include BipedLab::GrizTagFamily_t *tag49h14_create(); void tag49h14_destroy(BipedLab::GrizTagFamily_t *tf); diff --git a/include/thread_pool.h b/include/lidartag/thread_pool.hpp similarity index 100% rename from include/thread_pool.h rename to include/lidartag/thread_pool.hpp diff --git a/include/types.h b/include/lidartag/types.hpp similarity index 95% rename from include/types.h rename to include/lidartag/types.hpp index 4b9d1fc..ae269bd 100644 --- a/include/types.h +++ b/include/lidartag/types.hpp @@ -28,6 +28,9 @@ * AUTHOR: Bruce JK Huang (bjhuang@umich.edu) * WEBSITE: https://www.brucerobot.com/ */ + +#pragma once + #include "nanoflann.hpp" #include #include @@ -194,6 +197,26 @@ typedef struct { point right; } corners; +enum class LidartagErrorCode //C++11 scoped enum +{ + NoError = 0, + ClusterMinPointsCriteria = 1, + ClusterMinPointsCriteria2 = 2, + PlanarCheckCriteria = 3, + PlanarOutliersCriteria = 4, + DecodingPointsCriteria = 5, + DecodingRingsCriteria = 6, + CornerEstimationMinPointsCriteria = 7, + Line1EstimationCriteria = 8, + Line2EstimationCriteria = 9, + Line3EstimationCriteria = 10, + Line4EstimationCriteria = 11, + RectangleEstimationCirteria = 12, + TagSizeEstimationCriteria = 13, + OptimizationErrorCriteria = 14, + DecodingErrorCriteria = 15 +}; + typedef struct ClusterFamily { // EIGEN_MAKE_ALIGNED_OPERATOR_NEW int cluster_id; @@ -215,6 +238,7 @@ typedef struct ClusterFamily { pcl::PointCloud data; // data doesn't have edge points pcl::PointCloud edge_points; pcl::PointCloud transformed_edge_points; + pcl::PointCloud initial_corners; // If the first point of the ring is the cluster. // If so, the the indices fo the two sides will be far away @@ -279,15 +303,10 @@ typedef struct ClusterFamily { */ std::vector line_coeff; // Upper, left, bottom, right line (count-clockwise) - int detail_valid; + LidartagErrorCode detail_valid; int pose_estimation_status; int expected_points; - corners tag_corners; // KL: adding here for now - corners tag_boundary_corners; - std::vector corner_offset_array; - std::vector boundary_corner_offset_array; - } ClusterFamily_t; typedef struct GrizTagFamily { diff --git a/include/ultra_puck.h b/include/lidartag/ultra_puck.hpp similarity index 98% rename from include/ultra_puck.h rename to include/lidartag/ultra_puck.hpp index 2524ba3..e906e64 100644 --- a/include/ultra_puck.h +++ b/include/lidartag/ultra_puck.hpp @@ -28,8 +28,8 @@ * WEBSITE: https://www.brucerobot.com/ */ -#ifndef ULTRA_PUCK_H -#define ULTRA_PuCK_H +#ifndef ULTRA_PUCK_HPP +#define ULTRA_PuCK_HPP namespace BipedLab { diff --git a/include/utils.h b/include/lidartag/utils.hpp similarity index 99% rename from include/utils.h rename to include/lidartag/utils.hpp index 4b11cec..90689ef 100644 --- a/include/utils.h +++ b/include/lidartag/utils.hpp @@ -28,9 +28,8 @@ * WEBSITE: https://www.brucerobot.com/ */ -#pragma once -#ifndef UTILS_H -#define UTILS_H +#ifndef UTILS_HPP +#define UTILS_HPP #include #include // for variadic functions @@ -43,7 +42,7 @@ #include #include -#include "lidartag.h" +#include #include #include diff --git a/launch/lidartag_master.launch.xml b/launch/lidartag_master.launch.xml index 3535570..2bc4c6d 100644 --- a/launch/lidartag_master.launch.xml +++ b/launch/lidartag_master.launch.xml @@ -33,7 +33,6 @@ - > @@ -187,7 +186,6 @@ - diff --git a/launch/lidartag_master_pandar.launch.xml b/launch/lidartag_master_pandar.launch.xml index 9fa52bd..798e999 100755 --- a/launch/lidartag_master_pandar.launch.xml +++ b/launch/lidartag_master_pandar.launch.xml @@ -34,10 +34,7 @@ - - - > @@ -62,27 +59,12 @@ + + - - - - - @@ -91,22 +73,19 @@ - + - - + + - + - - - - + @@ -130,16 +109,17 @@ + - - + + - + - + @@ -165,7 +145,7 @@ - + @@ -194,7 +174,6 @@ - @@ -211,17 +190,24 @@ - - - - + + + + + + + + - - - - - + + + + + + + + diff --git a/launch/lidartag_twotags.launch.xml b/launch/lidartag_twotags.launch.xml index 53ec95b..1db8a6a 100755 --- a/launch/lidartag_twotags.launch.xml +++ b/launch/lidartag_twotags.launch.xml @@ -35,7 +35,6 @@ - diff --git a/launch/lidartag_twotags_pandar.launch.xml b/launch/lidartag_twotags_pandar.launch.xml index 983337a..b748f20 100755 --- a/launch/lidartag_twotags_pandar.launch.xml +++ b/launch/lidartag_twotags_pandar.launch.xml @@ -33,21 +33,18 @@ - - - - + @@ -66,6 +63,4 @@ - - diff --git a/outputs/.gitkeep b/outputs/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/apriltag_utils.cpp b/src/apriltag_utils.cpp index 1e34d22..2e2b607 100644 --- a/src/apriltag_utils.cpp +++ b/src/apriltag_utils.cpp @@ -71,7 +71,7 @@ Michigan. */ -#include "apriltag_utils.h" +#include // Adapted/modified from apriltags code namespace BipedAprilLab diff --git a/src/lidartag.cpp b/src/lidartag.cpp index cfc8358..2c1b1a0 100644 --- a/src/lidartag.cpp +++ b/src/lidartag.cpp @@ -42,13 +42,12 @@ #include #include #include +#include -#include - -#include "lidartag.h" -#include "apriltag_utils.h" -#include "utils.h" -#include "ultra_puck.h" +#include +#include +#include +#include #include /* sqrt, pow(a,b) */ #include /* srand, rand */ @@ -58,6 +57,8 @@ #include // log files #include #include +#include +#include /* CONSTANT */ @@ -90,7 +91,7 @@ namespace BipedLab { LidarTag::LidarTag(const rclcpp::NodeOptions & options) : Node("lidar_tag_node", options), broadcaster_(*this), point_cloud_received_(0), - clock_(this->get_clock()), pub_frame_("velodyne"), // what frame of the published pointcloud should be + clock_(this->get_clock()), lidar_frame_("velodyne"), // what frame of the published pointcloud should be stop_(0) { LidarTag::getParameters(); @@ -112,15 +113,9 @@ LidarTag::LidarTag(const rclcpp::NodeOptions & options) : RCLCPP_INFO(get_logger(), "ALL INITIALIZED!"); - if (sensor_qos_) { - lidar1_sub_ = this->create_subscription( - pointcloud_topic_, rclcpp::SensorDataQoS(), std::bind(&LidarTag::pointCloudCallback, - this, std::placeholders::_1)); - } - else { - lidar1_sub_ = this->create_subscription( - pointcloud_topic_, 50, std::bind(&LidarTag::pointCloudCallback, this, std::placeholders::_1)); - } + lidar1_sub_ = this->create_subscription( + "pointcloud_input", rclcpp::SensorDataQoS(), std::bind(&LidarTag::pointCloudCallback, + this, std::placeholders::_1)); edge_pub_ = this->create_publisher("whole_edged_pc", 10); transformed_points_pub_ = @@ -132,6 +127,7 @@ LidarTag::LidarTag(const rclcpp::NodeOptions & options) : edge3_pub_ = this->create_publisher("edge_group_3", 10); edge4_pub_ = this->create_publisher("edge_group_4", 10); boundary_pub_ = this->create_publisher("boundary_pts", 10); + initial_corners_pub_ = this->create_publisher("initial_corners", 10); cluster_pub_ = this->create_publisher("detected_pc", 10); payload_pub_ = this->create_publisher("associated_pattern_3d", 10); payload3d_pub_ = this->create_publisher("template_points_3d", 10); @@ -157,7 +153,7 @@ LidarTag::LidarTag(const rclcpp::NodeOptions & options) : clustered_points_pub_ = this->create_publisher("cluster_edge_pc", 5); detection_array_pub_ = this->create_publisher( - lidartag_detections_topic_, 10); + "detections_array", 10); lidartag_cluster_pub_ = this->create_publisher("lidartag_cluster_points", 10); lidartag_cluster_edge_points_pub_ = @@ -165,10 +161,6 @@ LidarTag::LidarTag(const rclcpp::NodeOptions & options) : lidartag_cluster_transformed_edge_points_pub_ = this->create_publisher( "lidartag_cluster_trasformed_edge_points", 10); - //detail_valid_marker_array_pub_ = - // this->create_publisher("detail_valid_marker", 10); - //detail_valid_text_pub_ = - // this->create_publisher("detail_valid_text", 10); intersection_marker_array_pub_ = this->create_publisher("intesection_markers", 10); transformed_edge_pc_pub_ = @@ -178,28 +170,15 @@ LidarTag::LidarTag(const rclcpp::NodeOptions & options) : beforetransformed_edge_pc_pub_ = this->create_publisher("before_transformed_edge_pc", 10); corners_array_pub_ = - this->create_publisher(corners_array_topic_, 10); + this->create_publisher("corners_array", 10); corners_markers_pub_ = this->create_publisher("corners_markers", 10); - //left_corners_pub_ = this->create_publisher("left_corner", 10); - //right_corners_pub_ = this->create_publisher("right_corner", 10); - //down_corners_pub = this->create_publisher("down_corner", 10); - //top_corners_pub_ = this->create_publisher("top_corner", 10); - boundary_corners_array_pub_ = this->create_publisher("boundary_corners_array", 10); boundary_corners_markers_pub_ = this->create_publisher("boundary_corners_markers", 10); - //left_boundary_corners_pub_ = - // this->create_publisher("left_boundary_corner", 10); - //right_boundary_corners_pub_ = - // this->create_publisher("right_boundary_corner", 10); - //down_boundary_corners_pub_ = - // this->create_publisher("down_boundary_corner", 10); - //top_boundary_corners_pub_ = - // this->create_publisher("top_boundary_corner", 10); colored_cluster_buff_pub_ = this->create_publisher("colored_cluster_buff", 10); @@ -209,6 +188,8 @@ LidarTag::LidarTag(const rclcpp::NodeOptions & options) : "cluster_buff_index_number_markers", 10); boundary_points_pub_ = this->create_publisher("boundary_points", 10); + ordered_pointcloud_markers_pub_ = + this->create_publisher("ordered_pointcloud_markers", 10); RCLCPP_INFO(get_logger(), "Waiting for pointcloud data"); @@ -237,7 +218,6 @@ rcl_interfaces::msg::SetParametersResult LidarTag::paramCallback( UPDATE_LIDARTAG_PARAM(params, ransac_threshold); UPDATE_LIDARTAG_PARAM(params, fine_cluster_threshold); UPDATE_LIDARTAG_PARAM(params, filling_gap_max_index); - UPDATE_LIDARTAG_PARAM(params, filling_max_points_threshold); UPDATE_LIDARTAG_PARAM(params, points_threshold_factor); UPDATE_LIDARTAG_PARAM(params, distance_to_plane_threshold); UPDATE_LIDARTAG_PARAM(params, max_outlier_ratio); @@ -250,11 +230,22 @@ rcl_interfaces::msg::SetParametersResult LidarTag::paramCallback( UPDATE_LIDARTAG_PARAM(params, cluster_min_index); UPDATE_LIDARTAG_PARAM(params, cluster_max_points_size); UPDATE_LIDARTAG_PARAM(params, cluster_min_points_size); + UPDATE_LIDARTAG_PARAM(params, depth_bound); + UPDATE_LIDARTAG_PARAM(params, min_rkhs_score); + UPDATE_LIDARTAG_PARAM(params, optional_fix_cluster); + UPDATE_LIDARTAG_PARAM(params, use_rectangle_model); + UPDATE_LIDARTAG_PARAM(params, rectangle_model_use_ransac); + UPDATE_LIDARTAG_PARAM(params, rectangle_model_max_iterations); + UPDATE_LIDARTAG_PARAM(params, rectangle_model_max_error); + UPDATE_LIDARTAG_PARAM(params, rectangle_fix_point_groups); + UPDATE_LIDARTAG_PARAM(params, refine_cluster_with_intersections); UPDATE_LIDARTAG_PARAM(params, debug_single_pointcloud); UPDATE_LIDARTAG_PARAM(params, debug_point_x); UPDATE_LIDARTAG_PARAM(params, debug_point_y); UPDATE_LIDARTAG_PARAM(params, debug_point_z); UPDATE_LIDARTAG_PARAM(params, debug_cluster_id); + UPDATE_LIDARTAG_PARAM(params, debug_ring_id); + UPDATE_LIDARTAG_PARAM(params, debug_scan_id); // transaction succeeds, now assign values params_ = params; @@ -263,6 +254,15 @@ rcl_interfaces::msg::SetParametersResult LidarTag::paramCallback( result.reason = e.what(); } + // These parameters are linked , so it is better to update them like this + params_.linkage_threshold = params_.linkage_tunable * payload_size_ * clearance_; + + rectangle_estimator_->setFilterByCoefficients(false); + rectangle_estimator_->setInlierError(params_.rectangle_model_max_error); + rectangle_estimator_->setFixPointGroups(params_.rectangle_fix_point_groups); + rectangle_estimator_->setMaxIterations(params_.rectangle_model_max_iterations); + rectangle_estimator_->setRANSAC(params_.rectangle_model_use_ransac); + return result; } @@ -288,6 +288,7 @@ void LidarTag::mainLoop() pcl::PointCloud::Ptr edge_group2(new pcl::PointCloud); pcl::PointCloud::Ptr edge_group3(new pcl::PointCloud); pcl::PointCloud::Ptr edge_group4(new pcl::PointCloud); + pcl::PointCloud::Ptr initial_corners_pc(new pcl::PointCloud); clusterpc->reserve(point_cloud_size_); clusteredgepc->reserve(point_cloud_size_); @@ -300,6 +301,7 @@ void LidarTag::mainLoop() payloadpc->reserve(point_cloud_size_); payload3dpc->reserve(point_cloud_size_); boundarypc->reserve(point_cloud_size_); + initial_corners_pc->reserve(point_cloud_size_); int valgrind_check = 0; @@ -375,6 +377,10 @@ void LidarTag::mainLoop() edge_group2->clear(); edge_group3->clear(); edge_group4->clear(); + initial_corners_pc->clear(); + + ordered_pointcloud_markers_pub_->publish(ordered_pointcloud_markers_); + ordered_pointcloud_markers_.markers.clear(); for (int ring = 0; ring < beam_num_; ++ring) { std::vector().swap(ordered_buff[ring]); @@ -386,35 +392,36 @@ void LidarTag::mainLoop() LidarTag::clusterToPclVectorAndMarkerPublisher( clusterbuff, clusterpc, clusteredgepc, payloadpc, payload3dpc, tagpc, ini_tagpc, edge_group1, - edge_group2, edge_group3, edge_group4, boundarypc, cluster_markers); + edge_group2, edge_group3, edge_group4, boundarypc, initial_corners_pc, cluster_markers); // publish lidartag corners // publish results for rviz LidarTag::plotIdealFrame(); - LidarTag::publishPointcloud(extracted_poi_pc, pub_frame_, string("wholeedge")); - LidarTag::publishPointcloud(clusteredgepc, pub_frame_, string("clusteredgepc")); - LidarTag::publishPointcloud(clusterpc, pub_frame_, string("cluster")); - LidarTag::publishPointcloud(edge_group1, pub_frame_, string("edgegroup1")); - LidarTag::publishPointcloud(edge_group2, pub_frame_, string("edgegroup2")); - LidarTag::publishPointcloud(edge_group3, pub_frame_, string("edgegroup3")); - LidarTag::publishPointcloud(edge_group4, pub_frame_, string("edgegroup4")); - LidarTag::publishPointcloud(boundarypc, pub_frame_, string("boundarypc")); + LidarTag::publishPointcloud(extracted_poi_pc, lidar_frame_, string("wholeedge")); + LidarTag::publishPointcloud(clusteredgepc, lidar_frame_, string("clusteredgepc")); + LidarTag::publishPointcloud(clusterpc, lidar_frame_, string("cluster")); + LidarTag::publishPointcloud(edge_group1, lidar_frame_, string("edgegroup1")); + LidarTag::publishPointcloud(edge_group2, lidar_frame_, string("edgegroup2")); + LidarTag::publishPointcloud(edge_group3, lidar_frame_, string("edgegroup3")); + LidarTag::publishPointcloud(edge_group4, lidar_frame_, string("edgegroup4")); + LidarTag::publishPointcloud(boundarypc, lidar_frame_, string("boundarypc")); + LidarTag::publishPointcloud(initial_corners_pc, lidar_frame_, string("initialcornerspc")); if (collect_dataset_) { if (result_statistics_.remaining_cluster_size == 1) { - LidarTag::publishPointcloud(payloadpc, pub_frame_, string("payload")); - LidarTag::publishPointcloud(payload3dpc, pub_frame_, string("payload3d")); - LidarTag::publishPointcloud(tagpc, pub_frame_, string("target")); - LidarTag::publishPointcloud(ini_tagpc, pub_frame_, string("initialtarget")); + LidarTag::publishPointcloud(payloadpc, lidar_frame_, string("payload")); + LidarTag::publishPointcloud(payload3dpc, lidar_frame_, string("payload3d")); + LidarTag::publishPointcloud(tagpc, lidar_frame_, string("target")); + LidarTag::publishPointcloud(ini_tagpc, lidar_frame_, string("initialtarget")); } else if (result_statistics_.remaining_cluster_size > 1) cout << "More than one!! " << endl; else cout << "Zero!! " << endl; } else { - LidarTag::publishPointcloud(payloadpc, pub_frame_, string("payload")); - LidarTag::publishPointcloud(payload3dpc, pub_frame_, string("payload3d")); - LidarTag::publishPointcloud(tagpc, pub_frame_, string("target")); - LidarTag::publishPointcloud(ini_tagpc, pub_frame_, string("initialtarget")); + LidarTag::publishPointcloud(payloadpc, lidar_frame_, string("payload")); + LidarTag::publishPointcloud(payload3dpc, lidar_frame_, string("payload3d")); + LidarTag::publishPointcloud(tagpc, lidar_frame_, string("target")); + LidarTag::publishPointcloud(ini_tagpc, lidar_frame_, string("initialtarget")); } if (sleep_to_display_) { @@ -435,7 +442,7 @@ void LidarTag::mainLoop() break; } } - } // ros::ok() + } // rclcpp::ok() } @@ -447,11 +454,7 @@ void LidarTag::getParameters() { std::string tag_size_string; - this->declare_parameter("sensor_qos"); - this->declare_parameter("frame_name"); this->declare_parameter("distance_threshold"); - this->declare_parameter("lidartag_detections_topic"); - this->declare_parameter("corners_array_topic"); this->declare_parameter("sleep_to_display"); this->declare_parameter("sleep_time_for_visulization"); this->declare_parameter("valgrind_check"); @@ -525,15 +528,20 @@ void LidarTag::getParameters() { this->declare_parameter("debug_point_y"); this->declare_parameter("debug_point_z"); this->declare_parameter("debug_cluster_id"); + this->declare_parameter("debug_ring_id"); + this->declare_parameter("debug_scan_id"); this->declare_parameter("pcl_visualize_cluster"); this->declare_parameter("clearance"); + this->declare_parameter("optional_fix_cluster"); + this->declare_parameter("use_rectangle_model"); + this->declare_parameter("rectangle_model_use_ransac"); + this->declare_parameter("rectangle_model_max_iterations"); + this->declare_parameter("rectangle_model_max_error"); + this->declare_parameter("rectangle_fix_point_groups"); + this->declare_parameter("refine_cluster_with_intersections"); + this->declare_parameter("min_rkhs_score"); - bool GotSensorQOS = this->get_parameter("sensor_qos", sensor_qos_); - bool GotPubFrame = this->get_parameter("frame_name", pub_frame_); bool GotThreshold = this->get_parameter("distance_threshold", distance_threshold_); - bool GotPublishTopic = - this->get_parameter("lidartag_detections_topic", lidartag_detections_topic_); - bool GotCornersArrayTopic = this->get_parameter("corners_array_topic", corners_array_topic_); bool GotSleepToDisplay = this->get_parameter("sleep_to_display", sleep_to_display_); bool GotSleepTimeForVis = this->get_parameter("sleep_time_for_visulization", sleep_time_for_vis_); bool GotValgrindCheck = this->get_parameter("valgrind_check", valgrind_check_); @@ -547,7 +555,6 @@ void LidarTag::getParameters() { bool GotAdaptiveThresholding = this->get_parameter("adaptive_thresholding", adaptive_thresholding_); bool GotCollectData = this->get_parameter("collect_data", collect_dataset_); - bool GotLidarTopic = this->get_parameter("pointcloud_topic", pointcloud_topic_); bool GotMaxQueueSize = this->get_parameter("max_queue_size", max_queue_size_); bool GotBeamNum = this->get_parameter("beam_number", beam_num_); bool GotSize = this->get_parameter("tag_size", payload_size_); @@ -558,7 +565,7 @@ void LidarTag::getParameters() { bool GotBlackBorder = this->get_parameter("black_border", black_border_); bool GotDistanceBound = this->get_parameter("distance_bound", distance_bound_); - bool GotDepthBound = this->get_parameter("depth_bound", depth_threshold_); + bool GotDepthBound = this->get_parameter("depth_bound", params_.depth_bound); bool GotFineClusterThreshold = this->get_parameter("fine_cluster_threshold", params_.fine_cluster_threshold); bool GotVerticalFOV = this->get_parameter("vertical_fov", vertical_fov_); @@ -620,29 +627,58 @@ void LidarTag::getParameters() { this->get_parameter("debug_point_z", params_.debug_point_z); bool GotDebugClusterId = this->get_parameter("debug_cluster_id", params_.debug_cluster_id); + bool GotDebugRingId = + this->get_parameter("debug_ring_id", params_.debug_ring_id); + bool GotDebugScanId = + this->get_parameter("debug_scan_id", params_.debug_scan_id); bool GotVisualizeCluster = this->get_parameter("pcl_visualize_cluster", pcl_visualize_cluster_); bool GotClearance = this->get_parameter("clearance", clearance_); + bool GotOptionalFixCluster = this->get_parameter("optional_fix_cluster", + params_.optional_fix_cluster); + bool GotUSeRectangleModel = this->get_parameter("use_rectangle_model", + params_.use_rectangle_model); + bool GotRectangleModelUseRansac = this->get_parameter("rectangle_model_use_ransac", + params_.rectangle_model_use_ransac); + bool GotRectangleModelMaxIterations = this->get_parameter("rectangle_model_max_iterations", + params_.rectangle_model_max_iterations); + bool GotRectangleMaxError = this->get_parameter("rectangle_model_max_error", + params_.rectangle_model_max_error); + bool GotRectangleFixPointGroups = this->get_parameter("rectangle_fix_point_groups", + params_.rectangle_fix_point_groups); + + + bool GotRefineClusterWithInteractions = this->get_parameter("refine_cluster_with_intersections", + params_.refine_cluster_with_intersections); + bool GotMinRKHSScore = this->get_parameter("min_rkhs_score", params_.min_rkhs_score); + + rectangle_estimator_ = std::make_shared(); + rectangle_estimator_->setFilterByCoefficients(false); + rectangle_estimator_->setInlierError(params_.rectangle_model_max_error); + rectangle_estimator_->setFixPointGroups(params_.rectangle_fix_point_groups); + rectangle_estimator_->setMaxIterations(params_.rectangle_model_max_iterations); + rectangle_estimator_->setRANSAC(params_.rectangle_model_use_ransac); std::istringstream is(tag_size_string); tag_size_list_.assign( std::istream_iterator( is ), std::istream_iterator() ); bool pass = utils::checkParameters( - {GotSensorQOS, GotPubFrame, GotFakeTag, GotLidarTopic, GotMaxQueueSize, GotBeamNum, GotOptPose, - GotDecodeId, GotPlaneFitting, GotOutPutPath, GotDistanceBound, - GotDepthBound, GotTagFamily, GotTagHamming, GotMaxDecodeHamming, GotFineClusterThreshold, - GotVerticalFOV, GotFillInGapThreshold, GotMaxOutlierRatio, GotPointsThresholdFactor, - GotDistanceToPlaneThreshold, GotAdaptiveThresholding, GotCollectData, - GotSleepToDisplay, GotSleepTimeForVis, GotValgrindCheck, GotPayloadIntensityThreshold, - GotBlackBorder, GotMinPerGrid, - GotDecodeMethod, GotDecodeMode, GotOptimizationMethod, GotGridViz, GotPublishTopic, - GotCornersArrayTopic, GotThreshold, GotNumPoints, GotNearBound, GotNumPointsRing, - GotCoefficient, GotTagSizeList, GotNumThreads, GotPrintInfo, GotOptimizePercent, - GotDebuginfo, GotDebugtime, GotLogData, GotDebugDecodingtime, GotLibraryPath, - GotNumCodes, GotCalibration, GotMinimumRingPoints, GotRingState, GotRingEstimation, - GotNumAccumulation, GotDerivativeMethod, GotUpbound, GotLowbound, GotCoaTunable, - GotTagsizeTunable, GotMaxClusterIndex, GotMinClusterIndex, GotMaxClusterPointsSize, - GotMinClusterPointsSize, GotDebugSinglePointcloud, GotDebugPointX, GotDebugPointY, - GotDebugPointZ, GotDebugClusterId, GotVisualizeCluster, GotClearance}); + {GotFakeTag, GotMaxQueueSize, GotBeamNum, GotOptPose, GotDecodeId, GotPlaneFitting, + GotOutPutPath, GotDistanceBound, GotDepthBound, GotTagFamily, GotTagHamming, + GotMaxDecodeHamming, GotFineClusterThreshold, GotVerticalFOV, GotFillInGapThreshold, + GotMaxOutlierRatio, GotPointsThresholdFactor, GotDistanceToPlaneThreshold, + GotAdaptiveThresholding, GotCollectData, GotSleepToDisplay, GotSleepTimeForVis, + GotValgrindCheck, GotPayloadIntensityThreshold, GotBlackBorder, GotMinPerGrid, + GotDecodeMethod, GotDecodeMode, GotOptimizationMethod, GotGridViz, GotThreshold, GotNumPoints, + GotNearBound, GotNumPointsRing, GotCoefficient, GotTagSizeList, GotNumThreads, GotPrintInfo, + GotOptimizePercent, GotDebuginfo, GotDebugtime, GotLogData, GotDebugDecodingtime, + GotLibraryPath, GotNumCodes, GotCalibration, GotMinimumRingPoints, GotRingState, + GotRingEstimation, GotNumAccumulation, GotDerivativeMethod, GotUpbound, GotLowbound, + GotCoaTunable, GotTagsizeTunable, GotMaxClusterIndex, GotMinClusterIndex, + GotMaxClusterPointsSize, GotMinClusterPointsSize, GotDebugSinglePointcloud, GotDebugPointX, + GotDebugPointY, GotDebugPointZ, GotDebugClusterId, GotDebugRingId, GotDebugScanId, + GotVisualizeCluster, GotClearance, GotOptionalFixCluster, GotUSeRectangleModel, + GotRectangleModelUseRansac, GotRectangleModelMaxIterations, GotRectangleMaxError, + GotRefineClusterWithInteractions, GotMinRKHSScore}); if (!pass) { rclcpp::shutdown(); @@ -665,16 +701,13 @@ void LidarTag::getParameters() { params_.ransac_threshold = payload_size_ / 10; - RCLCPP_INFO(get_logger(), "Subscribe to %s\n", pointcloud_topic_.c_str()); RCLCPP_INFO(get_logger(), "Use %i-beam LiDAR\n", beam_num_); RCLCPP_INFO(get_logger(), "Use %i threads\n", num_threads_); - RCLCPP_INFO(get_logger(), "depth_threshold_: %f \n", depth_threshold_); + RCLCPP_INFO(get_logger(), "depth_bound: %f \n", params_.depth_bound); RCLCPP_INFO(get_logger(), "payload_size_: %f \n", payload_size_); RCLCPP_INFO(get_logger(), "vertical_fov_: %f \n", vertical_fov_); RCLCPP_INFO(get_logger(), "fine_cluster_threshold: %i \n", params_.fine_cluster_threshold); RCLCPP_INFO(get_logger(), "filling_gap_max_index: %i \n", params_.filling_gap_max_index); - // RCLCPP_INFO(get_logger(), "_filling_max_points_threshold: %i \n", - // _filling_max_points_threshold); RCLCPP_INFO(get_logger(), "points_threshold_factor: %f \n", params_.points_threshold_factor); RCLCPP_INFO(get_logger(), "adaptive_thresholding_: %i \n", adaptive_thresholding_); RCLCPP_INFO(get_logger(), "collect_dataset_: %i \n", collect_dataset_); @@ -951,11 +984,13 @@ void LidarTag::publishPointcloud( edge4_pub_->publish(pcs_waited_to_pub); else if (which_publisher == "boundarypc") boundary_pub_->publish(pcs_waited_to_pub); + else if (which_publisher == "initialcornerspc") + initial_corners_pub_->publish(pcs_waited_to_pub); else if (which_publisher == "initialtarget") initag_pub_->publish(pcs_waited_to_pub); else if (which_publisher == "clusteredgepc") { pcs_waited_to_pub.header.stamp = current_scan_time_; - pcs_waited_to_pub.header.frame_id = pub_frame_; + pcs_waited_to_pub.header.frame_id = lidar_frame_; clustered_points_pub_->publish(pcs_waited_to_pub); } else if (which_publisher == "transpts") @@ -978,6 +1013,8 @@ void LidarTag::publishPointcloud( */ void LidarTag::pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr pc) { + lidar_frame_ = pc->header.frame_id; + // flag to make sure it receives a pointcloud // at the very begining of the program @@ -1004,7 +1041,7 @@ void LidarTag::pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr point_cloud1_queue_lock_.lock(); - while (point_cloud1_queue_.size() > max_queue_size_) + while (point_cloud1_queue_.size() >= max_queue_size_) point_cloud1_queue_.pop(); if (params_.debug_single_pointcloud) { @@ -1053,6 +1090,9 @@ inline void LidarTag::fillInOrderedPointcloud( p.ring = std::distance(lidar_system_.angle_list.begin(), it); } + if(p.z < params_.debug_point_z) + continue; + assert(("Ring Estimation Error", p.ring < beam_num_)); lidar_point.point = p; @@ -1061,6 +1101,8 @@ inline void LidarTag::fillInOrderedPointcloud( ordered_buff[p.ring].push_back(lidar_point); index[p.ring] += 1; } + + addOrderedPointcloudMarkers(ordered_buff); } /* @@ -1226,6 +1268,15 @@ void LidarTag::gradientAndGroupEdges( // 3 means two side points are edge points int edge_flag = LidarTag::getEdgePoints(ordered_buff, i, j, n); + /* debug_current_ring = i; + debug_current_scan = j; + + if (j == params_.debug_scan_id && + i == params_.debug_ring_id) + { + int x = 0; + } */ + if (edge_flag == 0) { continue; } @@ -1260,6 +1311,11 @@ void LidarTag::gradientAndGroupEdges( } } } + + // this is a one pass algorithm prone to miss some edges -> revisit + if (params_.optional_fix_cluster) { + fixClusters(edge_buff, cluster_buff); + } } /* @@ -1303,17 +1359,51 @@ int LidarTag::getEdgePoints( (point.getVector3fMap() - PointR.getVector3fMap()).norm()); double DepthGrad2 = std::abs( - (Point2L.getVector3fMap() - point2.getVector3fMap()).norm() - - (point2.getVector3fMap() - Point2R.getVector3fMap()).norm()); - - if (DepthGrad1 > depth_threshold_) { - if (DepthGrad2 > depth_threshold_) { + (Point2L.getVector3fMap() - point2.getVector3fMap()).norm() - + (point2.getVector3fMap() - Point2R.getVector3fMap()).norm()); + + if (i == params_.debug_ring_id) { + + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = lidar_frame_; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(5.0); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.scale.z = 0.02; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + + int distance = std::max(DepthGrad1, DepthGrad2) * 100; + //std::stringstream stream; + //stream << std::fixed << std::setprecision(2) << distance; + + marker.header.stamp = clock_->now(); + marker.id = j; + marker.text = distance > 100 ? "x" : to_string(distance); + marker.ns = "distance_" + to_string(i); + marker.pose.position.x = ordered_buff[i][j].point.x; + marker.pose.position.y = ordered_buff[i][j].point.y; + marker.pose.position.z = ordered_buff[i][j].point.z; + ordered_pointcloud_markers_.markers.push_back(marker); + } + + if (DepthGrad1 > params_.depth_bound) { + if (DepthGrad2 > params_.depth_bound) { return 3; } else { return 1; } } else { - if (DepthGrad2 > depth_threshold_) { + if (DepthGrad2 > params_.depth_bound) { return 2; } else { return 0; @@ -1351,6 +1441,7 @@ void LidarTag::fillInCluster( // tbb::parallel_for(int(0), (int)cluster_buff.size(), [&](int i) { for (int i = 0; i < cluster_buff.size(); ++i) { + ClusterFamily_t & cluster = cluster_buff[i]; // In a cluster // tbb::parallel_for(int(0), (int)cluster_buff.size(), [&](int i) { if (debug_time_) { @@ -1358,8 +1449,14 @@ void LidarTag::fillInCluster( } for (int j = 0; j < beam_num_; ++j) { - int max_index = cluster_buff[i].max_min_index_of_each_ring[j].max; - int min_index = cluster_buff[i].max_min_index_of_each_ring[j].min; + int max_index = cluster.max_min_index_of_each_ring[j].max; + int min_index = cluster.max_min_index_of_each_ring[j].min; + + /*if (cluster.cluster_id == params_.debug_cluster_id && + j == params_.debug_ring_id) + { + int x = 0; + } */ // no points on this ring if ((std::abs(min_index - 1e5) < 1e-5) || std::abs(max_index + 1) < 1e-5) { @@ -1383,28 +1480,28 @@ void LidarTag::fillInCluster( // (it has been in the cloud already) for (int k = min_index + 1; k < max_index; ++k) { // cout << "k1: " << k << endl; - if (isWithinClusterHorizon(ordered_buff[j][k], cluster_buff[i], 0)) { - cluster_buff[i].data.push_back(ordered_buff[j][k]); + if (isWithinClusterHorizon(ordered_buff[j][k], cluster, 0)) { + cluster.data.push_back(ordered_buff[j][k]); } } - cluster_buff[i].special_case = 0; + cluster.special_case = 0; } else { for (int k = 0; k < min_index; ++k) { // cout << "k2: " << k << endl; - if (isWithinClusterHorizon(ordered_buff[j][k], cluster_buff[i], 0)) { - cluster_buff[i].data.push_back(ordered_buff[j][k]); + if (isWithinClusterHorizon(ordered_buff[j][k], cluster, 0)) { + cluster.data.push_back(ordered_buff[j][k]); } } for (int k = max_index; k < ordered_buff[j].size(); ++k) { // cout << "k3: " << k << endl; - if (isWithinClusterHorizon(ordered_buff[j][k], cluster_buff[i], 0)) { - cluster_buff[i].data.push_back(ordered_buff[j][k]); + if (isWithinClusterHorizon(ordered_buff[j][k], cluster, 0)) { + cluster.data.push_back(ordered_buff[j][k]); } } - cluster_buff[i].special_case = 1; + cluster.special_case = 1; } } @@ -1418,22 +1515,22 @@ void LidarTag::fillInCluster( auto min_returns = min_returns_per_grid_ * std::pow((std::sqrt(tag_family_) + 2 * black_border_), 2); - if ((cluster_buff[i].data.size() + cluster_buff[i].edge_points.size()) < min_returns) { + if ((cluster.data.size() + cluster.edge_points.size()) < min_returns) { result_statistics_.cluster_removal.minimum_return++; result_statistics_.remaining_cluster_size--; if (mark_cluster_validity_) { - cluster_buff[i].valid = false; - cluster_buff[i].detail_valid = 1; + cluster.valid = false; + cluster.detail_valid = LidartagErrorCode::ClusterMinPointsCriteria; } // tbb::task::self().cancel_group_execution(); continue; } // Mark cluster as invalid if too many points in cluster - cluster_buff[i].expected_points = maxPointsCheck(cluster_buff[i]); + cluster.expected_points = maxPointsCheck(cluster); - if (cluster_buff[i].valid == false) { + if (cluster.valid == false) { // tbb::task::self().cancel_group_execution(); continue; } @@ -1450,10 +1547,10 @@ void LidarTag::fillInCluster( } else { pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - if (!rejectWithPlanarCheck(cluster_buff[i], inliers, coefficients, fplanefit)) { + if (!rejectWithPlanarCheck(cluster, inliers, coefficients, fplanefit)) { if (mark_cluster_validity_) { - cluster_buff[i].valid = false; - cluster_buff[i].detail_valid = 3; + cluster.valid = false; + cluster.detail_valid = LidartagErrorCode::PlanarCheckCriteria; } // tbb::task::self().cancel_group_execution(); continue; @@ -1462,18 +1559,18 @@ void LidarTag::fillInCluster( // Mark cluster as invalid if too many outliers in plane fitting auto percentage_inliers = inliers->indices.size() / - double(cluster_buff[i].data.size() + cluster_buff[i].edge_points.size()); + double(cluster.data.size() + cluster.edge_points.size()); - cluster_buff[i].percentages_inliers = percentage_inliers; + cluster.percentages_inliers = percentage_inliers; if (debug_info_) { RCLCPP_DEBUG_STREAM(get_logger(), "==== _planeOutliers ===="); float distance = std::sqrt( - pow(cluster_buff[i].average.x, 2) + pow(cluster_buff[i].average.y, 2) + - pow(cluster_buff[i].average.z, 2)); + pow(cluster.average.x, 2) + pow(cluster.average.y, 2) + + pow(cluster.average.z, 2)); RCLCPP_DEBUG_STREAM(get_logger(), "Distance : " << distance); RCLCPP_DEBUG_STREAM(get_logger(), - "Actual Points: " << cluster_buff[i].data.size() + cluster_buff[i].edge_points.size()); + "Actual Points: " << cluster.data.size() + cluster.edge_points.size()); } if (percentage_inliers < (1.0 - params_.max_outlier_ratio)) { @@ -1485,8 +1582,8 @@ void LidarTag::fillInCluster( result_statistics_.remaining_cluster_size--; if (mark_cluster_validity_) { - cluster_buff[i].valid = false; - cluster_buff[i].detail_valid = 4; + cluster.valid = false; + cluster.detail_valid = LidartagErrorCode::PlanarOutliersCriteria; continue; } } @@ -1497,22 +1594,22 @@ void LidarTag::fillInCluster( // Remove all outliers from cluster auto outliers_indices = utils::complementOfSet( - inliers->indices, (cluster_buff[i].data.size() + cluster_buff[i].edge_points.size())); + inliers->indices, (cluster.data.size() + cluster.edge_points.size())); - int edge_inlier = cluster_buff[i].edge_points.size(); + int edge_inlier = cluster.edge_points.size(); for (int k = 0; k < outliers_indices.size(); ++k) { int out_index = outliers_indices[k]; - if (out_index < cluster_buff[i].edge_points.size()) { + if (out_index < cluster.edge_points.size()) { edge_inlier -= 1; - cluster_buff[i].edge_points[out_index].valid = 0; + cluster.edge_points[out_index].valid = 0; } else { - cluster_buff[i].data[out_index - cluster_buff[i].edge_points.size()].valid = 0; + cluster.data[out_index - cluster.edge_points.size()].valid = 0; } } - cluster_buff[i].edge_inliers = edge_inlier; + cluster.edge_inliers = edge_inlier; if (debug_time_) { timing_.plane_fitting_removal_time += @@ -1521,17 +1618,17 @@ void LidarTag::fillInCluster( } } - if (!LidarTag::adaptiveThresholding(cluster_buff[i])) { + if (!LidarTag::adaptiveThresholding(cluster)) { // removal has been done inside the function if (mark_cluster_validity_) { - cluster_buff[i].valid = false; + cluster.valid = false; } // cluster_buff.erase(cluster_buff.begin()+i); // i--; } else { if (print_ros_info_ || debug_info_) { - RCLCPP_INFO_STREAM(get_logger(), "--ID: " << cluster_buff[i].cluster_id); - RCLCPP_INFO_STREAM(get_logger(), "---rotation: " << cluster_buff[i].rkhs_decoding.rotation_angle); + RCLCPP_INFO_STREAM(get_logger(), "--ID: " << cluster.cluster_id); + RCLCPP_INFO_STREAM(get_logger(), "---rotation: " << cluster.rkhs_decoding.rotation_angle); } } // } @@ -1607,7 +1704,7 @@ bool LidarTag::adaptiveThresholding(ClusterFamily_t & cluster) cluster.pose_estimation_status = status; if (status < 0) { - cluster.detail_valid = 13; + cluster.detail_valid = LidartagErrorCode::OptimizationErrorCriteria; return false; } else { @@ -1632,7 +1729,7 @@ bool LidarTag::adaptiveThresholding(ClusterFamily_t & cluster) if (!LidarTag::decodePayload(cluster)) { result_statistics_.cluster_removal.decoding_failure++; result_statistics_.remaining_cluster_size--; - cluster.detail_valid = 14; + cluster.detail_valid = LidartagErrorCode::DecodingErrorCriteria; return false; } else { if (debug_time_) { @@ -1729,7 +1826,7 @@ void LidarTag::tagToRobot( geometry_msgs::msg::TransformStamped transform_msg; transform_msg.header.stamp = point_cloud_header_.stamp; - transform_msg.header.frame_id = pub_frame_; + transform_msg.header.frame_id = lidar_frame_; transform_msg.child_frame_id = to_string(cluster_id)+"_rotated"; transform_msg.transform = tf2::toMsg(transform); @@ -1738,7 +1835,7 @@ void LidarTag::tagToRobot( tf2::Quaternion q2(normal_vec(0), normal_vec(1), normal_vec(2), 0); transform.setRotation(q2); transform_msg.header.stamp = point_cloud_header_.stamp; - transform_msg.header.frame_id = pub_frame_; + transform_msg.header.frame_id = lidar_frame_; transform_msg.child_frame_id = "LidarTag-ID" + to_string(cluster_id); transform_msg.transform = tf2::toMsg(transform); @@ -2013,12 +2110,12 @@ bool LidarTag::detectPayloadBoundries(ClusterFamily_t & cluster) result_statistics_.cluster_removal.boundary_point_check++; result_statistics_.remaining_cluster_size--; boundary_flag = false; - cluster.detail_valid = 5; + cluster.detail_valid = LidartagErrorCode::DecodingPointsCriteria; } else if (num_valid_rings < std::min(int(sqrt(tag_family_)), params_.minimum_ring_boundary_points)) { result_statistics_.cluster_removal.minimum_ring_points++; result_statistics_.remaining_cluster_size--; ring_point_flag = false; - cluster.detail_valid = 6; + cluster.detail_valid = LidartagErrorCode::DecodingRingsCriteria; } if (debug_info_) { @@ -2149,6 +2246,139 @@ Eigen::Vector3f LidarTag::estimateEdgeVector(ClusterFamily_t & cluster) return edge_vector; } +/* + * A function to estimate the four corners of a tag using 4x line RANSAC + */ +bool LidarTag::estimateCornersUsingLinesRANSAC(ClusterFamily_t & cluster, + pcl::PointCloud::Ptr & cloud1, pcl::PointCloud::Ptr & cloud2, + pcl::PointCloud::Ptr & cloud3, pcl::PointCloud::Ptr & cloud4, + Eigen::Vector3f & intersection1, Eigen::Vector3f & intersection2, + Eigen::Vector3f & intersection3, Eigen::Vector3f & intersection4) +{ + // To do: function this part + // get fitted line for points on the 1th side of the tag + Eigen::Vector4f line1; + Eigen::Vector4f line2; + Eigen::Vector4f line3; + Eigen::Vector4f line4; + pcl::PointCloud::Ptr line_cloud1(new pcl::PointCloud); + pcl::PointCloud::Ptr line_cloud2(new pcl::PointCloud); + pcl::PointCloud::Ptr line_cloud3(new pcl::PointCloud); + pcl::PointCloud::Ptr line_cloud4(new pcl::PointCloud); + line_cloud1->reserve(point_cloud_size_); + line_cloud1->clear(); + line_cloud2->reserve(point_cloud_size_); + line_cloud2->clear(); + line_cloud3->reserve(point_cloud_size_); + line_cloud3->clear(); + line_cloud4->reserve(point_cloud_size_); + line_cloud4->clear(); + + if (!LidarTag::getLines(cloud1, line1, line_cloud1)) { + if (debug_info_) { + RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); + } + + cluster.detail_valid = LidartagErrorCode::Line1EstimationCriteria; + return false; + } + if (!LidarTag::getLines(cloud2, line2, line_cloud2)) { + if (debug_info_) { + RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); + } + + cluster.detail_valid = LidartagErrorCode::Line2EstimationCriteria; + return false; + } + if (!LidarTag::getLines(cloud3, line3, line_cloud3)) { + if (debug_info_) { + RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); + } + + cluster.detail_valid = LidartagErrorCode::Line3EstimationCriteria; + return false; + } + if (!LidarTag::getLines(cloud4, line4, line_cloud4)) { + if (debug_info_) { + RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); + } + + cluster.detail_valid = LidartagErrorCode::Line4EstimationCriteria; + return false; + } + + // get intersections of four sides + intersection1 = getintersec(line1, line2); + intersection2 = getintersec(line2, line3); + intersection3 = getintersec(line3, line4); + intersection4 = getintersec(line1, line4); + + auto eigen2pcd = [](Eigen::Vector3f p){ + pcl::PointXYZ p2; + p2.x = p.x(); + p2.y = p.y(); + p2.z = p.z(); + return p2; + }; + + if (!estimateTargetSize(cluster, intersection1, intersection2, intersection3, intersection4)) { + cluster.detail_valid = LidartagErrorCode::TagSizeEstimationCriteria; + return false; + } + + return true; +} + +/* + * A function to estimate the four corners of a tag using 1x rectangle fiting with + * optional RANSAC + */ +bool LidarTag::estimateCornersUsingRectangleFitting(ClusterFamily_t & cluster, + pcl::PointCloud::Ptr & cloud1, pcl::PointCloud::Ptr & cloud2, + pcl::PointCloud::Ptr & cloud3, pcl::PointCloud::Ptr & cloud4, + Eigen::Vector3f & intersection1, Eigen::Vector3f & intersection2, + Eigen::Vector3f & intersection3, Eigen::Vector3f & intersection4) +{ + rectangle_estimator_->setInputPoints(cloud1, cloud2, cloud3, cloud4); + + if (!rectangle_estimator_->estimate()) { + cluster.detail_valid = LidartagErrorCode::RectangleEstimationCirteria; + + cloud1->width = cloud1->size(); + cloud2->width = cloud2->size(); + cloud3->width = cloud3->size(); + cloud4->width = cloud4->size(); + + cloud1->height = 1; + cloud2->height = 1; + cloud3->height = 1; + cloud4->height = 1; + + return false; + } + + std::vector corners = rectangle_estimator_->getCorners(); + assert(corners.size() == 4); + + auto corner_2d23d = [](auto & corner2d, auto & corner3d) { + corner3d.x() = corner2d.x(); + corner3d.y() = corner2d.y(); + corner3d.z() = 0.f; + }; + + corner_2d23d(corners[0], intersection1); + corner_2d23d(corners[1], intersection2); + corner_2d23d(corners[2], intersection3); + corner_2d23d(corners[3], intersection4); + + if (!estimateTargetSize(cluster, intersection1, intersection2, intersection3, intersection4)) { + cluster.detail_valid = LidartagErrorCode::TagSizeEstimationCriteria; + return false; + } + + return true; +} + /* [Edge points and principal axes] * A function to transform the edge points to the tag frame and split into 4 * groups for each group of points, do line fitting and get four corner points @@ -2191,7 +2421,7 @@ bool LidarTag::transformSplitEdges(ClusterFamily_t & cluster) pcl::toROSMsg(*before_transformed_edge_pc, before_transformed_edge_pc_msg); before_transformed_edge_pc_msg.header = point_cloud_header_; - before_transformed_edge_pc_msg.header.frame_id = pub_frame_; + before_transformed_edge_pc_msg.header.frame_id = lidar_frame_; beforetransformed_edge_pc_pub_->publish(before_transformed_edge_pc_msg); for (int i = 0; i < cluster.edge_points.size(); ++i) { @@ -2204,10 +2434,6 @@ bool LidarTag::transformSplitEdges(ClusterFamily_t & cluster) cluster.edge_points[i].point.y - cluster.average.y, cluster.edge_points[i].point.z - cluster.average.z); Eigen::Matrix transform_matrix = cluster.principal_axes; - // cluster.principal_axes << -0.866, 0.180, -0.466, -0.492, -0.130, 0.861, - // 0.095, 0.975, 0.201; - // Eigen::Matrix transform_matrix; - // transform_matrix = cluster.principal_axes; transform_matrix = (transform_matrix.transpose()).eval(); @@ -2261,93 +2487,70 @@ bool LidarTag::transformSplitEdges(ClusterFamily_t & cluster) RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); } - cluster.detail_valid = 7; + cluster.detail_valid = LidartagErrorCode::CornerEstimationMinPointsCriteria; return false; } - // To do: function this part - // get fitted line for points on the 1th side of the tag - Eigen::Vector4f line1; - Eigen::Vector4f line2; - Eigen::Vector4f line3; - Eigen::Vector4f line4; - pcl::PointCloud::Ptr line_cloud1(new pcl::PointCloud); - pcl::PointCloud::Ptr line_cloud2(new pcl::PointCloud); - pcl::PointCloud::Ptr line_cloud3(new pcl::PointCloud); - pcl::PointCloud::Ptr line_cloud4(new pcl::PointCloud); - line_cloud1->reserve(point_cloud_size_); - line_cloud1->clear(); - line_cloud2->reserve(point_cloud_size_); - line_cloud2->clear(); - line_cloud3->reserve(point_cloud_size_); - line_cloud3->clear(); - line_cloud4->reserve(point_cloud_size_); - line_cloud4->clear(); + Eigen::Vector3f intersection1, intersection2, intersection3, intersection4; - if (!LidarTag::getLines(cloud1, line1, line_cloud1)) { - if (debug_info_) { - RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); - } - - cluster.detail_valid = 8; + if (!params_.use_rectangle_model && !estimateCornersUsingLinesRANSAC( + cluster, cloud1, cloud2, cloud3, cloud4, + intersection1, intersection2, intersection3, intersection4)) + { return false; } - if (!LidarTag::getLines(cloud2, line2, line_cloud2)) { - if (debug_info_) { - RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); - } - - cluster.detail_valid = 9; + else if(params_.use_rectangle_model && !estimateCornersUsingRectangleFitting( + cluster, cloud1, cloud2, cloud3, cloud4, + intersection1, intersection2, intersection3, intersection4)) + { return false; } - if (!LidarTag::getLines(cloud3, line3, line_cloud3)) { - if (debug_info_) { - RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); - } - cluster.detail_valid = 10; - return false; - } - if (!LidarTag::getLines(cloud4, line4, line_cloud4)) { - if (debug_info_) { - RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); - } - cluster.detail_valid = 11; - return false; - } + // The data from these functions is only to visualize the new groups + auto cloud_to_edge_group = [&]( + pcl::PointCloud & cloud, + pcl::PointCloud & edges, + int intensity) + { + edges.clear(); - // get intersections of four sides - Eigen::Vector3f intersection1 = getintersec(line1, line2); - Eigen::Vector3f intersection2 = getintersec(line2, line3); - Eigen::Vector3f intersection3 = getintersec(line3, line4); - Eigen::Vector3f intersection4 = getintersec(line1, line4); + LidarPoints_t group_point = edges[0]; + group_point.point.intensity = intensity; + group_point.valid = 1; - Eigen::MatrixXf intersection_matrix; - intersection_matrix.col(0) = intersection1; - intersection_matrix.col(1) = intersection2; - intersection_matrix.col(2) = intersection3; - intersection_matrix.col(3) = intersection4; + edges.clear(); + Eigen::Vector3f translation = Eigen::Vector3f(cluster.average.x, cluster.average.y, cluster.average.z); - if (!estimateTargetSize(cluster, intersection1, intersection2, intersection3, intersection4)) { - cluster.detail_valid = 12; - return false; + for (int i = 0; i < cloud.size(); ++i) { + Eigen::Vector3f point(cloud[i].x, cloud[i].y, cloud[i].z); + point = cluster.principal_axes*point + translation; + group_point.point.x = point.x(); + group_point.point.y = point.y(); + group_point.point.z = point.z(); + group_point.point.intensity = 0; + edges.push_back(group_point); + } + }; + + if(params_.rectangle_fix_point_groups) { + cloud_to_edge_group(*cloud1, cluster.edge_group1, 0); + cloud_to_edge_group(*cloud2, cluster.edge_group2, 85); + cloud_to_edge_group(*cloud3, cluster.edge_group3, 170); + cloud_to_edge_group(*cloud4, cluster.edge_group4, 255); } + sensor_msgs::msg::PointCloud2 transformed_edge_pc_msg; pcl::toROSMsg(*transformed_edge_pc, transformed_edge_pc_msg); transformed_edge_pc_msg.header = point_cloud_header_; - transformed_edge_pc_msg.header.frame_id = pub_frame_; + transformed_edge_pc_msg.header.frame_id = lidar_frame_; transformed_edge_pc_pub_->publish(transformed_edge_pc_msg); std::vector intersection_list{ intersection1, intersection2, intersection3, intersection4}; publishIntersections(intersection_list); - //intersection1_ = intersection1; deprecated - //intersection2_ = intersection2; deprecated - //intersection3_ = intersection3; deprecated - //intersection4_ = intersection4; deprecated // associate four intersections with four coners of the template Eigen::MatrixXf payload_vertices(3, 4); @@ -2355,58 +2558,23 @@ bool LidarTag::transformSplitEdges(ClusterFamily_t & cluster) payload_vertices.col(1) = cluster.principal_axes * intersection2; payload_vertices.col(2) = cluster.principal_axes * intersection3; payload_vertices.col(3) = cluster.principal_axes * intersection4; - // payload_vertices << -0.0151639, -0.629135, 0.0127609, 0.624599, - // -0.178287, - // -0.325841, 0.167202, 0.319495, 0.728342, -0.0662743, -0.686448, - // 0.0829155; Eigen::MatrixXf ordered_payload_vertices = getOrderedCorners(payload_vertices, cluster); Eigen::MatrixXf vertices = Eigen::MatrixXf::Zero(3, 5); - // ordered_payload_vertices << -0.0151639, -0.629135, 0.0127609, 0.624599, - // -0.178287, -0.325841, 0.167202, 0.319495, 0.728342, -0.0662743, - // -0.686448, 0.0829155; - // Vertices << 0, 0, 0, 0, 0, - // 0, 0.515, 0.515, -0.515, -0.515, - // 0, 0.515, -0.515, -0.515, 0.515; - utils::formGrid(vertices, 0, 0, 0, cluster.tag_size); + // This re estimated the rotation since the principal axis are only an estimation ! + // Using the rectangle model over line ransac can provide better results Eigen::Matrix3f R; std::vector mats; mats = utils::fitGridNew(vertices, R, ordered_payload_vertices); - //U_ = mats.front(); deprecated - //mats.erase(mats.begin()); - // V_ = mats.front(); deprecated - //mats.erase(mats.begin()); - // r_ = mats.front(); deprecated - //mats.erase(mats.begin()); - //mats.clear(); - // R << -0.467, 0.861, 0.201 - // , -0.632, -0.484, 0.606 - // , 0.619, 0.156, 0.767; - //payload_vertices_ = ordered_payload_vertices; deprecated - //vertices_ = vertices; // deprecated - //ordered_payload_vertices_ = ordered_payload_vertices; deprecated - //R_ = R; deprecated + // used for visualization for corner points PointXYZRI showpoint; PointXYZRI showpoint_tag; - Eigen::MatrixXf::Index col; - - //ordered_payload_vertices.row(1).minCoeff(&col); deprecated - //utils::eigen2Corners(ordered_payload_vertices.col(col), cluster.tag_corners.right); - - //ordered_payload_vertices.row(1).maxCoeff(&col); - //utils::eigen2Corners(ordered_payload_vertices.col(col), cluster.tag_corners.left); + PointXYZRI refined_center = cluster.average; - //ordered_payload_vertices.row(2).minCoeff(&col); - //utils::eigen2Corners(ordered_payload_vertices.col(col), cluster.tag_corners.down); - - //ordered_payload_vertices.row(2).maxCoeff(&col); - //utils::eigen2Corners(ordered_payload_vertices.col(col), cluster.tag_corners.top); - - point p_corner; for (int i = 0; i < 4; ++i) { showpoint.intensity = 50; @@ -2414,25 +2582,31 @@ bool LidarTag::transformSplitEdges(ClusterFamily_t & cluster) showpoint.y = ordered_payload_vertices.col(i)(1); showpoint.z = ordered_payload_vertices.col(i)(2); - p_corner.x = ordered_payload_vertices.col(i)(0); - p_corner.y = ordered_payload_vertices.col(i)(1); - p_corner.z = ordered_payload_vertices.col(i)(2); + refined_center.x += 0.25f * showpoint.x; + refined_center.y += 0.25f * showpoint.y; + refined_center.z += 0.25f * showpoint.z; showpoint_tag.x = showpoint.x + cluster.average.x; showpoint_tag.y = showpoint.y + cluster.average.y; showpoint_tag.z = showpoint.z + cluster.average.z; transformed_pc->push_back(showpoint); transformed_pc_tag->push_back(showpoint_tag); - - cluster.corner_offset_array.push_back(p_corner); } - LidarTag::publishPointcloud(transformed_pc, pub_frame_, string("transpts")); - LidarTag::publishPointcloud(transformed_pc_tag, pub_frame_, string("transptstag")); + cluster.initial_corners = *transformed_pc_tag; + LidarTag::publishPointcloud(transformed_pc, lidar_frame_, string("transpts")); + LidarTag::publishPointcloud(transformed_pc_tag, lidar_frame_, string("transptstag")); // save initial lidar to tag pose matrix cluster.initial_pose.rotation = R; - cluster.initial_pose.translation << -cluster.average.x, -cluster.average.y, -cluster.average.z; + + if (params_.refine_cluster_with_intersections) { + cluster.initial_pose.translation << -refined_center.x, -refined_center.y, -refined_center.z; + } + else { + cluster.initial_pose.translation << -cluster.average.x, -cluster.average.y, -cluster.average.z; + } + cluster.initial_pose.translation = R * cluster.initial_pose.translation; Eigen::Vector3f euler_angles = cluster.initial_pose.rotation.eulerAngles(0, 1, 2); cluster.initial_pose.roll = euler_angles[0]; @@ -2471,22 +2645,6 @@ bool LidarTag::getLines( seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); - pcl::ModelCoefficients::Ptr coefficients_debug(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers_debug(new pcl::PointIndices); - pcl::SACSegmentation seg_debug; - pcl::ExtractIndices extract_debug; - - seg_debug.setOptimizeCoefficients(true); - seg_debug.setModelType(pcl::SACMODEL_LINE); - seg_debug.setMethodType(pcl::SAC_RANSAC); - seg_debug.setDistanceThreshold(0.02); // 0.015 ok - seg_debug.setMaxIterations(500); - - seg_debug.setInputCloud(cloud); - seg_debug.segment(*inliers_debug, *coefficients_debug); - - - if (debug_info_) { RCLCPP_DEBUG_STREAM(get_logger(), "Inliers size: " << inliers->indices.size()); } @@ -2506,14 +2664,6 @@ bool LidarTag::getLines( line << coefficients->values[0], coefficients->values[1], coefficients->values[3], coefficients->values[4]; - if (std::abs(line.x()) > 2.f) { - int x = 0; - } - - if (std::abs(coefficients_debug->values[0]) > 2.f) { - int x = 0; - } - return true; } @@ -2714,7 +2864,7 @@ void LidarTag::assignLine( const PointXYZRI point1, const PointXYZRI point2, const int count) { - marker.header.frame_id = pub_frame_; + marker.header.frame_id = lidar_frame_; marker.header.stamp = current_scan_time_; // marker.ns = string("Boundary_") + to_string(cluster.cluster_id); marker.ns = ns; @@ -3073,20 +3223,8 @@ void LidarTag::detectionArrayPublisher( lidartag_msgs::msg::LidarTagDetection detection; detection.header = point_cloud_header_; - // TODO: here we can only assign the id and tag size according to the - // distance. - // That only applies for certain calibration scene configuration - // imperical threshold for the static scene - // double distance_threshold = 7.0; - // std::cout << "Lidartag target depth: " << cluster.average.x << std::endl; - if (calibration_) { - if (cluster.average.x > distance_threshold_) - detection.id = 1; - else - detection.id = 3; - } - - detection.size = detection.id == 1 ? 1.215 : 0.8; // TODO: do not hardcode this... + detection.id = cluster.cluster_id; + detection.size = cluster.tag_size; pcl::PointCloud::Ptr clusterPC(new pcl::PointCloud); for (int i = 0; i < cluster.data.size(); ++i) { @@ -3217,7 +3355,7 @@ void LidarTag::publishLidartagCluster(const vector & cluster_bu output_ep_pc->points.resize(cluster_ep_pc->points.size()); output_tr_ep_pc->points.resize(cluster_tr_ep_pc->points.size()); point.header.stamp = clock_->now(); - point.header.frame_id = pub_frame_; + point.header.frame_id = lidar_frame_; point.point.x = cluster_buff[index_num].average.x; point.point.y = cluster_buff[index_num].average.y; point.point.z = cluster_buff[index_num].average.z; @@ -3247,9 +3385,9 @@ void LidarTag::publishLidartagCluster(const vector & cluster_bu pcl::toROSMsg(*output_pc, output_data_msg); pcl::toROSMsg(*output_ep_pc, output_ep_msg); pcl::toROSMsg(*output_tr_ep_pc, output_tr_ep_msg); - output_data_msg.header.frame_id = pub_frame_; - output_ep_msg.header.frame_id = pub_frame_; - output_tr_ep_msg.header.frame_id = pub_frame_; + output_data_msg.header.frame_id = lidar_frame_; + output_ep_msg.header.frame_id = lidar_frame_; + output_tr_ep_msg.header.frame_id = lidar_frame_; output_data_msg.header = point_cloud_header_; output_ep_msg.header = point_cloud_header_; output_tr_ep_msg.header = point_cloud_header_; @@ -3273,16 +3411,15 @@ void LidarTag::publishIntersections(const std::vector intersect visualization_msgs::msg::MarkerArray intersection_marker_array; intersection_marker_array.markers.resize(4); int index = 0; - rclcpp::Duration d(3.0); for (auto intersection : intersection_list) { - intersection_marker_array.markers[index].header.frame_id = pub_frame_; + intersection_marker_array.markers[index].header.frame_id = lidar_frame_; intersection_marker_array.markers[index].header.stamp = clock_->now(); intersection_marker_array.markers[index].ns = "intersection_marker"; intersection_marker_array.markers[index].id = index; intersection_marker_array.markers[index].type = visualization_msgs::msg::Marker::CUBE; intersection_marker_array.markers[index].action = visualization_msgs::msg::Marker::ADD; - intersection_marker_array.markers[index].lifetime = d; + intersection_marker_array.markers[index].lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); intersection_marker_array.markers[index].scale.x = 0.1; intersection_marker_array.markers[index].scale.y = 0.1; intersection_marker_array.markers[index].scale.z = 0.1; @@ -3343,4 +3480,47 @@ void LidarTag::printClusterResult(const std::vector & cluster_b fresult.close(); fresult << std::endl; } + +void LidarTag::addOrderedPointcloudMarkers(std::vector> & ordered_buff) +{ + for(int ring_id = 0; ring_id < ordered_buff.size(); ring_id++) { + + if(ring_id != params_.debug_ring_id) + continue; + + for(int index = 0; index < ordered_buff[ring_id].size(); index++) { + + auto & point = ordered_buff[ring_id][index]; + + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = lidar_frame_; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.03; + marker.scale.y = 0.03; + marker.scale.z = 0.03; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + + marker.header.stamp = clock_->now(); + marker.id = index; + marker.text = to_string(index); + marker.ns = "ring_" + to_string(ring_id); + marker.pose.position.x = point.point.x; + marker.pose.position.y = point.point.y; + marker.pose.position.z = point.point.z; + ordered_pointcloud_markers_.markers.push_back(marker); + + } + } +} + }; // namespace BipedLab diff --git a/src/lidartag_cluster.cpp b/src/lidartag_cluster.cpp index 304d5b5..8e206a9 100644 --- a/src/lidartag_cluster.cpp +++ b/src/lidartag_cluster.cpp @@ -28,8 +28,8 @@ * WEBSITE: https://www.brucerobot.com/ */ -#include "lidartag.h" -#include "ultra_puck.h" +#include +#include #include @@ -114,6 +114,14 @@ void LidarTag::clusterClassifier( //cout << "\033[1;31m============== \033[0m\n"; //cout << "cluster_buff size: " << cluster_buff.size() << endl; for (int i = 0; i < cluster_buff.size(); ++i) { + + /*if (cluster_buff[i].cluster_id == params_.debug_cluster_id && + point.index == params_.debug_scan_id && + debug_current_ring == params_.debug_ring_id) + { + int x = 0; + } */ + updateCluster(point, cluster_buff[i], new_cluster); if (!(new_cluster->flag)) { @@ -197,6 +205,13 @@ void LidarTag::updateCluster( // This point is inside this cluster if (!new_cluster->flag) { + + /*if (old_cluster.cluster_id == params_.debug_cluster_id && + point.point.ring == params_.debug_ring_id - 1) + { + int x = 0; + } */ + // update the boundary of the old cluster if (point.point.ring < old_cluster.bottom_ring) { old_cluster.bottom_ring = point.point.ring; @@ -265,4 +280,169 @@ bool LidarTag::isWithinClusterHorizon( (cluster.right_most_point.y - params_.linkage_threshold < point.point.y); } + void LidarTag::fixClusters( + const std::vector> & edge_buff, + std::vector & cluster_buff) + { + for(ClusterFamily_t & cluster : cluster_buff) + { + for(int ring_id = 0; ring_id < cluster.max_min_index_of_each_ring.size(); ring_id++) { + + /*if (cluster.cluster_id == params_.debug_cluster_id && + ring_id == params_.debug_ring_id) + { + int x = 0; + } */ + + const auto minmax = cluster.max_min_index_of_each_ring[ring_id]; + + if (minmax.min != minmax.max) { + continue; + } + + // find the id KL: this needs to be refactored and we need to put a binary search or something faster ! + const std::vector & ring_edges = edge_buff[ring_id]; + int edge_id = -1; + for (int j = 0; j < ring_edges.size(); j++) { + if (minmax.min == ring_edges[j].index) { + edge_id = j; + } + } + + assert(edge_id >= 0); + const auto & point_aux = ring_edges[edge_id]; + + for (int j = edge_id + 1; j < ring_edges.size(); j++) { + // check if it is outside or not from the + const LidarPoints_t & point = ring_edges[j]; + + if (point.index == point_aux.index) { + continue; + } + else if (!isWithinClusterHorizon(point, cluster, params_.linkage_threshold) || + (point.point.getVector3fMap() - point_aux.point.getVector3fMap()).norm() > 1.5) + { + break; + } + + // update the boundary of the old cluster + if (point.point.ring < cluster.bottom_ring) { + cluster.bottom_ring = point.point.ring; + } + if (point.point.ring > cluster.top_ring) { + cluster.top_ring = point.point.ring; + } + if (point.point.x + params_.linkage_threshold > cluster.front_most_point.x) { + cluster.front_most_point = point.point; + cluster.front_most_point.x += params_.linkage_threshold; + } + if (point.point.x - params_.linkage_threshold < cluster.back_most_point.x) { + cluster.back_most_point = point.point; + cluster.back_most_point.x -= params_.linkage_threshold; + } + + if (point.point.y + params_.linkage_threshold > cluster.left_most_point.y) { + cluster.left_most_point = point.point; + cluster.left_most_point.y += params_.linkage_threshold; + } + if (point.point.y - params_.linkage_threshold < cluster.right_most_point.y) { + cluster.right_most_point = point.point; + cluster.right_most_point.y -= params_.linkage_threshold; + } + + if (point.point.z + params_.linkage_threshold > cluster.top_most_point.z) { + cluster.top_most_point = point.point; + cluster.top_most_point.z += params_.linkage_threshold; + } + if (point.point.z - params_.linkage_threshold < cluster.bottom_most_point.z) { + cluster.bottom_most_point = point.point; + cluster.bottom_most_point.z -= params_.linkage_threshold; + } + + // update the max/min index of each ring in this cluster + if (cluster.max_min_index_of_each_ring[point.point.ring].max < point.index) + cluster.max_min_index_of_each_ring[point.point.ring].max = point.index; + + if (cluster.max_min_index_of_each_ring[point.point.ring].min > point.index) + cluster.max_min_index_of_each_ring[point.point.ring].min = point.index; + + // update the max/min intensity of this cluster + if (cluster.max_intensity.intensity < point.point.intensity) + cluster.max_intensity = point.point; + + if (cluster.min_intensity.intensity > point.point.intensity) + cluster.min_intensity = point.point; + + cluster.edge_points.push_back(point); + } + + for (int j = edge_id - 1; j >= 0; j--) { + // check if it is outside or not from the + const LidarPoints_t & point = ring_edges[j]; + + if (point.index == point_aux.index) { + continue; + } + else if (!isWithinClusterHorizon(point, cluster, params_.linkage_threshold) || + (point.point.getVector3fMap() - point_aux.point.getVector3fMap()).norm() > 1.5) + { + break; + } + + // update the boundary of the old cluster + if (point.point.ring < cluster.bottom_ring) { + cluster.bottom_ring = point.point.ring; + } + if (point.point.ring > cluster.top_ring) { + cluster.top_ring = point.point.ring; + } + if (point.point.x + params_.linkage_threshold > cluster.front_most_point.x) { + cluster.front_most_point = point.point; + cluster.front_most_point.x += params_.linkage_threshold; + } + if (point.point.x - params_.linkage_threshold < cluster.back_most_point.x) { + cluster.back_most_point = point.point; + cluster.back_most_point.x -= params_.linkage_threshold; + } + + if (point.point.y + params_.linkage_threshold > cluster.left_most_point.y) { + cluster.left_most_point = point.point; + cluster.left_most_point.y += params_.linkage_threshold; + } + if (point.point.y - params_.linkage_threshold < cluster.right_most_point.y) { + cluster.right_most_point = point.point; + cluster.right_most_point.y -= params_.linkage_threshold; + } + + if (point.point.z + params_.linkage_threshold > cluster.top_most_point.z) { + cluster.top_most_point = point.point; + cluster.top_most_point.z += params_.linkage_threshold; + } + if (point.point.z - params_.linkage_threshold < cluster.bottom_most_point.z) { + cluster.bottom_most_point = point.point; + cluster.bottom_most_point.z -= params_.linkage_threshold; + } + + // update the max/min index of each ring in this cluster + if (cluster.max_min_index_of_each_ring[point.point.ring].max < point.index) + cluster.max_min_index_of_each_ring[point.point.ring].max = point.index; + + if (cluster.max_min_index_of_each_ring[point.point.ring].min > point.index) + cluster.max_min_index_of_each_ring[point.point.ring].min = point.index; + + // update the max/min intensity of this cluster + if (cluster.max_intensity.intensity < point.point.intensity) + cluster.max_intensity = point.point; + + if (cluster.min_intensity.intensity > point.point.intensity) + cluster.min_intensity = point.point; + + cluster.edge_points.push_back(point); + } + + } + } + + } + } // namespace BipedLab diff --git a/src/lidartag_decode.cpp b/src/lidartag_decode.cpp index 567bd94..4e92bb4 100644 --- a/src/lidartag_decode.cpp +++ b/src/lidartag_decode.cpp @@ -29,11 +29,11 @@ * WEBSITE: https://www.brucerobot.com/ */ -#include "lidartag.h" -#include "apriltag_utils.h" -#include "tag16h5.h" -#include "tag49h14.h" -#include "utils.h" +#include +#include +#include +#include +#include #include #include @@ -204,7 +204,7 @@ int LidarTag::getCodeWeightedGaussian( visualization_msgs::msg::Marker grid_marker; visualization_msgs::msg::Marker line_strip; - line_strip.header.frame_id = pub_frame_; + line_strip.header.frame_id = lidar_frame_; line_strip.header.stamp = current_scan_time_; line_strip.ns = "boundary"; line_strip.action = visualization_msgs::msg::Marker::ADD; @@ -1651,7 +1651,7 @@ int LidarTag::getCodeRKHS(RKHSDecoding_t & rkhs_decoding, const double & tag_siz // } // utils::printVector(utils::convertEigenToSTDVector(rkhs_decoding.score)); - if (id_score < 12) { + if (id_score < params_.min_rkhs_score) { status = 0; if (debug_info_) { RCLCPP_WARN_STREAM(get_logger(), "==== getCodeRKHS ===="); @@ -1716,6 +1716,7 @@ bool LidarTag::decodePayload(ClusterFamily_t & cluster) } else { valid_tag = false; cluster.valid = 0; + cluster.cluster_id = -1; result_statistics_.cluster_removal.decoding_failure++; } } diff --git a/src/lidartag_pose.cpp b/src/lidartag_pose.cpp index edae557..0aad151 100644 --- a/src/lidartag_pose.cpp +++ b/src/lidartag_pose.cpp @@ -1,7 +1,7 @@ -#include -#include "lidartag.h" -#include "utils.h" +#include +#include +#include #include using namespace std; diff --git a/src/lidartag_prune.cpp b/src/lidartag_prune.cpp index 3524136..04b56aa 100644 --- a/src/lidartag_prune.cpp +++ b/src/lidartag_prune.cpp @@ -29,8 +29,8 @@ * WEBSITE: https://www.brucerobot.com/ */ -#include "lidartag.h" -#include "ultra_puck.h" +#include +#include #include #include @@ -143,15 +143,18 @@ int LidarTag::maxPointsCheck(ClusterFamily_t & cluster) if (mark_cluster_validity_) { cluster.valid = false; - cluster.detail_valid = 2; + cluster.detail_valid = LidartagErrorCode::ClusterMinPointsCriteria2; } } if (debug_info_) { RCLCPP_DEBUG_STREAM(get_logger(), "==== maxPointsCheck ===="); - RCLCPP_DEBUG_STREAM(get_logger(), "Distance : " << distance << ", num_horizontal_points: " << num_horizontal_points); + RCLCPP_DEBUG_STREAM(get_logger(), "Distance : " << distance << ", num_horizontal_points: " + << num_horizontal_points); RCLCPP_DEBUG_STREAM(get_logger(), "Expected Points: " << expected_points); - RCLCPP_DEBUG_STREAM(get_logger(), "Actual Points: " << cluster.data.size() + cluster.edge_points.size()); + RCLCPP_DEBUG_STREAM(get_logger(), "Actual Points: " + << cluster.data.size() + cluster.edge_points.size()); + if ((cluster.data.size() + cluster.edge_points.size()) > expected_points) RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); else @@ -202,15 +205,19 @@ bool LidarTag::rejectWithPlanarCheck( float distance = std::sqrt(pow(cluster.average.x, 2) + pow(cluster.average.y, 2) + pow(cluster.average.z, 2)); RCLCPP_DEBUG_STREAM(get_logger(), "Distance : " << distance); - RCLCPP_DEBUG_STREAM(get_logger(), "Actual Points: " << cluster.data.size() + cluster.edge_points.size()); + RCLCPP_DEBUG_STREAM(get_logger(), "Actual Points: " + << cluster.data.size() + cluster.edge_points.size()); RCLCPP_DEBUG_STREAM(get_logger(), "Inliers : " << inliers->indices.size()); - RCLCPP_DEBUG_STREAM(get_logger(), "Outliers : " << cluster.data.size() - inliers->indices.size()); + RCLCPP_DEBUG_STREAM(get_logger(), "Outliers : " + << cluster.data.size() - inliers->indices.size()); } if (inliers->indices.size() == 0) { if (debug_info_) { RCLCPP_DEBUG_STREAM(get_logger(), "Status: " << false); - RCLCPP_WARN_STREAM(get_logger(), "Failed to fit a plane model to the cluster."); + RCLCPP_WARN_STREAM(get_logger(), "Failed to fit a plane model to the cluster id=" + << cluster.cluster_id); } + result_statistics_.cluster_removal.plane_fitting++; result_statistics_.remaining_cluster_size--; diff --git a/src/lidartag_rviz.cpp b/src/lidartag_rviz.cpp index 1ad0e2d..418156f 100644 --- a/src/lidartag_rviz.cpp +++ b/src/lidartag_rviz.cpp @@ -29,7 +29,7 @@ * WEBSITE: https://www.brucerobot.com/ */ -#include "lidartag.h" +#include #include @@ -39,6 +39,7 @@ #define SQRT2 1.41421356237 using namespace std; +using namespace std::chrono_literals; namespace BipedLab { @@ -50,7 +51,7 @@ void LidarTag::assignMarker( const double r, const double g, const double b, const PointXYZRI & point, const int count, const double size, const string text) { - marker.header.frame_id = pub_frame_; + marker.header.frame_id = lidar_frame_; marker.header.stamp = current_scan_time_; marker.ns = name_space; marker.id = count; @@ -65,7 +66,7 @@ void LidarTag::assignMarker( marker.pose.orientation.w = 1.0; marker.text = text; // should disappear along with updateing rate - marker.lifetime = marker.lifetime = rclcpp::Duration(sleep_time_for_vis_ * 10); + marker.lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); // Set the scale of the marker -- 1x1x1 here means 1m on a side marker.scale.x = size; @@ -85,7 +86,7 @@ void LidarTag::assignVectorMarker( const PointXYZRI & centroid, const string text) { const double tag_depth = 0.02; - marker.header.frame_id = pub_frame_; + marker.header.frame_id = lidar_frame_; marker.header.stamp = current_scan_time_; marker.ns = name_space; marker.id = count; @@ -94,7 +95,7 @@ void LidarTag::assignVectorMarker( marker.text = text; // should disappear along with updateing rate - marker.lifetime = marker.lifetime = rclcpp::Duration(sleep_time_for_vis_); + marker.lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); // Set the scale of the marker -- 1x1x1 here means 1m on a side geometry_msgs::msg::Point p1; p1.x = 0; // centroid.x; @@ -173,6 +174,7 @@ void LidarTag::plotTagFrame(const ClusterFamily_t & cluster) line_list.color.b = 1.0; line_list.color.a = 1.0; line_list.scale.x = 0.01; + line_list.lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); vector> vertex = {{1, 1}, {1, -1}, {-1, -1}, {-1, 1}}; const double tag_depth = 0.02; @@ -252,13 +254,13 @@ visualization_msgs::msg::Marker LidarTag::visualizeVector( * A function to prepare for displaying results in rviz */ void LidarTag::clusterToPclVectorAndMarkerPublisher( - std::vector & cluster, pcl::PointCloud::Ptr out_cluster, + std::vector & clusters, pcl::PointCloud::Ptr out_cluster, pcl::PointCloud::Ptr out_edge_cluster, pcl::PointCloud::Ptr out_payload, pcl::PointCloud::Ptr out_payload_3d, pcl::PointCloud::Ptr out_target, pcl::PointCloud::Ptr out_initial_target, pcl::PointCloud::Ptr edge_group_1, pcl::PointCloud::Ptr edge_group_2, pcl::PointCloud::Ptr edge_group_3, pcl::PointCloud::Ptr edge_group_4, pcl::PointCloud::Ptr boundary_pts, - visualization_msgs::msg::MarkerArray & cluster_array) + pcl::PointCloud::Ptr initial_corners, visualization_msgs::msg::MarkerArray & cluster_array) { const double tag_depth = 0.02; /* initialize random seed for coloring the marker*/ @@ -285,12 +287,47 @@ void LidarTag::clusterToPclVectorAndMarkerPublisher( int points_in_clusters = 0; int clustercount = 0; - for (int key = 0; key < cluster.size(); ++key) { - if (cluster[key].valid != 1) { + for (int key = 0; key < clusters.size(); ++key) { + ClusterFamily_t & cluster = clusters[key]; + if (cluster.valid != 1 && static_cast(cluster.detail_valid) < static_cast(LidartagErrorCode::Line1EstimationCriteria)){ continue; } - LidarTag::plotTagFrame(cluster[key]); + if (mark_cluster_validity_) { + for (int i = 0; i < cluster.edge_points.size(); ++i) { + if (cluster.edge_points[i].valid != 1) { + continue; + } + + out_edge_cluster->push_back(cluster.edge_points[i].point); + } + + for (int i = 0; i < cluster.edge_group1.size(); ++i) { + edge_group_1->push_back(cluster.edge_group1[i].point); + } + + for (int i = 0; i < cluster.edge_group2.size(); ++i) { + edge_group_2->push_back(cluster.edge_group2[i].point); + } + + for (int i = 0; i < cluster.edge_group3.size(); ++i) { + edge_group_3->push_back(cluster.edge_group3[i].point); + } + + for (int i = 0; i < cluster.edge_group4.size(); ++i) { + edge_group_4->push_back(cluster.edge_group4[i].point); + } + } + + if (cluster.valid != 1 && cluster.detail_valid != LidartagErrorCode::DecodingErrorCriteria){ + continue; + } + + LidarTag::plotTagFrame(cluster); + + if (cluster.valid != 1){ + continue; + } visualization_msgs::msg::Marker marker; visualization_msgs::msg::Marker boundary_marker; @@ -303,82 +340,82 @@ void LidarTag::clusterToPclVectorAndMarkerPublisher( // Draw boundary marker of each cluster LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::CUBE, - "Boundary" + to_string(cluster[key].cluster_id), r, g, b, cluster[key].top_most_point, 0, + "Boundary" + to_string(cluster.cluster_id), r, g, b, cluster.top_most_point, 0, tag_depth); bound_mark_array.markers.push_back(boundary_marker); LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::CUBE, - "Boundary" + to_string(cluster[key].cluster_id), r, g, b, cluster[key].bottom_most_point, 1, + "Boundary" + to_string(cluster.cluster_id), r, g, b, cluster.bottom_most_point, 1, tag_depth); bound_mark_array.markers.push_back(boundary_marker); LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::CUBE, - "Boundary" + to_string(cluster[key].cluster_id), r, g, b, cluster[key].front_most_point, 2, + "Boundary" + to_string(cluster.cluster_id), r, g, b, cluster.front_most_point, 2, tag_depth); bound_mark_array.markers.push_back(boundary_marker); LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::CUBE, - "Boundary" + to_string(cluster[key].cluster_id), r, g, b, cluster[key].back_most_point, 3, + "Boundary" + to_string(cluster.cluster_id), r, g, b, cluster.back_most_point, 3, tag_depth); bound_mark_array.markers.push_back(boundary_marker); LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::CUBE, - "Boundary" + to_string(cluster[key].cluster_id), r, g, b, cluster[key].right_most_point, 4, + "Boundary" + to_string(cluster.cluster_id), r, g, b, cluster.right_most_point, 4, tag_depth); bound_mark_array.markers.push_back(boundary_marker); LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::CUBE, - "Boundary" + to_string(cluster[key].cluster_id), r, g, b, cluster[key].left_most_point, 5, + "Boundary" + to_string(cluster.cluster_id), r, g, b, cluster.left_most_point, 5, tag_depth); bound_mark_array.markers.push_back(boundary_marker); // Display cluster information // how many points are supposed to be on the this tag - // int AvePoints = LidarTag::areaPoints(cluster[key].average.x, + // int AvePoints = LidarTag::areaPoints(cluster.average.x, // payload_size_, payload_size_); LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::SPHERE, - "AveragePoint" + to_string(cluster[key].cluster_id), 1, 0, 0, cluster[key].average, 1, 0.05); + "AveragePoint" + to_string(cluster.cluster_id), 1, 0, 0, cluster.average, 1, 0.05); bound_mark_array.markers.push_back(boundary_marker); - // int SupposedPoints = LidarTag::areaPoints(cluster[key].average.x, + // int SupposedPoints = LidarTag::areaPoints(cluster.average.x, // payload_size_, payload_size_); LidarTag::assignMarker( boundary_marker, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - "Text" + to_string(cluster[key].cluster_id), 1, 1, 1, cluster[key].average, 1, 0.05, + "Text" + to_string(cluster.cluster_id), 1, 1, 1, cluster.average, 1, 0.05, string( - to_string(cluster[key].cluster_id) + ", " + "\nAt: " + to_string(cluster[key].average.x) + - ", " + to_string(cluster[key].average.y) + ", " + to_string(cluster[key].average.z) + - "\nNormal vector: " + to_string(cluster[key].normal_vector(0)) + ", " + - to_string(cluster[key].normal_vector(1)) + ", " + to_string(cluster[key].normal_vector(2)) + - "\nActual points: " + to_string(cluster[key].data.size()) + ", " + - "\nNumber of inliers: " + to_string(cluster[key].inliers) + ", " + - "\nPercentages of inliers: " + to_string(cluster[key].percentages_inliers) + ", " + - "\nBoundary points: " + to_string(cluster[key].boundary_pts) + ", " + - "\nBoundary rings: " + to_string(cluster[key].boundary_rings) + ", " + - "\nPayload points: " + to_string(cluster[key].payload_without_boundary) + ", " + - "\nPose_xyz: " + to_string(cluster[key].pose_tag_to_lidar.translation[0]) + ", " + - to_string(cluster[key].pose_tag_to_lidar.translation[1]) + ", " + - to_string(cluster[key].pose_tag_to_lidar.translation[2]) + - "\nPose_rpy: " + to_string(cluster[key].pose_tag_to_lidar.roll) + ", " + - to_string(cluster[key].pose_tag_to_lidar.pitch) + ", " + - to_string(cluster[key].pose_tag_to_lidar.yaw) + - "\nIntensity: " + to_string(cluster[key].max_intensity.intensity) + ", " + - to_string(cluster[key].min_intensity.intensity))); + to_string(cluster.cluster_id) + ", " + "\nAt: " + to_string(cluster.average.x) + + ", " + to_string(cluster.average.y) + ", " + to_string(cluster.average.z) + + "\nNormal vector: " + to_string(cluster.normal_vector(0)) + ", " + + to_string(cluster.normal_vector(1)) + ", " + to_string(cluster.normal_vector(2)) + + "\nActual points: " + to_string(cluster.data.size()) + ", " + + "\nNumber of inliers: " + to_string(cluster.inliers) + ", " + + "\nPercentages of inliers: " + to_string(cluster.percentages_inliers) + ", " + + "\nBoundary points: " + to_string(cluster.boundary_pts) + ", " + + "\nBoundary rings: " + to_string(cluster.boundary_rings) + ", " + + "\nPayload points: " + to_string(cluster.payload_without_boundary) + ", " + + "\nPose_xyz: " + to_string(cluster.pose_tag_to_lidar.translation[0]) + ", " + + to_string(cluster.pose_tag_to_lidar.translation[1]) + ", " + + to_string(cluster.pose_tag_to_lidar.translation[2]) + + "\nPose_rpy: " + to_string(cluster.pose_tag_to_lidar.roll) + ", " + + to_string(cluster.pose_tag_to_lidar.pitch) + ", " + + to_string(cluster.pose_tag_to_lidar.yaw) + + "\nIntensity: " + to_string(cluster.max_intensity.intensity) + ", " + + to_string(cluster.min_intensity.intensity))); bound_mark_array.markers.push_back(boundary_marker); LidarTag::assignVectorMarker( boundary_marker, visualization_msgs::msg::Marker::ARROW, - "NormalVector_z" + to_string(cluster[key].cluster_id), 0, 0, 1, 2, 0.01, - cluster[key].principal_axes.col(2), cluster[key].average); + "NormalVector_z" + to_string(cluster.cluster_id), 0, 0, 1, 2, 0.01, + cluster.principal_axes.col(2), cluster.average); bound_mark_array.markers.push_back(boundary_marker); if (id_decoding_) { visualization_msgs::msg::Marker id_marker; LidarTag::assignMarker( id_marker, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - "Text" + to_string(cluster[key].cluster_id), 1, 1, 1, cluster[key].average, 1, - cluster[key].tag_size * 0.7, string(to_string(cluster[key].rkhs_decoding.id))); + "Text" + to_string(cluster.cluster_id), 1, 1, 1, cluster.average, 1, + cluster.tag_size * 0.7, string(to_string(cluster.rkhs_decoding.id))); id_mark_array.markers.push_back(id_marker); } // Draw payload boundary marker @@ -386,54 +423,57 @@ void LidarTag::clusterToPclVectorAndMarkerPublisher( if (adaptive_thresholding_) { // Upper boundary - for (int i = 0; i < cluster[key].tag_edges.upper_line.size(); ++i) { + for (int i = 0; i < cluster.tag_edges.upper_line.size(); ++i) { LidarTag::assignMarker( payload_marker, visualization_msgs::msg::Marker::SPHERE, - "PayloadUpperBoundary_" + to_string(cluster[key].cluster_id), 0, 0, 1, - cluster[key].tag_edges.upper_line[i]->point, i, 0.015); + "PayloadUpperBoundary_" + to_string(cluster.cluster_id), 0, 0, 1, + cluster.tag_edges.upper_line[i]->point, i, 0.015); payload_mark_array.markers.push_back(payload_marker); } // Lower boundary - for (int i = 0; i < cluster[key].tag_edges.lower_line.size(); ++i) { + for (int i = 0; i < cluster.tag_edges.lower_line.size(); ++i) { LidarTag::assignMarker( payload_marker, visualization_msgs::msg::Marker::SPHERE, - "PayloadLowerBoundary_" + to_string(cluster[key].cluster_id), 0, 0, 1, - cluster[key].tag_edges.lower_line[i]->point, i, 0.015); + "PayloadLowerBoundary_" + to_string(cluster.cluster_id), 0, 0, 1, + cluster.tag_edges.lower_line[i]->point, i, 0.015); payload_mark_array.markers.push_back(payload_marker); } // Left boundary (green) - for (int i = 0; i < cluster[key].tag_edges.left_line.size(); ++i) { + for (int i = 0; i < cluster.tag_edges.left_line.size(); ++i) { LidarTag::assignMarker( payload_marker, visualization_msgs::msg::Marker::SPHERE, - "PayloadLeftBoundary_" + to_string(cluster[key].cluster_id), 0, 1, 0, - cluster[key].tag_edges.left_line[i]->point, i, 0.015); + "PayloadLeftBoundary_" + to_string(cluster.cluster_id), 0, 1, 0, + cluster.tag_edges.left_line[i]->point, i, 0.015); payload_mark_array.markers.push_back(payload_marker); } // Right boundary (red) - for (int i = 0; i < cluster[key].tag_edges.right_line.size(); ++i) { + for (int i = 0; i < cluster.tag_edges.right_line.size(); ++i) { LidarTag::assignMarker( payload_marker, visualization_msgs::msg::Marker::SPHERE, - "PayloadRightBoundary_" + to_string(cluster[key].cluster_id), 1, 0, 0, - cluster[key].tag_edges.right_line[i]->point, i, 0.015); + "PayloadRightBoundary_" + to_string(cluster.cluster_id), 1, 0, 0, + cluster.tag_edges.right_line[i]->point, i, 0.015); payload_mark_array.markers.push_back(payload_marker); } } else { int count = 0; - for (int i = 0; i < cluster[key].payload_boundary_ptr.size(); ++i) { + for (int i = 0; i < cluster.payload_boundary_ptr.size(); ++i) { LidarTag::assignMarker( payload_marker, visualization_msgs::msg::Marker::SPHERE, - "PayloadBoundary_" + to_string(cluster[key].cluster_id), 1, 0, 0, - cluster[key].payload_boundary_ptr[i]->point, i, 0.015); + "PayloadBoundary_" + to_string(cluster.cluster_id), 1, 0, 0, + cluster.payload_boundary_ptr[i]->point, i, 0.015); payload_mark_array.markers.push_back(payload_marker); count++; // } } } + // Add the initial corners obtained through either line or rectangle estimation + *initial_corners += cluster.initial_corners; + // corner points and RANSAC line if (adaptive_thresholding_) { Eigen::Vector4f eigen_point; @@ -443,90 +483,67 @@ void LidarTag::clusterToPclVectorAndMarkerPublisher( // Corners if (i != 3) { pcl::lineWithLineIntersection( - cluster[key].line_coeff[i], cluster[key].line_coeff[i + 1], eigen_point, 1e-2); + cluster.line_coeff[i], cluster.line_coeff[i + 1], eigen_point, 1e-2); } else { pcl::lineWithLineIntersection( - cluster[key].line_coeff[i], cluster[key].line_coeff[0], eigen_point, 1e-2); + cluster.line_coeff[i], cluster.line_coeff[0], eigen_point, 1e-2); } LidarTag::eigenVectorToPointXYZRI(eigen_point, point); LidarTag::assignMarker( payload_marker, visualization_msgs::msg::Marker::SPHERE, - "Corner_" + to_string(cluster[key].cluster_id), 0, 1, 1, point, i, tag_depth); + "Corner_" + to_string(cluster.cluster_id), 0, 1, 1, point, i, tag_depth); payload_mark_array.markers.push_back(payload_marker); // RANSAC LidarTag::assignMarker( payload_marker, visualization_msgs::msg::Marker::ARROW, - "RANSAC" + to_string(cluster[key].cluster_id), 1, 1, 0, point, i, 0.01); + "RANSAC" + to_string(cluster.cluster_id), 1, 1, 0, point, i, 0.01); double length = sqrt( - pow(cluster[key].line_coeff[i][3], 2) + pow(cluster[key].line_coeff[i][4], 2) + - pow(cluster[key].line_coeff[i][5], 2)); + pow(cluster.line_coeff[i][3], 2) + pow(cluster.line_coeff[i][4], 2) + + pow(cluster.line_coeff[i][5], 2)); payload_marker.scale.x = 0.15; - payload_marker.pose.orientation.x = cluster[key].line_coeff[i][3] / length; - payload_marker.pose.orientation.y = cluster[key].line_coeff[i][4] / length; - payload_marker.pose.orientation.z = cluster[key].line_coeff[i][5] / length; + payload_marker.pose.orientation.x = cluster.line_coeff[i][3] / length; + payload_marker.pose.orientation.y = cluster.line_coeff[i][4] / length; + payload_marker.pose.orientation.z = cluster.line_coeff[i][5] / length; payload_mark_array.markers.push_back(payload_marker); } } // Draw all detected-filled cluster points - for (int i = 0; i < cluster[key].data.size(); ++i) { - if (cluster[key].data[i].valid != 1) { + for (int i = 0; i < cluster.data.size(); ++i) { + if (cluster.data[i].valid != 1) { continue; } points_in_clusters++; - out_cluster->push_back(cluster[key].data[i].point); + out_cluster->push_back(cluster.data[i].point); - double intensity = cluster[key].data[i].point.intensity; + double intensity = cluster.data[i].point.intensity; LidarTag::assignMarker( - marker, visualization_msgs::msg::Marker::SPHERE, to_string(cluster[key].cluster_id), intensity, - intensity, intensity, cluster[key].data[i].point, i, 0.01); + marker, visualization_msgs::msg::Marker::SPHERE, to_string(cluster.cluster_id), intensity, + intensity, intensity, cluster.data[i].point, i, 0.01); cluster_array.markers.push_back(marker); } if (mark_cluster_validity_) { - for (int i = 0; i < cluster[key].edge_points.size(); ++i) { - if (cluster[key].edge_points[i].valid != 1) { - continue; - } - - out_edge_cluster->push_back(cluster[key].edge_points[i].point); - } - - for (int i = 0; i < cluster[key].edge_group1.size(); ++i) { - edge_group_1->push_back(cluster[key].edge_group1[i].point); - } - - for (int i = 0; i < cluster[key].edge_group2.size(); ++i) { - edge_group_2->push_back(cluster[key].edge_group2[i].point); - } - - for (int i = 0; i < cluster[key].edge_group3.size(); ++i) { - edge_group_3->push_back(cluster[key].edge_group3[i].point); - } - - for (int i = 0; i < cluster[key].edge_group4.size(); ++i) { - edge_group_4->push_back(cluster[key].edge_group4[i].point); - } - + for (int ring = 0; ring < beam_num_; ++ring) { - if (cluster[key].payload_right_boundary_ptr[ring] != 0) { - boundary_pts->push_back(cluster[key].payload_right_boundary_ptr[ring]->point); + if (cluster.payload_right_boundary_ptr[ring] != 0) { + boundary_pts->push_back(cluster.payload_right_boundary_ptr[ring]->point); } - if (cluster[key].payload_left_boundary_ptr[ring] != 0) { - boundary_pts->push_back(cluster[key].payload_left_boundary_ptr[ring]->point); + if (cluster.payload_left_boundary_ptr[ring] != 0) { + boundary_pts->push_back(cluster.payload_left_boundary_ptr[ring]->point); } } } if (id_decoding_) { - for (int i = 0; i < cluster[key].rkhs_decoding.associated_pattern_3d->cols(); ++i) { + for (int i = 0; i < cluster.rkhs_decoding.associated_pattern_3d->cols(); ++i) { PointXYZRI point; - point.x = cluster[key].rkhs_decoding.associated_pattern_3d->col(i)(0); - point.y = cluster[key].rkhs_decoding.associated_pattern_3d->col(i)(1); - point.z = cluster[key].rkhs_decoding.associated_pattern_3d->col(i)(2); + point.x = cluster.rkhs_decoding.associated_pattern_3d->col(i)(0); + point.y = cluster.rkhs_decoding.associated_pattern_3d->col(i)(1); + point.z = cluster.rkhs_decoding.associated_pattern_3d->col(i)(2); if (point.x >= 0) { point.intensity = 200; @@ -536,43 +553,43 @@ void LidarTag::clusterToPclVectorAndMarkerPublisher( out_payload->push_back(point); } - for (int i = 0; i < cluster[key].rkhs_decoding.template_points_3d.cols(); ++i) { + for (int i = 0; i < cluster.rkhs_decoding.template_points_3d.cols(); ++i) { PointXYZRI point; - point.x = cluster[key].rkhs_decoding.template_points_3d.col(i)(0); - point.y = cluster[key].rkhs_decoding.template_points_3d.col(i)(1); - point.z = cluster[key].rkhs_decoding.template_points_3d.col(i)(2); - point.intensity = cluster[key].rkhs_decoding.template_points_3d.col(i)(3); + point.x = cluster.rkhs_decoding.template_points_3d.col(i)(0); + point.y = cluster.rkhs_decoding.template_points_3d.col(i)(1); + point.z = cluster.rkhs_decoding.template_points_3d.col(i)(2); + point.intensity = cluster.rkhs_decoding.template_points_3d.col(i)(3); out_payload_3d->push_back(point); } } if (mark_cluster_validity_) { - for (int i = 0; i < cluster[key].rkhs_decoding.initial_template_points.cols(); ++i) { + for (int i = 0; i < cluster.rkhs_decoding.initial_template_points.cols(); ++i) { PointXYZRI point; - point.x = cluster[key].rkhs_decoding.initial_template_points.col(i)(0); - point.y = cluster[key].rkhs_decoding.initial_template_points.col(i)(1); - point.z = cluster[key].rkhs_decoding.initial_template_points.col(i)(2); - point.intensity = cluster[key].rkhs_decoding.initial_template_points.col(i)(3); + point.x = cluster.rkhs_decoding.initial_template_points.col(i)(0); + point.y = cluster.rkhs_decoding.initial_template_points.col(i)(1); + point.z = cluster.rkhs_decoding.initial_template_points.col(i)(2); + point.intensity = cluster.rkhs_decoding.initial_template_points.col(i)(3); out_initial_target->push_back(point); } - for (int i = 0; i < cluster[key].rkhs_decoding.template_points.cols(); ++i) { + for (int i = 0; i < cluster.rkhs_decoding.template_points.cols(); ++i) { PointXYZRI point; - point.x = cluster[key].rkhs_decoding.template_points.col(i)(0); - point.y = cluster[key].rkhs_decoding.template_points.col(i)(1); - point.z = cluster[key].rkhs_decoding.template_points.col(i)(2); - point.intensity = cluster[key].rkhs_decoding.template_points.col(i)(3); + point.x = cluster.rkhs_decoding.template_points.col(i)(0); + point.y = cluster.rkhs_decoding.template_points.col(i)(1); + point.z = cluster.rkhs_decoding.template_points.col(i)(2); + point.intensity = cluster.rkhs_decoding.template_points.col(i)(3); out_target->push_back(point); } } if (id_decoding_) { - addCorners(cluster[key], corners_array, corners_markers_array); + addCorners(cluster, corners_array, corners_markers_array); - //if (getBoundaryCorners(cluster[key], boundary_pts)) { - addBoundaryCorners(cluster[key], boundary_corners_array, boundary_corners_markers_array); + //if (getBoundaryCorners(cluster, boundary_pts)) { + addBoundaryCorners(cluster, boundary_corners_array, boundary_corners_markers_array); //} } // Publish to a lidartag channel - detectionArrayPublisher(cluster[key], detections_array); + detectionArrayPublisher(cluster, detections_array); } detections_array.header = point_cloud_header_; @@ -589,272 +606,12 @@ void LidarTag::clusterToPclVectorAndMarkerPublisher( id_marker_pub_->publish(id_mark_array); detection_array_pub_->publish(detections_array); - colorClusters(cluster); - displayClusterPointSize(cluster); - displayClusterIndexNumber(cluster); - publishLidartagCluster(cluster); + colorClusters(clusters); + displayClusterPointSize(clusters); + displayClusterIndexNumber(clusters); + publishLidartagCluster(clusters); } -/*void LidarTag::publishClusterInfo(const ClusterFamily_t cluster) -{ - jsk_msgs::msg::OverlayText detail_valid_text; - std::string output_str; - Eigen::IOFormat mat_format(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]"); - Eigen::Matrix3f M; - Eigen::Matrix3f input_mat; - M = U_ * r_ * V_.transpose(); - input_mat = vertices_.rightCols(4) * payload_vertices_.transpose(); - - int ring = std::round(beam_num_ / 2); - int longer_side_ring = std::round((cluster.top_ring + cluster.bottom_ring) / 2); - double point_resolution = 2 * M_PI / lidar_system_.ring_average_table[ring].average; - auto distance = - std::sqrt(pow(cluster.average.x, 2) + pow(cluster.average.y, 2) + pow(cluster.average.z, 2)); - int num_vertical_ring = std::abs(cluster.top_ring - cluster.bottom_ring) + 1; - int num_horizontal_points = - std::ceil((SQRT2 * payload_size_) / (distance * std::sin(point_resolution))); - - std::stringstream R_ss; - std::stringstream U_ss; - std::stringstream V_ss; - std::stringstream r_ss; - std::stringstream M_ss; - std::stringstream vertices_ss; - std::stringstream input_ss; - std::stringstream payload_vertices_ss; - std::stringstream ordered_payload_vertices_ss; - std::stringstream pa_ss; - R_ss << R_.format(mat_format); - U_ss << U_.format(mat_format); - V_ss << V_.format(mat_format); - r_ss << r_.format(mat_format); - M_ss << M.format(mat_format); - vertices_ss << vertices_.format(mat_format); - payload_vertices_ss << payload_vertices_.format(mat_format); - ordered_payload_vertices_ss << ordered_payload_vertices_.format(mat_format); - input_ss << input_mat.format(mat_format); - pa_ss << cluster.principal_axes.format(mat_format); - - detail_valid_text.action = jsk_msgs::msg::OverlayText::ADD; - detail_valid_text.left = 200; - detail_valid_text.top = 200; - detail_valid_text.width = 800; - detail_valid_text.height = 800; - detail_valid_text.text_size = 12; - output_str = - "cluster_id = " + to_string(cluster.cluster_id) + "\n" + "valid = " + to_string(cluster.valid) + - "\n" + "detail_valid = " + to_string(cluster.detail_valid) + "\n" + - "pose_estimation_status = " + to_string(cluster.pose_estimation_status) + "\n" + - "number of horizontal_points = " + to_string(num_horizontal_points) + "\n" + - "total duration = " + to_string(timing_.total_duration) + "\n" + "exceeded points size = " + - to_string(((cluster.data.size() + cluster.edge_points.size()) - cluster.expected_points)) + - "\n" + "average.x = " + to_string(cluster.average.x) + "\n" + - "average.y = " + to_string(cluster.average.y) + "\n" + - "average.z = " + to_string(cluster.average.z) + "\n" + - "pose.roll = " + to_string(cluster.pose.roll) + "\n" + - "pose.pitch = " + to_string(cluster.pose.pitch) + "\n" + - "pose.yaw = " + to_string(cluster.pose.yaw) + "\n" + - "initial_pose.roll = " + to_string(cluster.initial_pose.roll) + "\n" + - "initial_pose.pitch = " + to_string(cluster.initial_pose.pitch) + "\n" + - "initial_pose.yaw = " + to_string(cluster.initial_pose.yaw) + "\n" + - // "intersection1_x,y = " + to_string(intersection1_[0]) + ", " + - // to_string(intersection1_[1]) + "\n" + - // "intersection2_x,y = " + to_string(intersection2_[0]) + ", " + - // to_string(intersection2_[1]) + "\n" + - // "intersection3_x,y = " + to_string(intersection3_[0]) + ", " + - // to_string(intersection3_[1]) + "\n" + - // "intersection4_x,y = " + to_string(intersection4_[0]) + ", " + - // to_string(intersection4_[1]) + "\n" + - // "payload_vertices = " + payload_vertices_ss.str() + "\n" + - // "ordered_payload_vertices = " + ordered_payload_vertices_ss.str() + - // "\n" + "input = " + input_ss.str() + "\n" + - // "vertices = " + vertices_ss.str() + "\n" + - // "princlple axis = " + pa_ss.str() + "\n" + - // "U * r * Vt = " + M_ss.str() + "\n" + - // "U = " + U_ss.str() + "\n" + - // "r = " + r_ss.str() + "\n" + - // "V = " + V_ss.str() + "\n" + - "R(U*Vt) = " + R_ss.str() + "\n" + - "max_intensity = " + to_string(cluster.max_intensity.intensity) + "\n" + - "min_intensity = " + to_string(cluster.min_intensity.intensity) + "\n" + - "top ring = " + to_string(cluster.top_ring) + "\n" + - "bottom ring = " + to_string(cluster.bottom_ring) + "\n" + - // "edge_inliers = " + to_string(cluster.edge_inliers) + "\n" + - // "data_inliers = " + to_string(cluster.data_inliers) + "\n" + - // "inliers = " + to_string(cluster.inliers) + "\n" + - "percentages_inliers = " + to_string(cluster.percentages_inliers) + "\n" + - // "boundary_pts = " + to_string(cluster.boundary_pts) + "\n" + - // "payload_without_boundary = " + - // to_string(cluster.payload_without_boundary) + "\n" + - "tag_size = " + to_string(cluster.tag_size) + - "\n" - "payload_size = " + - to_string(payload_size_) + "\n"; - // "special case = " + - // to_string(cluster.special_case) + "\n"; - - detail_valid_text.text = output_str; - detail_valid_text_pub_->publish(detail_valid_text); -} */ - -/*bool LidarTag::getBoundaryCorners( - ClusterFamily_t & cluster, pcl::PointCloud::Ptr boundaryPts) -{ - Eigen::Vector4f line1; - Eigen::Vector4f line2; - Eigen::Vector4f line3; - Eigen::Vector4f line4; - pcl::PointCloud::Ptr cloud1(new pcl::PointCloud); - pcl::PointCloud::Ptr cloud2(new pcl::PointCloud); - pcl::PointCloud::Ptr cloud3(new pcl::PointCloud); - pcl::PointCloud::Ptr cloud4(new pcl::PointCloud); - pcl::PointCloud::Ptr line_cloud1(new pcl::PointCloud); - pcl::PointCloud::Ptr line_cloud2(new pcl::PointCloud); - pcl::PointCloud::Ptr line_cloud3(new pcl::PointCloud); - pcl::PointCloud::Ptr line_cloud4(new pcl::PointCloud); - - cloud1->points.resize(boundaryPts->points.size()); - cloud1->clear(); - cloud2->points.resize(boundaryPts->points.size()); - cloud2->clear(); - cloud3->points.resize(boundaryPts->points.size()); - cloud3->clear(); - cloud4->points.resize(boundaryPts->points.size()); - cloud4->clear(); - line_cloud1->points.resize(boundaryPts->points.size()); - line_cloud1->clear(); - line_cloud2->points.resize(boundaryPts->points.size()); - line_cloud2->clear(); - line_cloud3->points.resize(boundaryPts->points.size()); - line_cloud3->clear(); - line_cloud4->points.resize(boundaryPts->points.size()); - line_cloud4->clear(); - - for (size_t i = 0; i < boundaryPts->points.size(); i++) { - Eigen::Vector3f edge_point( - boundaryPts->points[i].x - cluster.average.x, boundaryPts->points[i].y - cluster.average.y, - boundaryPts->points[i].z - cluster.average.z); - Eigen::Matrix transform_matrix = cluster.principal_axes; - transform_matrix = (transform_matrix.transpose()).eval(); - Eigen::Vector3f transformed_edge_point = transform_matrix * edge_point; - pcl::PointXYZ p; - p.x = transformed_edge_point(0); - p.y = transformed_edge_point(1); - p.z = 0; - LidarPoints_t group_point; - group_point.point = boundaryPts->points[i]; - if (transformed_edge_point(0) > 0) { - if (transformed_edge_point(1) > 0) { - cloud1->points.push_back(p); - group_point.point.intensity = 0; - cluster.edge_group1.push_back(group_point); - } else { - cloud2->points.push_back(p); - group_point.point.intensity = 85; - cluster.edge_group2.push_back(group_point); - } - } else { - if (transformed_edge_point(1) > 0) { - cloud4->points.push_back(p); - group_point.point.intensity = 170; - cluster.edge_group4.push_back(group_point); - } else { - cloud3->points.push_back(p); - group_point.point.intensity = 255; - cluster.edge_group3.push_back(group_point); - } - } - } - if (!LidarTag::getLines(cloud1, line1, line_cloud1)) { - RCLCPP_WARN_STREAM(get_logger(), "Can not get boundary line1"); - return false; - } - - if (!LidarTag::getLines(cloud2, line2, line_cloud2)) { - RCLCPP_WARN_STREAM(get_logger(), "Can not get boundary line2"); - return false; - } - - if (!LidarTag::getLines(cloud3, line3, line_cloud3)) { - RCLCPP_WARN_STREAM(get_logger(), "Can not get boundary line3"); - return false; - } - - if (!LidarTag::getLines(cloud4, line4, line_cloud4)) { - RCLCPP_WARN_STREAM(get_logger(), "Can not get boundary line4"); - return false; - } - - Eigen::Vector3f intersection1 = LidarTag::getintersec(line1, line2); - Eigen::Vector3f intersection2 = LidarTag::getintersec(line2, line3); - Eigen::Vector3f intersection3 = LidarTag::getintersec(line3, line4); - Eigen::Vector3f intersection4 = LidarTag::getintersec(line1, line4); - - // Filter the cases when outliers pass RANSAC and produce a "bad" tag - if(intersection1.norm() > cluster.tag_size - || intersection2.norm() > cluster.tag_size - || intersection3.norm() > cluster.tag_size - || intersection4.norm() > cluster.tag_size) - { - return false; - } - - - Eigen::MatrixXf payload_vertices(3, 4); - payload_vertices.col(0) = cluster.principal_axes * intersection1; - payload_vertices.col(1) = cluster.principal_axes * intersection2; - payload_vertices.col(2) = cluster.principal_axes * intersection3; - payload_vertices.col(3) = cluster.principal_axes * intersection4; - - Eigen::MatrixXf ordered_payload_vertices = getOrderedCorners(payload_vertices, cluster); - Eigen::MatrixXf vertices = Eigen::MatrixXf::Zero(3, 5); - utils::formGrid(vertices, 0, 0, 0, cluster.tag_size); - Eigen::Matrix3f R; - std::vector mats; - mats = utils::fitGridNew(vertices, R, ordered_payload_vertices); - - Eigen::MatrixXf::Index col; - payload_vertices.row(1).minCoeff(&col); - utils::eigen2Corners(payload_vertices.col(col), cluster.tag_boundary_corners.right); - - payload_vertices.row(1).maxCoeff(&col); - utils::eigen2Corners(payload_vertices.col(col), cluster.tag_boundary_corners.left); - - payload_vertices.row(2).minCoeff(&col); - utils::eigen2Corners(payload_vertices.col(col), cluster.tag_boundary_corners.down); - - payload_vertices.row(2).maxCoeff(&col); - utils::eigen2Corners(payload_vertices.col(col), cluster.tag_boundary_corners.top); - - cluster.boundary_corner_offset_array.resize(4); - point p_corner; - for (int i = 0; i < 4; ++i) { - - p_corner.x = ordered_payload_vertices.col(i)(0); - p_corner.y = ordered_payload_vertices.col(i)(1); - p_corner.z = ordered_payload_vertices.col(i)(2); - - cluster.boundary_corner_offset_array[(6 - i - cluster.rkhs_decoding.rotation_angle) % 4] = p_corner; - } - - - for(int i = 0; i < 4; i++) { - float dx = std::abs(cluster.boundary_corner_offset_array[i].x - cluster.corner_offset_array[i].x); - float dy = std::abs(cluster.boundary_corner_offset_array[i].y - cluster.corner_offset_array[i].y); - float dz = std::abs(cluster.boundary_corner_offset_array[i].z - cluster.corner_offset_array[i].z); - - float r = std::sqrt(dx*dx + dy*dy + dz*dz); - if (std::abs(r) > 0.5f) { - int x = 0; - } - else { - int asd = 0; - } - } - - return true; -}*/ - void LidarTag::addCorners( const ClusterFamily_t & cluster, lidartag_msgs::msg::CornersArray & corners_array_msg, @@ -923,12 +680,12 @@ void LidarTag::addCornersAux( visualization_msgs::msg::Marker marker, bottom_left_marker, bottom_right_marker, top_right_marker, top_left_marker, center_marker; - marker.header.frame_id = pub_frame_; + marker.header.frame_id = lidar_frame_; marker.header.stamp = clock_->now(); marker.id = 0; marker.type = visualization_msgs::msg::Marker::SPHERE; marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(0.5); + marker.lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; @@ -998,7 +755,7 @@ void LidarTag::addCornersAux( corners_array_msg.corners.push_back(corners); } -void LidarTag::colorClusters(const std::vector & cluster) +void LidarTag::colorClusters(const std::vector & clusters) { pcl::PointCloud::Ptr colored_cluster_buff( new pcl::PointCloud); @@ -1007,15 +764,26 @@ void LidarTag::colorClusters(const std::vector & cluster) int r, g, b; srand(100); - for (int key = 0; key < cluster.size(); ++key) { - if (cluster[key].valid != 1) { + for (int key = 0; key < clusters.size(); ++key) { + const ClusterFamily_t & cluster = clusters[key]; + if (cluster.valid != 1) { r = rand() % 255; g = rand() % 255; b = rand() % 255; - for (int i = 0; i < cluster[key].data.size(); ++i) { - colored_point.x = cluster[key].data[i].point.x; - colored_point.y = cluster[key].data[i].point.y; - colored_point.z = cluster[key].data[i].point.z; + for (int i = 0; i < cluster.data.size(); ++i) { + colored_point.x = cluster.data[i].point.x; + colored_point.y = cluster.data[i].point.y; + colored_point.z = cluster.data[i].point.z; + colored_point.r = r; + colored_point.g = g; + colored_point.b = b; + colored_cluster_buff->points.push_back(colored_point); + } + + for (int i = 0; i < cluster.edge_points.size(); ++i) { + colored_point.x = cluster.edge_points[i].point.x; + colored_point.y = cluster.edge_points[i].point.y; + colored_point.z = cluster.edge_points[i].point.z; colored_point.r = r; colored_point.g = g; colored_point.b = b; @@ -1026,7 +794,7 @@ void LidarTag::colorClusters(const std::vector & cluster) sensor_msgs::msg::PointCloud2 colored_cluster_buff_msg; pcl::toROSMsg(*colored_cluster_buff, colored_cluster_buff_msg); - colored_cluster_buff_msg.header.frame_id = pub_frame_; + colored_cluster_buff_msg.header.frame_id = lidar_frame_; colored_cluster_buff_msg.header.stamp = clock_->now(); colored_cluster_buff_pub_->publish(colored_cluster_buff_msg); } @@ -1036,10 +804,10 @@ void LidarTag::displayClusterPointSize(const std::vector & clus visualization_msgs::msg::MarkerArray marker_array; marker_array.markers.resize(cluster_buff.size()); visualization_msgs::msg::Marker marker; - marker.header.frame_id = pub_frame_; + marker.header.frame_id = lidar_frame_; marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(0.5); + marker.lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; @@ -1060,7 +828,7 @@ void LidarTag::displayClusterPointSize(const std::vector & clus if (points_size > 50) { marker.header.stamp = clock_->now(); marker.id = cluster_buff[i].cluster_id; - marker.text = to_string(points_size); + marker.text = to_string(marker.id) + "/" + to_string(points_size); marker.ns = "ps_marker_" + to_string(cluster_buff[i].cluster_id); marker.pose.position.x = cluster_buff[i].average.x; marker.pose.position.y = cluster_buff[i].average.y; @@ -1077,10 +845,10 @@ void LidarTag::displayClusterIndexNumber(const std::vector & cl visualization_msgs::msg::MarkerArray marker_array; marker_array.markers.resize(cluster_buff.size()); visualization_msgs::msg::Marker marker; - marker.header.frame_id = pub_frame_; + marker.header.frame_id = lidar_frame_; marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(0.5); + marker.lifetime = rclcpp::Duration::from_seconds(sleep_time_for_vis_ * 10); marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; @@ -1100,7 +868,7 @@ void LidarTag::displayClusterIndexNumber(const std::vector & cl if (points_size > 50) { marker.header.stamp = clock_->now(); marker.id = cluster_buff[i].cluster_id; - marker.text = to_string(i) + "/" + to_string(cluster_buff[i].detail_valid); + marker.text = to_string(i) + "/" + to_string(static_cast(cluster_buff[i].detail_valid)); marker.ns = "in_marker_" + to_string(cluster_buff[i].cluster_id); marker.pose.position.x = cluster_buff[i].average.x; marker.pose.position.y = cluster_buff[i].average.y; diff --git a/src/main.cpp b/src/main.cpp index 5ddd0a0..4ef504f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -29,7 +29,7 @@ */ -#include +#include #include using namespace std; @@ -43,7 +43,5 @@ int main(int argc, char **argv){ rclcpp::spin(node); rclcpp::shutdown(); - cout << "Done!" << endl; - return 0; } diff --git a/src/rectangle_estimator.cpp b/src/rectangle_estimator.cpp new file mode 100644 index 0000000..eb2211d --- /dev/null +++ b/src/rectangle_estimator.cpp @@ -0,0 +1,503 @@ +// Copyright 2021 Tier 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 + +RectangleEstimator::RectangleEstimator() +{ + use_ransac_ = false; + filter_by_coefficients_ = true; + max_ransac_iterations_ = 20; + max_error_ = 0.05; + fix_point_groups_ = false; + + augmented_matrix_.resize(111, 6); +} + +void RectangleEstimator::setInputPoints( + pcl::PointCloud::Ptr & points1, + pcl::PointCloud::Ptr & points2, + pcl::PointCloud::Ptr & points3, + pcl::PointCloud::Ptr & points4) +{ + points1_ = points1; + points2_ = points2; + points3_ = points3; + points4_ = points4; + + points.clear(); + points += *points1; + points += *points2; + points += *points3; + points += *points4; +} + +bool RectangleEstimator::estimate() +{ + // This implementation assumes ordered points to detect probably wrong estimations + // * + // 4 / \ 1 + // / \ + // * * + // \ / + // 3 \ / 2 + // * + + bool status; + preparePointsMatrix(); + + if (use_ransac_) { + status = estimate_ransac(); + } + else { + status = estimate_imlp(points1_, points2_, points3_, points4_, estimated_n, estimated_c); + } + + bool status_tmp = status; + + if (status && filter_by_coefficients_) { + status = checkCoefficients(); + } + + if(status != status_tmp) { + bool asd = 0; + } + + return status; +} + +bool RectangleEstimator::estimate_ransac() +{ + int ransac_min_points = 2; + + pcl::PointCloud::Ptr points1(new pcl::PointCloud); + pcl::PointCloud::Ptr points2(new pcl::PointCloud); + pcl::PointCloud::Ptr points3(new pcl::PointCloud); + pcl::PointCloud::Ptr points4(new pcl::PointCloud); + + points1->points.resize(ransac_min_points); + points2->points.resize(ransac_min_points); + points3->points.resize(ransac_min_points); + points4->points.resize(ransac_min_points); + + std::random_device dev; + std::mt19937 rng(dev()); + + auto sample_points = [&](auto & in_points, auto & out_points) + { + out_points->clear(); + std::vector index_vector; + for (int i=0; i < in_points->points.size(); ++i) { + index_vector.push_back(i); + } + + std::random_shuffle ( index_vector.begin(), index_vector.end() ); + + for(int i = 0; i < ransac_min_points; i++) { + int input_index = index_vector[i]; + out_points->push_back(in_points->points[input_index]); + } + }; + + double min_error = 1e12; + int max_inliers = 0; + bool status = false; + + Eigen::VectorXd errors1, errors2, errors3, errors4; + std::vector inlier_vector; + + for (int i = 0; i < max_ransac_iterations_; i++) { + + sample_points(points1_, points1); + sample_points(points2_, points2); + sample_points(points3_, points3); + sample_points(points4_, points4); + + Eigen::Vector4d h_c; + Eigen::Vector2d h_n; + + bool h_status = estimate_imlp(points1, points2, points3, points4, h_n, h_c); + int h_inliers = 0; + + double error = getModelErrorAndInliers(h_n, h_c, h_inliers, + inlier_vector, inlier_vector, inlier_vector, inlier_vector, + errors1, errors2, errors3, errors4, false); + + if (h_status && (h_inliers > max_inliers || h_inliers == max_inliers && error < min_error)) { + min_error = error; + max_inliers = h_inliers; + estimated_n = h_n; + estimated_c = h_c; + status = h_status; + + if (fix_point_groups_) { + fixPointGroups(errors1, errors2, errors3, errors4); + } + + } + } + + if (!status) { + return false; + } + + int inliers = 0; + std::vector inliers1, inliers2, inliers3, inliers4; + + double error = getModelErrorAndInliers(estimated_n, estimated_c, inliers, + inliers1, inliers2, inliers3, inliers4, + errors1, errors2, errors3, errors4, true); + + auto eigen_to_pointcloud = [&](std::vector & inliers_vector, + auto & points) + { + points->resize(inliers_vector.size()); + + for (int i = 0; i < inliers_vector.size(); i++) { + points->points[i].x = inliers_vector[i].x(); + points->points[i].y = inliers_vector[i].y(); + } + }; + + eigen_to_pointcloud(inliers1, points1); + eigen_to_pointcloud(inliers2, points2); + eigen_to_pointcloud(inliers3, points3); + eigen_to_pointcloud(inliers4, points4); + + status = estimate_imlp(points1, points2, points3, points4, estimated_n, estimated_c); + + return status; +} + +bool RectangleEstimator::estimate_imlp( + pcl::PointCloud::Ptr points1, + pcl::PointCloud::Ptr points2, + pcl::PointCloud::Ptr points3, + pcl::PointCloud::Ptr points4, + Eigen::Vector2d & n_out, + Eigen::Vector4d & c_out + ) +{ + int num_points_1 = points1->points.size(); + int num_points_2 = points2->points.size(); + int num_points_3 = points3->points.size(); + int num_points_4 = points4->points.size(); + + int num_points = num_points_1 + num_points_2 + num_points_3 + num_points_4; + + if (num_points_1 < 2 || num_points_2 < 2 || num_points_3 < 2 + || num_points_4 < 2 || num_points < 6 ) + { + return false; + } + + augmented_matrix_.resize(num_points, 6); + augmented_matrix_.setZero(); + + int i = 0; + + auto lambda = [&](auto & points, int column) + { + for (auto & p : points) { + this->augmented_matrix_(i, column) = 1.0; + this->augmented_matrix_(i, 4) = column % 2 == 0 ? p.x : p.y; + this->augmented_matrix_(i, 5) = column % 2 == 0 ? p.y : -p.x; + + i += 1; + } + }; + + lambda(points1->points, 0); + lambda(points2->points, 1); + lambda(points3->points, 2); + lambda(points4->points, 3); + + Eigen::HouseholderQR qr(augmented_matrix_); + Eigen::MatrixXd temp = qr.matrixQR().triangularView(); + Eigen::MatrixXd r_matrix = temp.topRows(augmented_matrix_.cols()); + Eigen::MatrixXd r_matrix_sub = r_matrix.block(4, 4, 2, 2); + + Eigen::JacobiSVD svd(r_matrix_sub, Eigen::ComputeThinU | Eigen::ComputeThinV); + + Eigen::MatrixXd v = svd.matrixV(); + Eigen::MatrixXd n = v.col(1); + + Eigen::MatrixXd aux1 = r_matrix.block(0, 0, 4, 4); + Eigen::MatrixXd aux2 = r_matrix.block(0, 4, 4, 2); + + Eigen::MatrixXd c = -(aux1.inverse()) * (aux2) * n; + + Eigen::MatrixXd error1 = aux1 * c + aux2 * n; + + Eigen::VectorXd x; + x.resize(6); + x.block(0,0,4,1) = c; + x.block(4,0,2,1) = n; + + Eigen::VectorXd error2 = augmented_matrix_ * x; + + c_out = c; + n_out = n; + + return true; +} + +std::vector RectangleEstimator::getCorners() +{ + return getCorners(estimated_n, estimated_c); +} + +std::vector RectangleEstimator::getCorners(Eigen::Vector2d & n, Eigen::Vector4d & c) +{ + std::vector corners; + + for (int i = 0; i < 4; i++) { + double c1 = c(i); + double c2 = c((i + 1) % 4); + + double nx1 = i % 2 == 0 ? n(0) : -n(1); + double ny1 = i % 2 == 0 ? n(1) : n(0); + double nx2 = (i +1) % 2 == 0 ? n(0) : -n(1); + double ny2 = (i +1) % 2 == 0 ? n(1) : n(0); + + double y = (nx2*c1 - nx1*c2) / (nx1*ny2 - nx2*ny1); + double x = (-c1 - ny1*y) / nx1; + + corners.push_back(Eigen::Vector2d(x, y)); + } + + return corners; +} + +Eigen::Vector4d RectangleEstimator::getCCoefficients() const +{ + return estimated_c; +} + +Eigen::Vector2d RectangleEstimator::getNCoefficients() const +{ + return estimated_n; +} + +void RectangleEstimator::setRANSAC(bool enable) +{ + use_ransac_ = enable; +} + +void RectangleEstimator::setFilterByCoefficients(bool enable) +{ + filter_by_coefficients_ = enable; +} + +void RectangleEstimator::setMaxIterations(int iterations) +{ + max_ransac_iterations_ = iterations; +} + +void RectangleEstimator::setInlierError(double error) +{ + max_error_ = error; +} + +void RectangleEstimator::setFixPointGroups(bool enable) +{ + fix_point_groups_ = enable; +} + +bool RectangleEstimator::checkCoefficients() const +{ + Eigen::Vector2d reference(1.0, 1.0); + reference.normalize(); + double max_cos_distance = std::cos(45.0 * M_PI / 180.0); + + return estimated_n.dot(reference) >= max_cos_distance; +} + +void RectangleEstimator::preparePointsMatrix() +{ + int num_points = points1_->size() + points2_->size() + + points3_->size()+ points4_->size(); + points_matrix_.resize(num_points, 2); + + int index = 0; + + auto lambda = [&](auto & points, int column) + { + for (auto & p : points) { + this->points_matrix_(index, 0) = p.x; + this->points_matrix_(index, 1) = p.y; + index += 1; + } + }; + + lambda(points1_->points, 0); + lambda(points2_->points, 1); + lambda(points3_->points, 2); + lambda(points4_->points, 3); +} + +double RectangleEstimator::getModelErrorAndInliers( + Eigen::Vector2d & n, Eigen::Vector4d & c, int & inliers, + std::vector & inliers1, std::vector & inliers2, + std::vector & inliers3, std::vector & inliers4, + Eigen::VectorXd & errors1, Eigen::VectorXd & errors2, + Eigen::VectorXd & errors3, Eigen::VectorXd & errors4, + bool add_inliers) +{ + Eigen::Vector2d n1 = n; + Eigen::Vector2d n2(-n1.y(), n1.x()); + + Eigen::Vector2d d0_paralell(-n1.y(), n1.x()); + Eigen::Vector2d d0_perpendicular(n1.x(), n1.y()); + + Eigen::Vector2d d1_paralell(-n2.y(), n2.x()); + Eigen::Vector2d d1_perpendicular(n2.x(), n2.y()); + + Eigen::Vector2d p0(0, -c(0) / n1.y()); + Eigen::Vector2d p1(0, -c(1) / n2.y()); + Eigen::Vector2d p2(0, -c(2) / n1.y()); + Eigen::Vector2d p3(0, -c(3) / n2.y()); + + std::vector corners = getCorners(n, c); + double t0_1 = (corners[0] - p0).dot(d0_paralell); + double t0_2 = (corners[3] - p0).dot(d0_paralell); + double t0_min = std::min(t0_1, t0_2); + double t0_max = std::max(t0_1, t0_2); + double t1_1 = (corners[1] - p1).dot(d1_paralell); + double t1_2 = (corners[0] - p1).dot(d1_paralell); + double t1_min = std::min(t1_1, t1_2); + double t1_max = std::max(t1_1, t1_2); + double t2_1 = (corners[2] - p2).dot(d0_paralell); + double t2_2 = (corners[1] - p2).dot(d0_paralell); + double t2_min = std::min(t2_1, t2_2); + double t2_max = std::max(t2_1, t2_2); + double t3_1 = (corners[3] - p3).dot(d1_paralell); + double t3_2 = (corners[2] - p3).dot(d1_paralell); + double t3_min = std::min(t3_1, t3_2); + double t3_max = std::max(t3_1, t3_2); + + errors1.resize(points_matrix_.rows()); + errors2.resize(points_matrix_.rows()); + errors3.resize(points_matrix_.rows()); + errors4.resize(points_matrix_.rows()); + + auto calculate_line_error = [&]( + Eigen::Vector2d & p0, + Eigen::Vector2d & d_paralell, + Eigen::Vector2d & d1_perpendicular, + double t_min, double t_max, + Eigen::VectorXd & error, + std::vector & inliers_vector) + { + for(int i = 0; i < points_matrix_.rows(); i++) + { + Eigen::Vector2d q = points_matrix_.row(i); + + double t = (q - p0).dot(d_paralell); + double lambda = std::abs((q - p0).dot(d1_perpendicular)); + double t_error = t <= t_min ? t_min - t : t > t_max ? t - t_max : 0.0; + double e = std::max(t_error, lambda); + error(i) = e; + + if(add_inliers && e <= max_error_) { + inliers_vector.push_back(q); + } + } + }; + + calculate_line_error(p0, d0_paralell, d0_perpendicular, t0_min, t0_max, errors1, inliers1); + calculate_line_error(p1, d1_paralell, d1_perpendicular, t1_min, t1_max, errors2, inliers2); + calculate_line_error(p2, d0_paralell, d0_perpendicular, t2_min, t2_max, errors3, inliers3); + calculate_line_error(p3, d1_paralell, d1_perpendicular, t3_min, t3_max, errors4, inliers4); + + Eigen::VectorXd error = errors1.cwiseMin(errors2).cwiseMin(errors3).cwiseMin(errors4); + + inliers = 0; + + for(int i = 0; i < points_matrix_.rows(); i++){ + if (error(i) < max_error_) { + inliers++; + } + } + + return error.mean(); +} + +void RectangleEstimator::fixPointGroups( + Eigen::VectorXd & errors1, Eigen::VectorXd & errors2, + Eigen::VectorXd & errors3, Eigen::VectorXd & errors4) +{ + assert(points.size() == errors1.rows()); + + Eigen::VectorXd min_errors = errors1.cwiseMin(errors2).cwiseMin(errors3).cwiseMin(errors4); + + int num_points1 = 0; + int num_points2 = 0; + int num_points3 = 0; + int num_points4 = 0; + + for(int i = 0; i < points.size(); i++){ + if (errors1(i) == min_errors(i)) { + num_points1++; + } + else if (errors2(i) == min_errors(i)) { + num_points2++; + } + else if (errors3(i) == min_errors(i)) { + num_points3++; + } + else if (errors4(i) == min_errors(i)) { + num_points4++; + } + else { + assert(false); + } + } + + if (num_points1 < 3 || num_points2 < 3 || num_points3 < 3 || num_points4 < 3) { + return; + } + + points1_->clear(); + points2_->clear(); + points3_->clear(); + points4_->clear(); + + for(int i = 0; i < points.size(); i++){ + if (errors1(i) == min_errors(i)) { + points1_->push_back(points[i]); + } + else if (errors2(i) == min_errors(i)) { + points2_->push_back(points[i]); + } + else if (errors3(i) == min_errors(i)) { + points3_->push_back(points[i]); + } + else if (errors4(i) == min_errors(i)) { + points4_->push_back(points[i]); + } + else { + assert(false); + } + } + + assert(points1_->size() >= 2); + assert(points2_->size() >= 2); + assert(points3_->size() >= 2); + assert(points4_->size() >= 2); +} diff --git a/src/tag16h5.cpp b/src/tag16h5.cpp index ac49bf4..0f4ab12 100644 --- a/src/tag16h5.cpp +++ b/src/tag16h5.cpp @@ -28,7 +28,7 @@ * WEBSITE: https://www.brucerobot.com/ */ -#include "tag16h5.h" +#include #include BipedLab::GrizTagFamily_t * tag16h5_create() diff --git a/src/tag49h14.cpp b/src/tag49h14.cpp index db1a30c..3e83ce4 100644 --- a/src/tag49h14.cpp +++ b/src/tag49h14.cpp @@ -28,7 +28,7 @@ * WEBSITE: https://www.brucerobot.com/ */ -#include "tag49h14.h" +#include #include BipedLab::GrizTagFamily_t * tag49h14_create() diff --git a/src/utils.cpp b/src/utils.cpp index 0b57062..321728b 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -29,12 +29,13 @@ * WEBSITE: https://www.brucerobot.com/ */ -#include "utils.h" +#include +#include + #include #include // std::sort, std::stable_sort #include #include // std::iota -#include "lidartag.h" namespace BipedLab { diff --git a/test/test_rectangle_estimation.cpp b/test/test_rectangle_estimation.cpp new file mode 100644 index 0000000..ac080f5 --- /dev/null +++ b/test/test_rectangle_estimation.cpp @@ -0,0 +1,184 @@ +// Copyright 2021 Tier 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 + +constexpr double epsilon = 1e-6; + +TEST(lidartag, rectangle_estimation) +{ + RectangleEstimator estimator; + + pcl::PointCloud::Ptr points1(new pcl::PointCloud); + pcl::PointCloud::Ptr points2(new pcl::PointCloud); + pcl::PointCloud::Ptr points3(new pcl::PointCloud); + pcl::PointCloud::Ptr points4(new pcl::PointCloud); + + int num_points_per_side = 6; + + points1->points.reserve(num_points_per_side); + points2->points.reserve(num_points_per_side); + points3->points.reserve(num_points_per_side); + points4->points.reserve(num_points_per_side); + + double min_t = 0.1; + double max_t = 0.9; + + for(int i = 0; i < num_points_per_side; i++) { + + double t = min_t + (max_t - min_t) * (1.0 * i) / num_points_per_side; + + double tx = t; + double ty = 1.0 - t; + + points1->points.push_back(pcl::PointXYZ( tx, ty, 0.0)); + points2->points.push_back(pcl::PointXYZ( tx, -ty, 0.0)); + points3->points.push_back(pcl::PointXYZ( -tx, -ty, 0.0)); + points4->points.push_back(pcl::PointXYZ( -tx ,ty, 0.0)); + } + + estimator.setInputPoints(points1, points2, points3, points4); + + estimator.estimate(); + + Eigen::Vector4d c = estimator.getCCoefficients(); + Eigen::Vector2d n = estimator.getNCoefficients(); + + int inliers = 0; + std::vector inliers_vector; + double error = estimator.getModelErrorAndInliers(n, c, inliers, + inliers_vector, inliers_vector, inliers_vector, inliers_vector, false); + + ASSERT_NEAR(0, error, 1e-3); + ASSERT_EQ(4*num_points_per_side, inliers); + + double x = 0.5 * std::sqrt(2); + + ASSERT_NEAR(x, n(0), 1e-3); + ASSERT_NEAR(x, n(1), 1e-3); + + ASSERT_NEAR(-x, c(0), 1e-3); + ASSERT_NEAR(x, c(1), 1e-3); + ASSERT_NEAR(x, c(2), 1e-3); + ASSERT_NEAR(-x, c(3), 1e-3); + + std::vector corners = estimator.getCorners(); + ASSERT_EQ(4, corners.size()); + + ASSERT_NEAR(1.0, corners[0].x(), 1e-3); + ASSERT_NEAR(0.0, corners[0].y(), 1e-3); + + ASSERT_NEAR(0.0, corners[1].x(), 1e-3); + ASSERT_NEAR(-1.0, corners[1].y(), 1e-3); + + ASSERT_NEAR(-1.0, corners[2].x(), 1e-3); + ASSERT_NEAR(0.0, corners[2].y(), 1e-3); + + ASSERT_NEAR(0.0, corners[3].x(), 1e-3); + ASSERT_NEAR(1.0, corners[3].y(), 1e-3); +} + +TEST(lidartag, rectangle_ransac_estimation) +{ + RectangleEstimator estimator; + + pcl::PointCloud::Ptr points1(new pcl::PointCloud); + pcl::PointCloud::Ptr points2(new pcl::PointCloud); + pcl::PointCloud::Ptr points3(new pcl::PointCloud); + pcl::PointCloud::Ptr points4(new pcl::PointCloud); + + int num_points_per_side = 6; + + points1->points.reserve(num_points_per_side); + points2->points.reserve(num_points_per_side); + points3->points.reserve(num_points_per_side); + points4->points.reserve(num_points_per_side); + + double min_t = 0.1; + double max_t = 0.9; + + for(int i = 0; i < num_points_per_side; i++) { + + double t = min_t + (max_t - min_t) * (1.0 * i) / num_points_per_side; + + double tx = t; + double ty = 1.0 - t; + + points1->points.push_back(pcl::PointXYZ( tx, ty, 0.0)); + points2->points.push_back(pcl::PointXYZ( tx, -ty, 0.0)); + points3->points.push_back(pcl::PointXYZ( -tx, -ty, 0.0)); + points4->points.push_back(pcl::PointXYZ( -tx ,ty, 0.0)); + } + + points1->points.push_back(pcl::PointXYZ(10, 10, 0.0)); + points2->points.push_back(pcl::PointXYZ(-10, 10, 0.0)); + points3->points.push_back(pcl::PointXYZ(10, -10, 0.0)); + points4->points.push_back(pcl::PointXYZ(-10, -10, 0.0)); + + ASSERT_EQ(7, points1->size()); + ASSERT_EQ(7, points2->size()); + ASSERT_EQ(7, points3->size()); + ASSERT_EQ(7, points4->size()); + + estimator.setInlierError(0.05); + estimator.setMaxIterations(100); + estimator.setRANSAC(true); + + estimator.setInputPoints(points1, points2, points3, points4); + + estimator.estimate(); + + Eigen::Vector4d c = estimator.getCCoefficients(); + Eigen::Vector2d n = estimator.getNCoefficients(); + + double x = 0.5 * std::sqrt(2); + + Eigen::Vector2d n_test(x,x); + Eigen::Vector4d c_test(-x, x, x, -x); + + int inliers = 0; + std::vector inliers_vector; + double error = estimator.getModelErrorAndInliers(n_test, c_test, inliers, + inliers_vector, inliers_vector, inliers_vector, inliers_vector, false); + + ASSERT_NEAR(x, n(0), 1e-3); + ASSERT_NEAR(x, n(1), 1e-3); + + ASSERT_NEAR(-x, c(0), 1e-3); + ASSERT_NEAR(x, c(1), 1e-3); + ASSERT_NEAR(x, c(2), 1e-3); + ASSERT_NEAR(-x, c(3), 1e-3); + + std::vector corners = estimator.getCorners(); + ASSERT_EQ(4, corners.size()); + + ASSERT_NEAR(1.0, corners[0].x(), 1e-3); + ASSERT_NEAR(0.0, corners[0].y(), 1e-3); + + ASSERT_NEAR(0.0, corners[1].x(), 1e-3); + ASSERT_NEAR(-1.0, corners[1].y(), 1e-3); + + ASSERT_NEAR(-1.0, corners[2].x(), 1e-3); + ASSERT_NEAR(0.0, corners[2].y(), 1e-3); + + ASSERT_NEAR(0.0, corners[3].x(), 1e-3); + ASSERT_NEAR(1.0, corners[3].y(), 1e-3); +}