| 579 | // For adaptive |
| 580 | template<typename PointT> |
| 581 | inline |
| 582 | void PatchWork<PointT>::extract_piecewiseground( |
| 583 | const int zone_idx, const pcl::PointCloud<PointT> &src, |
| 584 | pcl::PointCloud<PointT> &dst, |
| 585 | pcl::PointCloud<PointT> &non_ground_dst) { |
| 586 | // 0. Initialization |
| 587 | if (!ground_pc_.empty()) ground_pc_.clear(); |
| 588 | if (!dst.empty()) dst.clear(); |
| 589 | if (!non_ground_dst.empty()) non_ground_dst.clear(); |
| 590 | // 1. set seeds! |
| 591 | |
| 592 | extract_initial_seeds_(zone_idx, src, ground_pc_); |
| 593 | // 2. Extract ground |
| 594 | for (int i = 0; i < num_iter_; i++) { |
| 595 | estimate_plane_(ground_pc_); |
| 596 | ground_pc_.clear(); |
| 597 | |
| 598 | //pointcloud to matrix |
| 599 | Eigen::MatrixXf points(src.points.size(), 3); |
| 600 | int j = 0; |
| 601 | for (auto &p:src.points) { |
| 602 | points.row(j++) << p.x, p.y, p.z; |
| 603 | } |
| 604 | // ground plane model |
| 605 | Eigen::VectorXf result = points * normal_; |
| 606 | // threshold filter |
| 607 | for (int r = 0; r < result.rows(); r++) { |
| 608 | if (i < num_iter_ - 1) { |
| 609 | if (result[r] < th_dist_d_) { |
| 610 | ground_pc_.points.push_back(src[r]); |
| 611 | } |
| 612 | } else { // Final stage |
| 613 | if (result[r] < th_dist_d_) { |
| 614 | dst.points.push_back(src[r]); |
| 615 | } else { |
| 616 | if (i == num_iter_ - 1) { |
| 617 | non_ground_dst.push_back(src[r]); |
| 618 | } |
| 619 | } |
| 620 | } |
| 621 | } |
| 622 | } |
| 623 | } |
| 624 | |
| 625 | |
| 626 | template<typename PointT> |