| 522 | |
| 523 | template <typename PointT> |
| 524 | inline void PatchWork<PointT>::estimate_sensor_height(pcl::PointCloud<PointT> cloud_in) { |
| 525 | // ATAT: All-Terrain Automatic HeighT estimator |
| 526 | Ring ring_for_ATAT(num_sectors_for_ATAT_); |
| 527 | for (auto const &pt : cloud_in.points) { |
| 528 | int sector_idx; |
| 529 | double r = xy2radius(pt.x, pt.y); |
| 530 | |
| 531 | float sector_size_for_ATAT = 2 * M_PI / num_sectors_for_ATAT_; |
| 532 | |
| 533 | if ((r <= max_r_for_ATAT_) && (r > min_range_)) { |
| 534 | double theta = xy2theta(pt.x, pt.y); |
| 535 | |
| 536 | sector_idx = min(static_cast<int>((theta / sector_size_for_ATAT)), num_sectors_for_ATAT_); |
| 537 | ring_for_ATAT[sector_idx].cloud_.points.emplace_back(pt); |
| 538 | } |
| 539 | } |
| 540 | |
| 541 | // Assign valid measurements and corresponding linearities/planarities |
| 542 | vector<double> ground_elevations_wrt_the_origin; |
| 543 | vector<double> linearities; |
| 544 | vector<double> planarities; |
| 545 | for (int i = 0; i < num_sectors_for_ATAT_; ++i) { |
| 546 | if (static_cast<int>(ring_for_ATAT[i].cloud_.size()) < num_min_pts_) { |
| 547 | continue; |
| 548 | } |
| 549 | |
| 550 | pcl::PointCloud<PointT> dummy_est_ground; |
| 551 | pcl::PointCloud<PointT> dummy_est_non_ground; |
| 552 | perform_regionwise_ground_segmentation(ring_for_ATAT[i], false); |
| 553 | |
| 554 | const auto &feat = ring_for_ATAT[i].feature_; |
| 555 | |
| 556 | const double ground_z_vec = abs(feat.normal_(2)); |
| 557 | const double ground_z_elevation = feat.mean_(2); |
| 558 | |
| 559 | // Check whether the vector is sufficiently upright and flat |
| 560 | if (ground_z_vec > uprightness_thr_ && feat.linearity_ < 0.9) { |
| 561 | ground_elevations_wrt_the_origin.push_back(ground_z_elevation); |
| 562 | linearities.push_back(feat.linearity_); |
| 563 | planarities.push_back(feat.planarity_); |
| 564 | } |
| 565 | } |
| 566 | |
| 567 | // Setting for consensus set-based height estimation |
| 568 | // It is equal to component-wise translation estimation (COTE) in TEASER++ |
| 569 | int N = ground_elevations_wrt_the_origin.size(); |
| 570 | cout << "\033[1;33m[ATAT] N: " << N << " -> " << ground_elevations_wrt_the_origin.size() |
| 571 | << "\033[0m" << endl; |
| 572 | if (ground_elevations_wrt_the_origin.size() == 0) { |
| 573 | throw invalid_argument( |
| 574 | "No valid ground points for ATAT! Please check the " |
| 575 | "input data and `max_r_for_ATAT`"); |
| 576 | } |
| 577 | Eigen::Matrix<double, 1, Eigen::Dynamic> values = Eigen::MatrixXd::Ones(1, N); |
| 578 | Eigen::Matrix<double, 1, Eigen::Dynamic> ranges = noise_bound_ * Eigen::MatrixXd::Ones(1, N); |
| 579 | Eigen::Matrix<double, 1, Eigen::Dynamic> weights = |
| 580 | 1.0 / (noise_bound_ * noise_bound_) * Eigen::MatrixXd::Ones(1, N); |
| 581 | for (int i = 0; i < N; ++i) { |