| 303 | |
| 304 | template<typename PointT> |
| 305 | inline |
| 306 | void PatchWork<PointT>::extract_initial_seeds_( |
| 307 | const int zone_idx, const pcl::PointCloud<PointT> &p_sorted, |
| 308 | pcl::PointCloud<PointT> &init_seeds) { |
| 309 | init_seeds.points.clear(); |
| 310 | |
| 311 | // LPR is the mean of low point representative |
| 312 | double sum = 0; |
| 313 | int cnt = 0; |
| 314 | |
| 315 | int init_idx = 0; |
| 316 | // Empirically, adaptive seed selection applying to Z1 is fine |
| 317 | static double lowest_h_margin_in_close_zone = (sensor_height_ == 0.0)? -0.1 : adaptive_seed_selection_margin_ * sensor_height_; |
| 318 | if (zone_idx == 0) { |
| 319 | for (int i = 0; i < p_sorted.points.size(); i++) { |
| 320 | if (p_sorted.points[i].z < lowest_h_margin_in_close_zone) { |
| 321 | ++init_idx; |
| 322 | } else { |
| 323 | break; |
| 324 | } |
| 325 | } |
| 326 | } |
| 327 | |
| 328 | // Calculate the mean height value. |
| 329 | for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { |
| 330 | sum += p_sorted.points[i].z; |
| 331 | cnt++; |
| 332 | } |
| 333 | double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 |
| 334 | |
| 335 | // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ |
| 336 | for (int i = 0; i < p_sorted.points.size(); i++) { |
| 337 | if (p_sorted.points[i].z < lpr_height + th_seeds_) { |
| 338 | init_seeds.points.push_back(p_sorted.points[i]); |
| 339 | } |
| 340 | } |
| 341 | } |
| 342 | |
| 343 | |
| 344 | /* |