| 597 | |
| 598 | template <typename PointT> |
| 599 | inline void PatchWork<PointT>::estimate_ground(const pcl::PointCloud<PointT> &cloud_in, |
| 600 | pcl::PointCloud<PointT> &ground, |
| 601 | pcl::PointCloud<PointT> &nonground, |
| 602 | double &time_taken) { |
| 603 | // Just for visualization |
| 604 | poly_list_.header.stamp = ros::Time::now(); |
| 605 | if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); |
| 606 | if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); |
| 607 | |
| 608 | if (initialized_ && ATAT_ON_) { |
| 609 | estimate_sensor_height(cloud_in); |
| 610 | initialized_ = false; |
| 611 | std::cout << "\033[1;32m=> Complete to estimate the sensor height: " << sensor_height_ |
| 612 | << "\033[0m" << std::endl; |
| 613 | } |
| 614 | |
| 615 | static double start, end; |
| 616 | // static double start, t0, t1, t2; |
| 617 | // double t_total_ground = 0.0; |
| 618 | double t_total_estimate = 0.0; |
| 619 | // 1.Msg to pointcloud |
| 620 | pcl::PointCloud<PointT> cloud_in_tmp = cloud_in; |
| 621 | |
| 622 | start = ros::Time::now().toSec(); |
| 623 | |
| 624 | // Error point removal |
| 625 | // As there are some error mirror reflection under the ground, |
| 626 | // Sort point according to height, here uses z-axis in default |
| 627 | // -2.0 is a rough criteria |
| 628 | size_t i = 0; |
| 629 | while (i < cloud_in_tmp.points.size()) { |
| 630 | if (cloud_in_tmp.points[i].z < -sensor_height_ - 2.0) { |
| 631 | std::iter_swap(cloud_in_tmp.points.begin() + i, cloud_in_tmp.points.end() - 1); |
| 632 | cloud_in_tmp.points.pop_back(); |
| 633 | } else { |
| 634 | ++i; |
| 635 | } |
| 636 | } |
| 637 | |
| 638 | // t1 = ros::Time::now().toSec(); |
| 639 | flush_patches(regionwise_patches_); |
| 640 | pc2regionwise_patches(cloud_in_tmp, regionwise_patches_); |
| 641 | |
| 642 | ground.clear(); |
| 643 | nonground.clear(); |
| 644 | reverted_points_by_flatness_.clear(); |
| 645 | rejected_points_by_elevation_.clear(); |
| 646 | |
| 647 | int num_patches = patch_indices_.size(); |
| 648 | |
| 649 | // HT comments: TBB w/ blocked_range was faster in my desktop |
| 650 | // 117 Hz vs 126 Hz |
| 651 | // tbb::parallel_for(0, num_patches, [&](int i) { |
| 652 | tbb::parallel_for(tbb::blocked_range<int>(0, num_patches), [&](tbb::blocked_range<int> r) { |
| 653 | for (int k = r.begin(); k < r.end(); ++k) { |
| 654 | const auto &[ring_idx, sector_idx] = patch_indices_[k]; |
| 655 | auto &patch = regionwise_patches_[ring_idx][sector_idx]; |
| 656 |
no test coverage detected