| 422 | |
| 423 | template <typename PointT> |
| 424 | inline void PatchWork<PointT>::extract_initial_seeds_(const pcl::PointCloud<PointT> &p_sorted, |
| 425 | pcl::PointCloud<PointT> &init_seeds, |
| 426 | bool is_close_to_origin, |
| 427 | bool is_h_available) { |
| 428 | init_seeds.points.clear(); |
| 429 | |
| 430 | // LPR is the mean of low point representative |
| 431 | double sum = 0; |
| 432 | int cnt = 0; |
| 433 | |
| 434 | int init_idx = 0; |
| 435 | // Empirically, adaptive seed selection applying to Z1 is fine |
| 436 | if (is_h_available) { |
| 437 | static double lowest_h_margin_in_close_zone = |
| 438 | (sensor_height_ == 0.0) ? -0.1 : adaptive_seed_selection_margin_ * sensor_height_; |
| 439 | if (is_close_to_origin) { |
| 440 | for (size_t i = 0; i < p_sorted.points.size(); i++) { |
| 441 | if (p_sorted.points[i].z < lowest_h_margin_in_close_zone) { |
| 442 | ++init_idx; |
| 443 | } else { |
| 444 | break; |
| 445 | } |
| 446 | } |
| 447 | } |
| 448 | } |
| 449 | |
| 450 | // Calculate the mean height value. |
| 451 | for (size_t i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { |
| 452 | sum += p_sorted.points[i].z; |
| 453 | cnt++; |
| 454 | } |
| 455 | double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0 |
| 456 | |
| 457 | // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ |
| 458 | for (size_t i = 0; i < p_sorted.points.size(); i++) { |
| 459 | if (p_sorted.points[i].z < lpr_height + th_seeds_) { |
| 460 | init_seeds.points.push_back(p_sorted.points[i]); |
| 461 | } |
| 462 | } |
| 463 | } |
| 464 | |
| 465 | template <typename PointT> |
| 466 | inline double PatchWork<PointT>::consensus_set_based_height_estimation( |