diff --git a/README.md b/README.md index d92d2cd..8beb606 100644 --- a/README.md +++ b/README.md @@ -43,10 +43,6 @@ It's an overall updated version of **R-GPF of ERASOR** [**[Code](https://github. ### Characteristics -* Single hpp file (`include/patchwork/patchwork.hpp`) - -* Robust ground consistency - As shown in the demo videos, our method shows the most promising robust performance compared with other state-of-the-art methods, especially, our method focuses on the little perturbation of precision/recall as shown in [this figure](img/seq_00_pr_zoom.pdf). Please kindly note that the concept of *traversable area* and *ground* is quite different! Please refer to our paper. diff --git a/include/patchwork/patchwork.hpp b/include/patchwork/patchwork.hpp index 38208d8..2aac440 100755 --- a/include/patchwork/patchwork.hpp +++ b/include/patchwork/patchwork.hpp @@ -11,14 +11,15 @@ #include #include +#include "zone_models.hpp" #include #include // vector cout operator -template -std::ostream& operator<<(std::ostream& os, const std::vector& vec) { +template +std::ostream &operator<<(std::ostream &os, const std::vector &vec) { os << "[ "; - for (const auto& element : vec) { + for (const auto &element : vec) { os << element << " "; } os << "]"; @@ -27,16 +28,20 @@ std::ostream& operator<<(std::ostream& os, const std::vector& vec) { // conditional parameter loading function for unorthodox (and normal) parameter locations template -bool condParam(ros::NodeHandle* nh, const std::string& param_name, T& param_val, const T& default_val, const std::string& prefix = "/patchwork") { +bool condParam(ros::NodeHandle *nh, + const std::string ¶m_name, + T ¶m_val, + const T &default_val, + const std::string &prefix = "/patchwork") { if (nh->hasParam(prefix + "/" + param_name)) { if (nh->getParam(prefix + "/" + param_name, param_val)) { ROS_INFO_STREAM("param '" << prefix << "/" << param_name << "' -> '" << param_val << "'"); return true; } - } - else if (nh->hasParam(ros::this_node::getName() + "/" + param_name)) { + } else if (nh->hasParam(ros::this_node::getName() + "/" + param_name)) { if (nh->getParam(ros::this_node::getName() + "/" + param_name, param_val)) { - ROS_INFO_STREAM("param '" << ros::this_node::getName() << "/" << param_name << "' -> '" << param_val << "'"); + ROS_INFO_STREAM( + "param '" << ros::this_node::getName() << "/" << param_name << "' -> '" << param_val << "'"); return true; } } @@ -55,12 +60,12 @@ bool condParam(ros::NodeHandle* nh, const std::string& param_name, T& param_val, #define COLOR_RED 1.0 // red #define COLOR_GLOBALLY_TOO_HIGH_ELEVATION 0.8 // I'm not sure...haha -int NOT_ASSIGNED = -2; -int FEW_POINTS = -1; -int UPRIGHT_ENOUGH = 0; // cyan -int FLAT_ENOUGH = 1; // green -int TOO_HIGH_ELEVATION = 2; // blue -int TOO_TILTED = 3; // red +int NOT_ASSIGNED = -2; +int FEW_POINTS = -1; +int UPRIGHT_ENOUGH = 0; // cyan +int FLAT_ENOUGH = 1; // green +int TOO_HIGH_ELEVATION = 2; // blue +int TOO_TILTED = 3; // red int GLOBALLY_TOO_HIGH_ELEVATION = 4; std::vector COLOR_MAP = {COLOR_CYAN, COLOR_GREEN, COLOR_BLUE, COLOR_RED, COLOR_GLOBALLY_TOO_HIGH_ELEVATION}; @@ -78,49 +83,67 @@ template bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; } -struct PatchIdx -{ - int zone_idx_; - int ring_idx_; - int sector_idx_; - int concentric_idx_; -}; struct PCAFeature { Eigen::Vector3f principal_; Eigen::Vector3f normal_; Eigen::Vector3f singular_values_; Eigen::Vector3f mean_; - float d_; - float th_dist_d_; - float linearity_; - float planarity_; + float d_; + float th_dist_d_; + float linearity_; + float planarity_; +}; + +template +struct Patch { + bool is_close_to_origin_ = false; // If so, we can set threshold more conservatively + int ring_idx_ = NOT_ASSIGNED; + int sector_idx_ = NOT_ASSIGNED; + + int status_ = NOT_ASSIGNED; + + PCAFeature feature_; + + pcl::PointCloud cloud_; + pcl::PointCloud ground_; + pcl::PointCloud non_ground_; + + void clear() { + if (!cloud_.empty()) cloud_.clear(); + if (!ground_.empty()) ground_.clear(); + if (!non_ground_.empty()) non_ground_.clear(); + } }; template class PatchWork { -public: + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef std::vector > Ring; - typedef std::vector Zone; + typedef std::vector> Ring; + typedef std::vector RegionwisePatches; PatchWork() {}; std::string frame_patchwork = "map"; template - bool condParam(ros::NodeHandle* nh, const std::string& param_name, T& param_val, const T& default_val, const std::string& prefix = "/patchwork") const { + bool condParam(ros::NodeHandle *nh, + const std::string ¶m_name, + T ¶m_val, + const T &default_val, + const std::string &prefix = "/patchwork") const { if (nh->hasParam(prefix + "/" + param_name)) { if (nh->getParam(prefix + "/" + param_name, param_val)) { ROS_INFO_STREAM("param '" << prefix << "/" << param_name << "' -> '" << param_val << "'"); return true; } - } - else if (nh->hasParam(ros::this_node::getName() + "/" + param_name)) { + } else if (nh->hasParam(ros::this_node::getName() + "/" + param_name)) { if (nh->getParam(ros::this_node::getName() + "/" + param_name, param_val)) { - ROS_INFO_STREAM("param '" << ros::this_node::getName() << "/" << param_name << "' -> '" << param_val << "'"); + ROS_INFO_STREAM( + "param '" << ros::this_node::getName() << "/" << param_name << "' -> '" << param_val << "'"); return true; } } @@ -151,7 +174,8 @@ class PatchWork { condParam(nh, "uniform/num_rings", num_rings_, 30); condParam(nh, "uniform/num_sectors", num_sectors_, 108); condParam(nh, "uprightness_thr", uprightness_thr_, 0.5); // The larger, the more strict - condParam(nh, "adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); // The more larger, the more soft + // The larger, the more soft + condParam(nh, "adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); // It is not in the paper // It is also not matched our philosophy, but it is employed to reject some FPs easily & intuitively. @@ -160,7 +184,8 @@ class PatchWork { condParam(nh, "global_elevation_threshold", global_elevation_thr_, 0.0); if (using_global_thr_) { - cout << "\033[1;33m[Warning] Global elevation threshold is turned on :" << global_elevation_thr_ << "\033[0m" << endl; + cout << "\033[1;33m[Warning] Global elevation threshold is turned on :" << global_elevation_thr_ + << "\033[0m" << endl; } else { cout << "Global thr. is not in use" << endl; } ROS_INFO("Sensor Height: %f", sensor_height_); @@ -171,26 +196,22 @@ class PatchWork { ROS_INFO("Distance Threshold: %f", th_dist_); ROS_INFO("Max. range:: %f", max_range_); ROS_INFO("Min. range:: %f", min_range_); - ROS_INFO("Num. rings: %d", num_rings_); - ROS_INFO("Num. sectors: %d", num_sectors_); ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); // CZM denotes 'Concentric Zone Model'. Please refer to our paper - condParam(nh, "czm/num_zones", num_zones_, 4); - condParam(nh, "czm/num_sectors_each_zone", num_sectors_each_zone_, {16, 32, 54, 32}); - condParam(nh, "czm/num_rings_each_zone", num_rings_each_zone_, {2, 4, 4, 4}); - condParam(nh, "czm/min_ranges_each_zone", min_ranges_, {2.7, 12.3625, 22.025, 41.35}); + // 2024.07.28. I feel `num_zones_`, `num_sectors_each_zone_`, num_rings_each_zone_` are rarely fine-tuned. + // So I've decided to provide predefined parameter sets for sensor types + condParam(nh, "sensor_model", sensor_model_, std::string("HDL-64E")); + zone_model_ = ConcentricZoneModel(sensor_model_, sensor_height_, min_range_, max_range_); condParam(nh, "czm/elevation_thresholds", elevation_thr_, {0.523, 0.746, 0.879, 1.125}); condParam(nh, "czm/flatness_thresholds", flatness_thr_, {0.0005, 0.000725, 0.001, 0.001}); ROS_INFO("\033[1;32mUprightness\33[0m threshold: %f", uprightness_thr_); - ROS_INFO("\033[1;32mElevation\33[0m thresholds: %f %f %f %f", elevation_thr_[0],elevation_thr_[1], elevation_thr_[2], elevation_thr_[3]); - ROS_INFO("\033[1;32mFlatness\033[0m thresholds: %f %f %f %f", flatness_thr_[0], flatness_thr_[1], flatness_thr_[2], flatness_thr_[3]); - - ROS_INFO("Num. zones: %d", num_zones_); - - check_input_parameters_are_correct(); -// cout_params(); + ROS_INFO("\033[1;32mElevation\33[0m thresholds: %f %f %f %f", + elevation_thr_[0], elevation_thr_[1], elevation_thr_[2], elevation_thr_[3]); + ROS_INFO("\033[1;32mFlatness\033[0m thresholds: %f %f %f %f", + flatness_thr_[0], flatness_thr_[1], flatness_thr_[2], flatness_thr_[3]); + ROS_INFO("Num. zones: %zu", zone_model_.num_zones_); // It equals to elevation_thr_.size()/flatness_thr_.size(); num_rings_of_interest_ = elevation_thr_.size(); @@ -203,74 +224,39 @@ class PatchWork { reverted_points_by_flatness_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - PlanePub = nh->advertise("/gpf/plane", 100); + PlanePub = nh->advertise("/gpf/plane", 100); RevertedCloudPub = nh->advertise("/revert_pc", 100); RejectedCloudPub = nh->advertise("/reject_pc", 100); - min_range_z2_ = min_ranges_[1]; - min_range_z3_ = min_ranges_[2]; - min_range_z4_ = min_ranges_[3]; - - min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; - ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), - (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), - (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), - (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; + const auto &num_sectors_each_zone_ = zone_model_.sensor_config_.num_sectors_for_each_zone_; sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), 2 * M_PI / num_sectors_each_zone_.at(2), 2 * M_PI / num_sectors_each_zone_.at(3)}; cout << "INITIALIZATION COMPLETE" << endl; - for (int iter = 0; iter < num_zones_; ++iter) { - Zone z; - initialize_zone(z, num_sectors_each_zone_.at(iter), num_rings_each_zone_.at(iter)); - ConcentricZoneModel_.push_back(z); - } - PatchIdx patch_idx; - int concentric_idx_tmp = 0; - for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) { - for (uint16_t ring_idx = 0; ring_idx < num_rings_each_zone_[zone_idx]; ++ring_idx) { - for (uint16_t sector_idx = 0; sector_idx < num_sectors_each_zone_[zone_idx]; ++sector_idx) { - patch_idx.zone_idx_ = zone_idx; - patch_idx.ring_idx_ = ring_idx; - patch_idx.sector_idx_ = sector_idx; - patch_idx.concentric_idx_ = concentric_idx_tmp; - patch_indices_.emplace_back(patch_idx); - } - concentric_idx_tmp++; - } - } - - int num_patches = patch_indices_.size(); - indices_.resize(num_patches); - std::iota(indices_.begin(), indices_.end(), 0); - statuses_.assign(num_patches, NOT_ASSIGNED); - features_.resize(num_patches); - regionwise_grounds_.resize(num_patches); - regionwise_nongrounds_.resize(num_patches); + initialize(regionwise_patches_, zone_model_); } void estimate_ground( - const pcl::PointCloud &cloud_in, - pcl::PointCloud &ground, - pcl::PointCloud &nonground, - double &time_taken); + const pcl::PointCloud &cloud_in, + pcl::PointCloud &ground, + pcl::PointCloud &nonground, + double &time_taken); geometry_msgs::PolygonStamped set_plane_polygon(const MatrixXf &normal_v, const float &d); -private: + private: // For ATAT (All-Terrain Automatic heighT estimator) - bool ATAT_ON_; + bool ATAT_ON_; double noise_bound_; double max_r_for_ATAT_; - int num_sectors_for_ATAT_; + int num_sectors_for_ATAT_; int num_iter_; int num_lpr_; int num_min_pts_; int num_rings_; int num_sectors_; - int num_zones_; int num_rings_of_interest_; double sensor_height_; @@ -280,9 +266,6 @@ class PatchWork { double min_range_; double uprightness_thr_; double adaptive_seed_selection_margin_; - double min_range_z2_; // 12.3625 - double min_range_z3_; // 22.025 - double min_range_z4_; // 41.35 bool verbose_; bool initialized_ = true; @@ -291,20 +274,12 @@ class PatchWork { bool using_global_thr_; double global_elevation_thr_; - double ring_size; - double sector_size; // For visualization - bool visualize_; + bool visualize_; - vector indices_; - vector statuses_; - vector patch_indices_; - vector features_; - vector> regionwise_grounds_; - vector> regionwise_nongrounds_; - - vector num_sectors_each_zone_; - vector num_rings_each_zone_; + std::string sensor_model_; + vector> patch_indices_; // For multi-threading. {ring_idx, sector_idx} + ConcentricZoneModel zone_model_; vector sector_sizes_; vector ring_sizes_; @@ -312,88 +287,94 @@ class PatchWork { vector elevation_thr_; vector flatness_thr_; - vector ConcentricZoneModel_; + RegionwisePatches regionwise_patches_; jsk_recognition_msgs::PolygonArray poly_list_; ros::Publisher PlanePub, RevertedCloudPub, RejectedCloudPub; pcl::PointCloud reverted_points_by_flatness_, rejected_points_by_elevation_; + void initialize(RegionwisePatches &patches, const ConcentricZoneModel &zone_model); - void initialize_zone(Zone &z, int num_sectors, int num_rings); - - void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings); + void flush_patches(RegionwisePatches &patches); double xy2theta(const double &x, const double &y); double xy2radius(const double &x, const double &y); - void pc2czm(const pcl::PointCloud &src, std::vector &czm); + void pc2regionwise_patches(const pcl::PointCloud &src, RegionwisePatches &patches); - void estimate_plane_(const pcl::PointCloud &ground, PCAFeature& feat); + void estimate_plane_(const pcl::PointCloud &ground, PCAFeature &feat); - void extract_piecewiseground( - const int zone_idx, const pcl::PointCloud &src, - PCAFeature &feat, - pcl::PointCloud ®ionwise_ground, - pcl::PointCloud ®ionwise_nonground, - bool is_h_available=true); + void perform_regionwise_ground_segmentation(Patch &patch, const bool is_h_available = true); - double consensus_set_based_height_estimation(const Eigen::RowVectorXd& X, - const Eigen::RowVectorXd& ranges, - const Eigen::RowVectorXd& weights); + double consensus_set_based_height_estimation(const Eigen::RowVectorXd &X, + const Eigen::RowVectorXd &ranges, + const Eigen::RowVectorXd &weights); void estimate_sensor_height(pcl::PointCloud cloud_in); - void extract_initial_seeds_( - const int zone_idx, const pcl::PointCloud &p_sorted, - pcl::PointCloud &init_seeds, bool is_h_available=true); - - void check_input_parameters_are_correct(); - - void cout_params(); + void extract_initial_seeds_(const pcl::PointCloud &p_sorted, pcl::PointCloud &init_seeds, + bool is_close_to_origin, bool is_h_available); /*** * For visulization of Ground Likelihood Estimation */ - geometry_msgs::PolygonStamped set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split); + geometry_msgs::PolygonStamped set_polygons(int ring_idx, int sector_idx, int num_split); int determine_ground_likelihood_estimation_status( - const int concentric_idx, - const double z_vec, - const double z_elevation, - const double surface_variable); + const int concentric_idx, + const double z_vec, + const double z_elevation, + const double surface_variable); }; - template inline -void PatchWork::initialize_zone(Zone &z, int num_sectors, int num_rings) { - z.clear(); - pcl::PointCloud cloud; - cloud.reserve(1000); - Ring ring; - for (int i = 0; i < num_sectors; i++) { - ring.emplace_back(cloud); - } - for (int j = 0; j < num_rings; j++) { - z.emplace_back(ring); +void PatchWork::initialize(RegionwisePatches &patches, const ConcentricZoneModel &zone_model) { + patches.clear(); + patch_indices_.clear(); + Patch patch; + + // Reserve memory in advance to boost speed + patch.cloud_.reserve(1000); + patch.ground_.reserve(1000); + patch.non_ground_.reserve(1000); + + // ToDo. Make it more general + // In polar coordinates, `num_columns` are `num_sectors` and `num_rows` are `num_rings`, respectively + int num_rows = zone_model_.num_total_rings_; + const auto &num_sectors_per_ring = zone_model.num_sectors_per_ring_; + + for (int j = 0; j < num_rows; j++) { + Ring ring; + patch.ring_idx_ = j; + patch.is_close_to_origin_ = j < zone_model.max_ring_index_in_first_zone; + for (int i = 0; i < num_sectors_per_ring[j]; i++) { + patch.sector_idx_ = i; + ring.emplace_back(patch); + + patch_indices_.emplace_back(j, i); + } + patches.emplace_back(ring); } } template inline -void PatchWork::flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings) { - for (int i = 0; i < num_sectors; i++) { - for (int j = 0; j < num_rings; j++) { - if (!patches[j][i].points.empty()) patches[j][i].points.clear(); +void PatchWork::flush_patches(RegionwisePatches &patches) { + int num_rows = patches.size(); + for (int j = 0; j < num_rows; j++) { + int num_columns = patches[j].size(); + for (int i = 0; i < num_columns; i++) { + patches[j][i].clear(); } } } template inline -void PatchWork::estimate_plane_(const pcl::PointCloud &ground, PCAFeature& feat) { +void PatchWork::estimate_plane_(const pcl::PointCloud &ground, PCAFeature &feat) { Eigen::Vector4f pc_mean; Eigen::Matrix3f cov; pcl::computeMeanAndCovarianceMatrix(ground, cov, pc_mean); @@ -402,26 +383,26 @@ void PatchWork::estimate_plane_(const pcl::PointCloud &ground, P Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); feat.singular_values_ = svd.singularValues(); - feat.linearity_ = ( feat.singular_values_(0) - feat.singular_values_(1) ) / feat.singular_values_(0); - feat.planarity_ = ( feat.singular_values_(1) - feat.singular_values_(2) ) / feat.singular_values_(0); + feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); + feat.normal_ = (svd.matrixU().col(2)); if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive feat.normal_ = -feat.normal_; } // mean ground seeds value - feat.mean_ = pc_mean.head<3>(); + feat.mean_ = pc_mean.head<3>(); // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); feat.th_dist_d_ = th_dist_ - feat.d_; } template inline -void PatchWork::extract_initial_seeds_( - const int zone_idx, const pcl::PointCloud &p_sorted, - pcl::PointCloud &init_seeds, bool is_h_available) { +void PatchWork::extract_initial_seeds_(const pcl::PointCloud &p_sorted, + pcl::PointCloud &init_seeds, + bool is_close_to_origin, bool is_h_available) { init_seeds.points.clear(); // LPR is the mean of low point representative @@ -432,8 +413,8 @@ void PatchWork::extract_initial_seeds_( // Empirically, adaptive seed selection applying to Z1 is fine if (is_h_available) { static double lowest_h_margin_in_close_zone = (sensor_height_ == 0.0) ? -0.1 : adaptive_seed_selection_margin_ * - sensor_height_; - if (zone_idx == 0) { + sensor_height_; + if (is_close_to_origin) { for (size_t i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lowest_h_margin_in_close_zone) { ++init_idx; @@ -445,11 +426,11 @@ void PatchWork::extract_initial_seeds_( } // Calculate the mean height value. - for (size_t i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { + for (size_t i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } - double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 + double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ for (size_t i = 0; i < p_sorted.points.size(); i++) { @@ -458,41 +439,42 @@ void PatchWork::extract_initial_seeds_( } } } + template inline -double PatchWork::consensus_set_based_height_estimation(const Eigen::RowVectorXd& X, - const Eigen::RowVectorXd& ranges, - const Eigen::RowVectorXd& weights) { +double PatchWork::consensus_set_based_height_estimation(const Eigen::RowVectorXd &X, + const Eigen::RowVectorXd &ranges, + const Eigen::RowVectorXd &weights) { // check input parameters - dimension inconsistent test - assert(!( (X.rows() != ranges.rows()) || (X.cols() != ranges.cols()) )); + assert(!((X.rows() != ranges.rows()) || (X.cols() != ranges.cols()))); // check input parameters - only one element test - assert(!( (X.rows() == 1) && (X.cols() == 1) )); // TODO: admit a trivial solution + assert(!((X.rows() == 1) && (X.cols() == 1))); // TODO: admit a trivial solution - int N = X.cols(); + int N = X.cols(); std::vector> h; - for (size_t i= 0; (int)i < N ;++i){ - h.push_back(std::make_pair(X(i) - ranges(i), i+1)); - h.push_back(std::make_pair(X(i) + ranges(i), -i-1)); + for (size_t i = 0; (int) i < N; ++i) { + h.push_back(std::make_pair(X(i) - ranges(i), i + 1)); + h.push_back(std::make_pair(X(i) + ranges(i), -i - 1)); } // ascending order std::sort(h.begin(), h.end(), [](std::pair a, std::pair b) { return a.first < b.first; }); - int nr_centers = 2 * N; - Eigen::RowVectorXd x_hat = Eigen::MatrixXd::Zero(1, nr_centers); - Eigen::RowVectorXd x_cost = Eigen::MatrixXd::Zero(1, nr_centers); + int nr_centers = 2 * N; + Eigen::RowVectorXd x_hat = Eigen::MatrixXd::Zero(1, nr_centers); + Eigen::RowVectorXd x_cost = Eigen::MatrixXd::Zero(1, nr_centers); - double ranges_inverse_sum = ranges.sum(); - double dot_X_weights = 0; - double dot_weights_consensus = 0; - int consensus_set_cardinal = 0; - double sum_xi = 0; - double sum_xi_square = 0; + double ranges_inverse_sum = ranges.sum(); + double dot_X_weights = 0; + double dot_weights_consensus = 0; + int consensus_set_cardinal = 0; + double sum_xi = 0; + double sum_xi_square = 0; - for (size_t i = 0 ; (int)i < nr_centers ; ++i){ + for (size_t i = 0; (int) i < nr_centers; ++i) { - int idx = int(std::abs(h.at(i).second)) - 1; // Indices starting at 1 + int idx = int(std::abs(h.at(i).second)) - 1; // Indices starting at 1 int epsilon = (h.at(i).second > 0) ? 1 : -1; consensus_set_cardinal += epsilon; @@ -504,7 +486,7 @@ double PatchWork::consensus_set_based_height_estimation(const Eigen::Row x_hat(i) = dot_X_weights / dot_weights_consensus; - double residual = consensus_set_cardinal * x_hat(i) * x_hat(i) + sum_xi_square - 2 * sum_xi * x_hat(i); + double residual = consensus_set_cardinal * x_hat(i) * x_hat(i) + sum_xi_square - 2 * sum_xi * x_hat(i); x_cost(i) = residual + ranges_inverse_sum; } @@ -519,7 +501,7 @@ template inline void PatchWork::estimate_sensor_height(pcl::PointCloud cloud_in) { // ATAT: All-Terrain Automatic HeighT estimator - Ring ring_for_ATAT(num_sectors_for_ATAT_); + Ring ring_for_ATAT(num_sectors_for_ATAT_); for (auto const &pt : cloud_in.points) { int sector_idx; double r = xy2radius(pt.x, pt.y); @@ -530,7 +512,7 @@ void PatchWork::estimate_sensor_height(pcl::PointCloud cloud_in) double theta = xy2theta(pt.x, pt.y); sector_idx = min(static_cast((theta / sector_size_for_ATAT)), num_sectors_for_ATAT_); - ring_for_ATAT[sector_idx].points.emplace_back(pt); + ring_for_ATAT[sector_idx].cloud_.points.emplace_back(pt); } } @@ -538,14 +520,14 @@ void PatchWork::estimate_sensor_height(pcl::PointCloud cloud_in) vector ground_elevations_wrt_the_origin; vector linearities; vector planarities; - for (int i = 0; i < num_sectors_for_ATAT_; ++i) { - - if((int)ring_for_ATAT[i].size() < num_min_pts_ ){ continue; } + for (int i = 0; i < num_sectors_for_ATAT_; ++i) { + if ((int) ring_for_ATAT[i].cloud_.size() < num_min_pts_) { continue; } pcl::PointCloud dummy_est_ground; pcl::PointCloud dummy_est_non_ground; - PCAFeature feat; - extract_piecewiseground(0, ring_for_ATAT[i], feat, dummy_est_ground, dummy_est_non_ground, false); + perform_regionwise_ground_segmentation(ring_for_ATAT[i], false); + + const auto & feat = ring_for_ATAT[i].feature_; const double ground_z_vec = abs(feat.normal_(2)); const double ground_z_elevation = feat.mean_(2); @@ -559,22 +541,26 @@ void PatchWork::estimate_sensor_height(pcl::PointCloud cloud_in) } // Setting for consensus set-based height estimation + // It is equal to component-wise translation estimation (COTE) in TEASER++ int N = ground_elevations_wrt_the_origin.size(); - cout << "\033[1;33m[ATAT] N: " << N << " -> " << ground_elevations_wrt_the_origin.size() <<"\033[0m" << endl; + cout << "\033[1;33m[ATAT] N: " << N << " -> " << ground_elevations_wrt_the_origin.size() << "\033[0m" << endl; if (ground_elevations_wrt_the_origin.size() == 0) { - throw invalid_argument("No valid ground points for ATAT! Please check the input data and `max_r_for_ATAT`"); + throw invalid_argument("No valid ground points for ATAT! Please check the input data and `max_r_for_ATAT`"); } - Eigen::Matrix values = Eigen::MatrixXd::Ones(1, N); - Eigen::Matrix ranges = noise_bound_ * Eigen::MatrixXd::Ones(1, N); - Eigen::Matrix weights = 1.0 / (noise_bound_ * noise_bound_) * Eigen::MatrixXd::Ones(1, N); - for (int i = 0; i < N; ++i) { - values(0, i) = ground_elevations_wrt_the_origin[i]; - ranges(0, i) = ranges(0, i) * linearities[i]; + Eigen::Matrix values = Eigen::MatrixXd::Ones(1, N); + Eigen::Matrix ranges = noise_bound_ * Eigen::MatrixXd::Ones(1, N); + Eigen::Matrix weights = + 1.0 / (noise_bound_ * noise_bound_) * Eigen::MatrixXd::Ones(1, N); + for (int i = 0; i < N; ++i) { + values(0, i) = ground_elevations_wrt_the_origin[i]; + ranges(0, i) = ranges(0, i) * linearities[i]; weights(0, i) = weights(0, i) * planarities[i] * planarities[i]; } double estimated_h = consensus_set_based_height_estimation(values, ranges, weights); - cout << "\033[1;33m[ATAT] The sensor height is auto-calibrated via the ground points in the vicinity of the vehicle\033[0m" << endl; + cout + << "\033[1;33m[ATAT] The sensor height is auto-calibrated via the ground points in the vicinity of the vehicle\033[0m" + << endl; cout << "\033[1;33m[ATAT] Elevation of the ground w.r.t. the origin is " << estimated_h << " m\033[0m" << endl; // Note that these are opposites @@ -584,10 +570,8 @@ void PatchWork::estimate_sensor_height(pcl::PointCloud cloud_in) template inline void PatchWork::estimate_ground( - const pcl::PointCloud &cloud_in, - pcl::PointCloud &ground, - pcl::PointCloud &nonground, - double &time_taken) { + const pcl::PointCloud &cloud_in, pcl::PointCloud &ground, + pcl::PointCloud &nonground, double &time_taken) { // Just for visualization poly_list_.header.stamp = ros::Time::now(); @@ -597,14 +581,15 @@ void PatchWork::estimate_ground( if (initialized_ && ATAT_ON_) { estimate_sensor_height(cloud_in); initialized_ = false; + std::cout << "\033[1;32mComplete to estimate the sensor height: " << sensor_height_ << "\033[0m" << std::endl; } - static double start, end; + static double start, end; // static double start, t0, t1, t2; // double t_total_ground = 0.0; double t_total_estimate = 0.0; // 1.Msg to pointcloud - pcl::PointCloud cloud_in_tmp = cloud_in; + pcl::PointCloud cloud_in_tmp = cloud_in; start = ros::Time::now().toSec(); @@ -623,12 +608,9 @@ void PatchWork::estimate_ground( } // t1 = ros::Time::now().toSec(); + flush_patches(regionwise_patches_); - for (int k = 0; k < num_zones_; ++k) { - flush_patches_in_zone(ConcentricZoneModel_[k], num_sectors_each_zone_[k], num_rings_each_zone_[k]); - } - - pc2czm(cloud_in_tmp, ConcentricZoneModel_); + pc2regionwise_patches(cloud_in_tmp, regionwise_patches_); ground.clear(); nonground.clear(); @@ -640,63 +622,55 @@ void PatchWork::estimate_ground( // HT comments: TBB w/ blocked_range was faster in my desktop // 117 Hz vs 126 Hz // tbb::parallel_for(0, num_patches, [&](int i) { - tbb::parallel_for(tbb::blocked_range(0, num_patches), - [&](tbb::blocked_range r) { - for (int i = r.begin(); i < r.end(); ++i) { - const auto &patch_idx = patch_indices_[i]; - const int zone_idx = patch_idx.zone_idx_; - const int ring_idx = patch_idx.ring_idx_; - const int sector_idx = patch_idx.sector_idx_; - const int concentric_idx = patch_idx.concentric_idx_; - - auto &patch = ConcentricZoneModel_[zone_idx][ring_idx][sector_idx]; - - auto &feat = features_[i]; - auto ®ionwise_ground = regionwise_grounds_[i]; - auto ®ionwise_nonground = regionwise_nongrounds_[i]; - auto &status = statuses_[i]; - - if ((int)patch.points.size() > num_min_pts_) { - // 22.05.02 update + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { + for (int k = r.begin(); k < r.end(); ++k) { + const auto &[ring_idx, sector_idx] = patch_indices_[k]; + auto &patch = regionwise_patches_[ring_idx][sector_idx]; + + if ((int) patch.cloud_.points.size() > num_min_pts_) { + // 2022.05.02 update // Region-wise sorting is adopted, which is much faster than global sorting! - sort(patch.points.begin(), patch.points.end(), point_z_cmp); - extract_piecewiseground(zone_idx, patch, feat, regionwise_ground, regionwise_nonground); + sort(patch.cloud_.points.begin(), patch.cloud_.points.end(), point_z_cmp); + perform_regionwise_ground_segmentation(patch); + + const auto &feat = patch.feature_; const double ground_z_vec = abs(feat.normal_(2)); const double ground_z_elevation = feat.mean_(2); const double surface_variable = feat.singular_values_.minCoeff() / - (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + (feat.singular_values_(0) + feat.singular_values_(1) + + feat.singular_values_(2)); - status = determine_ground_likelihood_estimation_status(concentric_idx, ground_z_vec, - ground_z_elevation, surface_variable); + patch.status_ = determine_ground_likelihood_estimation_status(ring_idx, + ground_z_vec, + ground_z_elevation, + surface_variable); } else { // Why? Because it is better to reject noise points // That is, these noise points sometimes lead to mis-recognition or wrong clustering // Thus, in practice, just rejecting them is better than using them // But note that this may degrade quantitative ground segmentation performance - regionwise_ground = patch; - regionwise_nonground.clear(); - status = FEW_POINTS; + patch.ground_ = patch.cloud_; + patch.non_ground_.clear(); + patch.status_ = FEW_POINTS; } } }); - std::for_each(indices_.begin(), indices_.end(), [&](const int &i) { - const auto &patch_idx = patch_indices_[i]; - const int zone_idx = patch_idx.zone_idx_; - const int ring_idx = patch_idx.ring_idx_; - const int sector_idx = patch_idx.sector_idx_; - const int concentric_idx = patch_idx.concentric_idx_; + std::for_each(patch_indices_.begin(), patch_indices_.end(), [&](const std::pair &index_pair) { + int ring_idx = index_pair.first; + int sector_idx = index_pair.second; + + const auto &patch = regionwise_patches_[ring_idx][sector_idx]; - // const auto &patch = ConcentricZoneModel_[zone_idx][ring_idx][sector_idx]; - const auto &feat = features_[i]; - const auto ®ionwise_ground = regionwise_grounds_[i]; - const auto ®ionwise_nonground = regionwise_nongrounds_[i]; - const auto status = statuses_[i]; + const auto &feat = patch.feature_; + const auto ®ionwise_ground = patch.ground_; + const auto ®ionwise_nonground = patch.non_ground_; + const auto status = patch.status_; if (visualize_ && (status != FEW_POINTS && status != NOT_ASSIGNED)) { - auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3); + auto polygons = set_polygons(ring_idx, sector_idx, 3); polygons.header = poly_list_.header; poly_list_.polygons.emplace_back(polygons); poly_list_.likelihood.emplace_back(COLOR_MAP[status]); @@ -712,15 +686,15 @@ void PatchWork::estimate_ground( nonground += regionwise_nonground; } else if (status == GLOBALLY_TOO_HIGH_ELEVATION) { cout << "\033[1;33m[Global elevation] " << feat.mean_(2) << " > " << global_elevation_thr_ - << "\033[0m\n"; + << "\033[0m\n"; nonground += regionwise_ground; nonground += regionwise_nonground; } else if (status == TOO_HIGH_ELEVATION) { if (verbose_) { std::cout << "\033[1;34m[Elevation] Rejection operated. Check " - << concentric_idx - << "th param. of elevation_thr_: " << -sensor_height_ + elevation_thr_[concentric_idx] - << " < " << feat.mean_(2) << "\033[0m\n"; + << ring_idx + << "th param. of elevation_thr_: " << -sensor_height_ + elevation_thr_[ring_idx] + << " < " << feat.mean_(2) << "\033[0m\n"; rejected_points_by_elevation_ += regionwise_ground; } nonground += regionwise_ground; @@ -728,11 +702,12 @@ void PatchWork::estimate_ground( } else if (status == FLAT_ENOUGH) { if (verbose_) { std::cout << "\033[1;36m[Flatness] Recovery operated. Check " - << concentric_idx - << "th param. flatness_thr_: " << flatness_thr_[concentric_idx] + << ring_idx + << "th param. flatness_thr_: " << flatness_thr_[ring_idx] << " > " << feat.singular_values_.minCoeff() / - (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)) << "\033[0m\n"; + (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)) + << "\033[0m\n"; reverted_points_by_flatness_ += regionwise_ground; } ground += regionwise_ground; @@ -747,8 +722,8 @@ void PatchWork::estimate_ground( t_total_estimate += t_tmp3 - t_tmp2; }); - end = ros::Time::now().toSec(); - time_taken = end - start; + end = ros::Time::now().toSec(); + time_taken = end - start; // ofstream time_txt("/home/shapelim/patchwork_time_anal.txt", std::ios::app); // time_txt<::xy2theta(const double &x, const double &y) { // 0 ~ 2 return 2 * M_PI + atan2(y, x);// 3, 4 quadrant } */ - auto atan_value = atan2(y,x); // EDITED! - return atan_value > 0 ? atan_value : atan_value + 2*M_PI; // EDITED! + auto atan_value = atan2(y, x); // EDITED! + return atan_value > 0 ? atan_value : atan_value + 2 * M_PI; // EDITED! } template @@ -789,83 +764,54 @@ double PatchWork::xy2radius(const double &x, const double &y) { template inline -void PatchWork::pc2czm(const pcl::PointCloud &src, std::vector &czm) { +void PatchWork::pc2regionwise_patches(const pcl::PointCloud &src, RegionwisePatches &patches) { std::for_each(src.points.begin(), src.points.end(), [&](const auto &pt) { - int ring_idx, sector_idx; - double r = xy2radius(pt.x, pt.y); - if ((r <= max_range_) && (r > min_range_)) { - double theta = xy2theta(pt.x, pt.y); - - if (r < min_range_z2_) { // In First rings - ring_idx = - min(static_cast(((r - min_range_) / ring_sizes_[0])), num_rings_each_zone_[0] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[0])), num_sectors_each_zone_[0] - 1); - czm[0][ring_idx][sector_idx].points.emplace_back(pt); - } else if (r < min_range_z3_) { - ring_idx = - min(static_cast(((r - min_range_z2_) / ring_sizes_[1])), num_rings_each_zone_[1] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[1])), num_sectors_each_zone_[1] - 1); - czm[1][ring_idx][sector_idx].points.emplace_back(pt); - } else if (r < min_range_z4_) { - ring_idx = - min(static_cast(((r - min_range_z3_) / ring_sizes_[2])), num_rings_each_zone_[2] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[2])), num_sectors_each_zone_[2] - 1); - czm[2][ring_idx][sector_idx].points.emplace_back(pt); - } else { // Far! - ring_idx = - min(static_cast(((r - min_range_z4_) / ring_sizes_[3])), num_rings_each_zone_[3] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[3])), num_sectors_each_zone_[3] - 1); - czm[3][ring_idx][sector_idx].points.emplace_back(pt); - } + const auto &[ring_idx, sector_idx] = zone_model_.get_ring_sector_idx(pt.x, pt.y); + if (ring_idx != INVALID_RING_IDX && ring_idx != OVERFLOWED_IDX) { + patches[ring_idx][sector_idx].cloud_.points.emplace_back(pt); } -// } }); } // For adaptive template inline -void PatchWork::extract_piecewiseground( - const int zone_idx, const pcl::PointCloud &src, - PCAFeature &feat, - pcl::PointCloud ®ionwise_ground, - pcl::PointCloud ®ionwise_nonground, - bool is_h_available) { +void PatchWork::perform_regionwise_ground_segmentation(Patch &patch, const bool is_h_available) { // 0. Initialization - int N = src.points.size(); + int N = patch.cloud_.points.size(); pcl::PointCloud ground_tmp; ground_tmp.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - if (!regionwise_ground.empty()) regionwise_ground.clear(); - if (!regionwise_nonground.empty()) regionwise_nonground.clear(); + if (!patch.ground_.empty()) patch.ground_.clear(); + if (!patch.non_ground_.empty()) patch.non_ground_.clear(); // 1. set seeds! - extract_initial_seeds_(zone_idx, src, ground_tmp, is_h_available); + extract_initial_seeds_(patch.cloud_, ground_tmp, patch.is_close_to_origin_, is_h_available); // 2. Extract ground for (int i = 0; i < num_iter_; i++) { - estimate_plane_(ground_tmp, feat); + estimate_plane_(ground_tmp, patch.feature_); ground_tmp.clear(); //pointcloud to matrix Eigen::MatrixXf points(N, 3); int j = 0; - for (auto &p:src.points) { + for (auto &p : patch.cloud_.points) { points.row(j++) = p.getVector3fMap(); } // ground plane model - Eigen::VectorXf result = points * feat.normal_; + Eigen::VectorXf result = points * patch.feature_.normal_; // threshold filter for (int r = 0; r < N; r++) { if (i < num_iter_ - 1) { - if (result[r] < feat.th_dist_d_) { - ground_tmp.points.emplace_back(src[r]); + if (result[r] < patch.feature_.th_dist_d_) { + ground_tmp.points.emplace_back(patch.cloud_[r]); } } else { // Final stage - if (result[r] < feat.th_dist_d_) { - regionwise_ground.points.emplace_back(src[r]); + if (result[r] < patch.feature_.th_dist_d_) { + patch.ground_.points.emplace_back(patch.cloud_.points[r]); } else { - regionwise_nonground.points.emplace_back(src[r]); + patch.non_ground_.points.emplace_back(patch.cloud_.points[r]); } } } @@ -874,22 +820,24 @@ void PatchWork::extract_piecewiseground( template inline -geometry_msgs::PolygonStamped PatchWork::set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split) { +geometry_msgs::PolygonStamped PatchWork::set_polygons(int ring_idx, int sector_idx, int num_split) { + const static auto &boundary_ranges = zone_model_.boundary_ranges_; + int num_sectors = zone_model_.num_sectors_per_ring_[ring_idx]; geometry_msgs::PolygonStamped polygons; // Set point of polygon. Start from RL and ccw geometry_msgs::Point32 point; - + double sector_size = 2.0 * M_PI / static_cast(num_sectors); + double angle_incremental = sector_size / static_cast(num_split); // RL - double zone_min_range = min_ranges_[zone_idx]; - double r_len = r_idx * ring_sizes_[zone_idx] + zone_min_range; - double angle = theta_idx * sector_sizes_[zone_idx]; + double r_len = boundary_ranges[ring_idx]; + double angle = sector_idx * sector_size; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); // RU - r_len = r_len + ring_sizes_[zone_idx]; + r_len = boundary_ranges[ring_idx + 1]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; @@ -897,21 +845,21 @@ geometry_msgs::PolygonStamped PatchWork::set_polygons(int zone_idx, int // RU -> LU for (int idx = 1; idx <= num_split; ++idx) { - angle = angle + sector_sizes_[zone_idx] / num_split; + angle = angle + angle_incremental; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); } - r_len = r_len - ring_sizes_[zone_idx]; + r_len = boundary_ranges[ring_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); for (int idx = 1; idx < num_split; ++idx) { - angle = angle - sector_sizes_[zone_idx] / num_split; + angle = angle - angle_incremental; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; @@ -923,16 +871,16 @@ geometry_msgs::PolygonStamped PatchWork::set_polygons(int zone_idx, int template inline int PatchWork::determine_ground_likelihood_estimation_status( - const int concentric_idx, - const double z_vec, - const double z_elevation, - const double surface_variable) { + const int ring_idx, + const double z_vec, + const double z_elevation, + const double surface_variable) { if (z_vec < uprightness_thr_) { return TOO_TILTED; } else { //orthogonal - if (concentric_idx < num_rings_of_interest_) { - if (z_elevation > -sensor_height_ + elevation_thr_[concentric_idx]) { - if (flatness_thr_[concentric_idx] > surface_variable) { + if (ring_idx < num_rings_of_interest_) { + if (z_elevation > -sensor_height_ + elevation_thr_[ring_idx]) { + if (flatness_thr_[ring_idx] > surface_variable) { return FLAT_ENOUGH; } else { return TOO_HIGH_ELEVATION; @@ -950,49 +898,4 @@ int PatchWork::determine_ground_likelihood_estimation_status( } } -template -inline -void PatchWork::check_input_parameters_are_correct() { - string SET_SAME_SIZES_OF_PARAMETERS = "Some parameters are wrong! the size of parameters should be same"; - - int n_z = num_zones_; - int n_r = num_rings_each_zone_.size(); - int n_s = num_sectors_each_zone_.size(); - int n_m = min_ranges_.size(); - - if ((n_z != n_r) || (n_z != n_s) || (n_z != n_m)) { - throw invalid_argument(SET_SAME_SIZES_OF_PARAMETERS); - } - - if ((n_r != n_s) || (n_r != n_m) || (n_s != n_m)) { - throw invalid_argument(SET_SAME_SIZES_OF_PARAMETERS); - } - - if (min_range_ != min_ranges_[0]) { - throw invalid_argument("Setting min. ranges are weired! The first term should be eqaul to min_range_"); - } - - if (elevation_thr_.size() != flatness_thr_.size()) { - throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); - } -} - -template -inline -void PatchWork::cout_params() { - cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % - num_sectors_each_zone_[2] % - num_sectors_each_zone_[3]).str() << endl; - cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % - num_rings_each_zone_[1] % - num_rings_each_zone_[2] % - num_rings_each_zone_[3]).str() << endl; - cout << (boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % elevation_thr_[0] % elevation_thr_[1] % - elevation_thr_[2] % - elevation_thr_[3]).str() << endl; - cout << (boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % flatness_thr_[0] % flatness_thr_[1] % - flatness_thr_[2] % - flatness_thr_[3]).str() << endl; -} - #endif diff --git a/include/patchwork/sensor_configs.hpp b/include/patchwork/sensor_configs.hpp new file mode 100755 index 0000000..d6e9322 --- /dev/null +++ b/include/patchwork/sensor_configs.hpp @@ -0,0 +1,96 @@ +// +// Created by shapelim on 22. 10. 23. +// + +#ifndef PATCHWORK_SENSOR_CONFIG_HPP +#define PATCHWORK_SENSOR_CONFIG_HPP + +#include +#include + +using namespace std; + +struct SensorConfig { + vector > num_laser_channels_per_zone_; + vector num_rings_for_each_zone_; + vector num_sectors_for_each_zone_; + // Please refer to setPriorIdxesOfPatches + + int num_channels_; + float lower_fov_boundary_; + float vertical_angular_resolution_; + int horizontal_resolution_; + + SensorConfig(const string &sensor_name) { + cout << "\033[1;32mTarget Sensor: " << sensor_name << "\033[0m" << endl; + if (sensor_name == "VLP-16") { // For Kimera-Multi dataset + // https://www.amtechs.co.jp/product/VLP-16-Puck.pdf + num_laser_channels_per_zone_ = {{2, 1}, + {1, 1}, + {1}, + {1}}; + num_rings_for_each_zone_ = {2, 2, 1, 1}; + num_sectors_for_each_zone_ = {16, 32, 56, 32}; + lower_fov_boundary_ = -15; + vertical_angular_resolution_ = 2.0; + // https://github.com/TixiaoShan/LIO-SAM/blob/master/config/params.yaml + horizontal_resolution_ = 1800; + num_channels_ = 16; + } else if (sensor_name == "HDL-32E") { + // https://velodynelidar.com/wp-content/uploads/2019/12/97-0038-Rev-N-97-0038-DATASHEETWEBHDL32E_Web.pdf + num_laser_channels_per_zone_ = {{10, 5}, + {3, 2, 1, 1}, + {1, 1, 1}, + {1, 1, 1}}; + num_rings_for_each_zone_ = {2, 4, 3, 3}; + num_sectors_for_each_zone_ = {16, 32, 56, 32}; + lower_fov_boundary_ = -30.67; + vertical_angular_resolution_ = 1.33; + horizontal_resolution_ = 1080; + num_channels_ = 32; + } else if (sensor_name == "HDL-64E") { + num_laser_channels_per_zone_ = {{24, 12}, + { 4, 3, 2, 2}, + { 2, 2, 2, 1}, + { 1, 1, 1, 1, 1}}; + num_rings_for_each_zone_ = {2, 4, 4, 5}; + num_sectors_for_each_zone_ = {16, 32, 56, 32}; + lower_fov_boundary_ = -24.8; + vertical_angular_resolution_ = 0.4; + // https://github.com/TixiaoShan/LIO-SAM/blob/master/config/params.yaml + horizontal_resolution_ = 1800; + num_channels_ = 64; + } else if (sensor_name == "OS1-16") { // For NTU_VIRAL dataset + // https://www.dataspeedinc.com/app/uploads/2019/10/Ouster-OS1-Datasheet.pdf + // But not work in NTU-VIRAL dataset, haha + num_laser_channels_per_zone_ = {{2, 1}, + {1, 1}, + {1}, + {1}}; + num_rings_for_each_zone_ = {2, 2, 1, 1}; + num_sectors_for_each_zone_ = {16, 32, 56, 32}; + lower_fov_boundary_ = -16.6; + vertical_angular_resolution_ = 2.075; + // There are three main options: 512, 1024, 2048 + // https://github.com/TixiaoShan/LIO-SAM/blob/master/config/params.yaml + horizontal_resolution_ = 1024; + num_channels_ = 16; + } else if (sensor_name == "OS1-64") { + num_laser_channels_per_zone_ = {{12, 6}, + { 3, 2, 1, 1}, + { 1, 1, 1}, + { 1, 1, 1}}; + num_rings_for_each_zone_ = {2, 4, 3, 3}; + num_sectors_for_each_zone_ = {16, 32, 56, 32}; + lower_fov_boundary_ = -22.5; + vertical_angular_resolution_ = 0.7; + horizontal_resolution_ = 1024; + num_channels_ = 64; + } else { + throw invalid_argument("Sensor name is wrong! Please check the parameter"); + } + } + SensorConfig() = default; +}; + +#endif //PATCHWORK_SENSOR_CONFIG_HPP diff --git a/include/patchwork/zone_models.hpp b/include/patchwork/zone_models.hpp new file mode 100755 index 0000000..42b8e26 --- /dev/null +++ b/include/patchwork/zone_models.hpp @@ -0,0 +1,262 @@ +// +// Created by shapelim on 22. 10. 23. +// + +#ifndef PATCHWORK_ZONE_MODEL_HPP +#define PATCHWORK_ZONE_MODEL_HPP + +#include +#include +#include "patchwork/sensor_configs.hpp" +#include "sensor_configs.hpp" + +#define INVALID_RING_IDX -1 +#define OVERFLOWED_IDX -2 + +class ZoneModel { + public: + + ZoneModel() {} + + ~ZoneModel() {} + + SensorConfig sensor_config_; + + /* + * Common functions + */ + inline size_t size() const; + // Important! 'virtual' is necessary to be declared in the base class + // + the functions must be declared, i.e. {} is needed + virtual void set_parameters() {} + +// +// template +// int loadCloud(size_t idx, pcl::PointCloud &cloud) const {} +// +// virtual void getGTLabeledScan(size_t i, pcl::PointCloud& cloud) {} +// +// // Estimated labels are added +// virtual void getScanAndPose(size_t i, pcl::PointCloud& cloud, Eigen::Matrix4f &pose) { +// cout << "[DefaultLoader] Default getScanandPose is Loaded" << endl; +// } +// +// virtual void loadEstGroundAndInstanceLabels(const int i, std::vector& ground_label, +// std::vector& instance_label) {} +// +// virtual void assignLabels(const std::vector ground_labels, const std::vector instance_labels, +// const float min_z_voi, const float max_z_voi, +// pcl::PointCloud& src_cloud, uint32_t& max_instance) {} +// +// virtual void testInheritance() { cout << "Test inheritance" << endl; } + +}; + +class ConcentricZoneModel : public ZoneModel { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + ConcentricZoneModel() {} + + ConcentricZoneModel(const std::string &sensor_model, const double sensor_height, + const float min_range, const float max_range) { + sensor_config_ = SensorConfig(sensor_model); + num_zones_ = sensor_config_.num_laser_channels_per_zone_.size(); + max_ring_index_in_first_zone = sensor_config_.num_laser_channels_per_zone_[0].size(); + + sensor_height_ = sensor_height; + min_range_ = min_range; + max_range_ = max_range; + + sqr_min_range_ = min_range * min_range; + sqr_max_range_ = max_range * max_range; + + set_concentric_zone_model(); + } + bool is_range_boundary_set_; + size_t max_ring_index_in_first_zone; + size_t num_zones_; + size_t num_total_rings_; + double sensor_height_; + float min_range_; + float max_range_; + float sqr_min_range_; + float sqr_max_range_; + + vector num_sectors_per_ring_; + // sqr: For reducing computational complexity + vector sqr_boundary_ranges_; + // For visualization + vector boundary_ranges_; + vector boundary_ratios_; + + ~ConcentricZoneModel() {} + + inline void set_concentric_zone_model() { + set_num_sectors_for_each_ring(); + // Seting ring boundaries to consider # of laser rings + float smallest_incidence_angle = 90.0 + sensor_config_.lower_fov_boundary_; + // For defensive programming + if (tan(DEG2RAD(smallest_incidence_angle)) * sensor_height_ < min_range_) { + cout << tan(DEG2RAD(smallest_incidence_angle)) * sensor_height_ << " vs " << min_range_ << endl; + throw invalid_argument("[NZM] The parameter `min_r` is wrong. Check your sensor height or min. range"); + } + sanity_check(); + set_sqr_boundary_ranges(sensor_height_, smallest_incidence_angle); + } + + inline void sanity_check() { + string SET_SAME_SIZES_OF_PARAMETERS = "Some parameters are wrong! the size of parameters should be same"; + + int n_z = num_zones_; + int n_r = sensor_config_.num_rings_for_each_zone_.size(); + int n_s = sensor_config_.num_sectors_for_each_zone_.size(); + + if ((n_z != n_r) || (n_z != n_s) || (n_r != n_s)) { + throw invalid_argument(SET_SAME_SIZES_OF_PARAMETERS); + } + } + + bool is_boundary_set() { + return is_range_boundary_set_; + } + inline void set_num_sectors_for_each_ring() { + num_sectors_per_ring_.clear(); + int count = 0; + for (const auto &num_rings : sensor_config_.num_rings_for_each_zone_) { + for (int j = 0; j < num_rings; ++j) { + num_sectors_per_ring_.push_back(sensor_config_.num_sectors_for_each_zone_[count]); + } + count++; + } + num_total_rings_ = num_sectors_per_ring_.size(); + } + + void set_sqr_boundary_ranges(const double sensor_height, const float smallest_incidence_angle) { + /*** + * Why squared value? + * Because `sqrt` operation requires more computational cost + */ + // sqr (square): For speed-up + // original : For viz polygon + is_range_boundary_set_ = true; + + boundary_ranges_.clear(); + boundary_ranges_.push_back(min_range_); + sqr_boundary_ranges_.clear(); + sqr_boundary_ranges_.push_back(pow(min_range_, 2)); + float incidence_angle = smallest_incidence_angle; + std::cout << "min_range: " << min_range_ << std::endl; + + float incidence_angle_prev = incidence_angle; + int count = 0; + for (int i = 0; i < sensor_config_.num_laser_channels_per_zone_.size(); ++i) { + vector num_channels_per_ring = sensor_config_.num_laser_channels_per_zone_[i]; + for (int j = 0; j < num_channels_per_ring.size(); ++j) { + incidence_angle += + static_cast(num_channels_per_ring[j]) * sensor_config_.vertical_angular_resolution_; + float incidence_angle_w_margin = + incidence_angle + 0.5 * sensor_config_.vertical_angular_resolution_; // For safety margin + cout << "\033[1;32m" << incidence_angle_w_margin << "\033[0m" << endl; + + // Check whether the angle is over the 90 degrees + float boundary_range; + if (incidence_angle_w_margin >= 90) { + cout << "\033[1;33mIncidence angle is over the 90 deg!!\033[0m" << endl; + cout << "\033[1;33mBins are evenly divided!\033[0m" << endl; + static float denominator = static_cast(num_total_rings_ - count + 1); + static float left_b = boundary_ranges_.back(); + static float right_b = max_range_; + float k = num_total_rings_ - count; + boundary_range = left_b * (denominator - k) / denominator + max_range_ * k / denominator; + } else { + boundary_range = tan(DEG2RAD(incidence_angle_w_margin)) * sensor_height; + incidence_angle_prev = incidence_angle_w_margin; + } + boundary_ranges_.push_back(boundary_range); + sqr_boundary_ranges_.push_back(pow(boundary_range, 2)); + cout << boundary_range << " m " << endl; + ++count; + } + } + float total_diff = boundary_ranges_.back() - min_range_; + for (const auto boundary_range : boundary_ranges_) { + float ratio = (boundary_range - min_range_) / total_diff; + boundary_ratios_.push_back(ratio); + } + } + + inline void cout_params() { + const auto &num_sectors_each_zone_ = sensor_config_.num_sectors_for_each_zone_; + const auto &num_rings_each_zone_ = sensor_config_.num_rings_for_each_zone_; + + std::cout + << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % + num_sectors_each_zone_[2] % + num_sectors_each_zone_[3]).str() << endl; + std::cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % + num_rings_each_zone_[1] % + num_rings_each_zone_[2] % + num_rings_each_zone_[3]).str() << endl; + } + + inline float xy2sqr_r(const float &x, const float &y) { + return x * x + y * y; + } + + inline float xy2theta(const float &x, const float &y) { // 0 ~ 2 * PI + /* + if (y >= 0) { + return atan2(y, x); // 1, 2 quadrant + } else { + return 2 * M_PI + atan2(y, x);// 3, 4 quadrant + } + */ + auto atan_value = atan2(y, x); // EDITED! + return atan_value > 0 ? atan_value : atan_value + 2 * M_PI; // EDITED! + } + + inline int get_ring_idx(const float &x, const float &y) { + static auto &b = sqr_boundary_ranges_; + float sqr_r = xy2sqr_r(x, y); + // Exception for UAVs such as NTU VIRAL dataset + if (sqr_r < b[0]) { +// std::cout << "\033[1;33mInvalid ring idx has come:"; +// std::cout << "(" << sqrt(sqr_r) << " < " << sqrt(b[0]) << ")\033[0m" << std::endl; + return INVALID_RING_IDX; + } + if (sqr_r > sqr_max_range_) { + return OVERFLOWED_IDX; + } + + int bound_idx = lower_bound(b.begin(), b.end(), sqr_r) - b.begin(); + // bound_idx: idx whose value is larger than sqr_r + // Thus, b[bound_idx - 1] < sqr_r < b[bound_idx] + // And note that num_rings + 1 = b.size(); thus, minus 1 is needed + return bound_idx - 1; + } + + inline int get_sector_idx(const float &x, const float &y, const int ring_idx) { + float theta = xy2theta(x, y); + int num_sectors = num_sectors_per_ring_[ring_idx]; + float sector_size = 2.0 * M_PI / static_cast(num_sectors); + + // min: for defensive programming + return min(static_cast(theta / sector_size), num_sectors - 1); + } + + inline std::pair get_ring_sector_idx(const float &x, const float &y) { + int ring_idx = get_ring_idx(x, y); + if (ring_idx == INVALID_RING_IDX) { + return std::make_pair(INVALID_RING_IDX, INVALID_RING_IDX); + } + if (ring_idx == OVERFLOWED_IDX) { + return std::make_pair(OVERFLOWED_IDX, OVERFLOWED_IDX); + } + + int sector_idx = get_sector_idx(x, y, ring_idx); + return std::make_pair(ring_idx, sector_idx); + } + +}; + +#endif //PATCHWORK_SENSOR_CONFIG_HPP diff --git a/launch/offline_kitti.launch b/launch/offline_kitti.launch index 48158bc..b537b6b 100755 --- a/launch/offline_kitti.launch +++ b/launch/offline_kitti.launch @@ -1,14 +1,17 @@ + + + + + + "patchwork" + false + false + 0 + 10000 - -"patchwork" -false -false -0 -10000 -"/media/shapelim/UX960NVMe11/kitti_semantic/dataset/sequences/04" - - + + - + diff --git a/nodes/offline_kitti.cpp b/nodes/offline_kitti.cpp index b23cd17..71b5dc0 100644 --- a/nodes/offline_kitti.cpp +++ b/nodes/offline_kitti.cpp @@ -145,7 +145,12 @@ int main(int argc, char**argv) { // - - - - - - - - - - - - - - - - - - - - // If you want to save precision/recall in a text file, revise this part // - - - - - - - - - - - - - - - - - - - - - output_filename = "/home/shapelim/patchwork.txt"; + const char* home_dir = std::getenv("HOME"); + if (home_dir == nullptr) { + std::cerr << "Error: HOME environment variable not set." << std::endl; + return 1; + } + std::string output_filename = std::string(home_dir) + "/patchwork_quantitaive_results.txt"; ofstream ground_output(output_filename, ios::app); ground_output << n << "," << time_taken << "," << precision << "," << recall << "," << precision_naive << "," << recall_naive; ground_output << std::endl; diff --git a/rviz/patchwork_viz.rviz b/rviz/patchwork_viz.rviz index c54dd41..19866fc 100755 --- a/rviz/patchwork_viz.rviz +++ b/rviz/patchwork_viz.rviz @@ -11,7 +11,7 @@ Panels: - /est_ground1/est_ground1 - /est_ground1/est_ground_filtered1 Splitter Ratio: 0.6044568419456482 - Tree Height: 467 + Tree Height: 444 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -266,7 +266,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - Enabled: false + Enabled: true Name: TFPN - Class: rviz/Group Displays: @@ -326,7 +326,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - Enabled: true + Enabled: false Name: est_ground Enabled: true Global Options: @@ -372,17 +372,17 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 1.1047967672348022 Target Frame: - Yaw: 3.141618013381958 + Yaw: 2.351613998413086 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 759 + Height: 736 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001c10000025efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000372000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056d00000039fc0100000002fb0000000800540069006d006501000000000000056d000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003a60000025e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c100000247fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000247000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000372000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056d00000039fc0100000002fb0000000800540069006d006501000000000000056d000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003a60000024700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -392,5 +392,5 @@ Window Geometry: Views: collapsed: true Width: 1389 - X: 531 - Y: 120 + X: 2069 + Y: 492