| 349 | |
| 350 | template<typename PointT> |
| 351 | inline |
| 352 | void PatchWork<PointT>::estimate_ground( |
| 353 | const pcl::PointCloud<PointT> &cloud_in, |
| 354 | pcl::PointCloud<PointT> &cloud_out, |
| 355 | pcl::PointCloud<PointT> &cloud_nonground, |
| 356 | double &time_taken) { |
| 357 | |
| 358 | // Just for visualization |
| 359 | poly_list_.header.stamp = ros::Time::now(); |
| 360 | if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); |
| 361 | if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); |
| 362 | |
| 363 | static double start, t0, t1, t2, end; |
| 364 | |
| 365 | double t_total_ground = 0.0; |
| 366 | double t_total_estimate = 0.0; |
| 367 | // 1.Msg to pointcloud |
| 368 | pcl::PointCloud<PointT> laserCloudIn; |
| 369 | laserCloudIn = cloud_in; |
| 370 | |
| 371 | start = ros::Time::now().toSec(); |
| 372 | |
| 373 | // 2.Sort on Z-axis value. 오름차순 |
| 374 | sort(laserCloudIn.points.begin(), laserCloudIn.end(), point_z_cmp<PointT>); |
| 375 | |
| 376 | t0 = ros::Time::now().toSec(); |
| 377 | // 3.Error point removal |
| 378 | // As there are some error mirror reflection under the ground, |
| 379 | // here regardless point under 1.8* sensor_height |
| 380 | // Sort point according to height, here uses z-axis in default |
| 381 | |
| 382 | // 원하는 높이를 찾는다. |
| 383 | auto it = laserCloudIn.points.begin(); |
| 384 | for (int i = 0; i < laserCloudIn.points.size(); i++) { |
| 385 | if (laserCloudIn.points[i].z < -1.8 * sensor_height_) { |
| 386 | it++; |
| 387 | } else { |
| 388 | break; |
| 389 | } |
| 390 | } |
| 391 | |
| 392 | // 낮은곳 부터 원하는 높이까지 포인트를 제거한다. |
| 393 | laserCloudIn.points.erase(laserCloudIn.points.begin(), it); |
| 394 | |
| 395 | t1 = ros::Time::now().toSec(); |
| 396 | // 4. pointcloud -> regionwise setting |
| 397 | for (int k = 0; k < num_zones_; ++k) { |
| 398 | flush_patches_in_zone(ConcentricZoneModel_[k], num_sectors_each_zone_[k], num_rings_each_zone_[k]); |
| 399 | } |
| 400 | pc2czm(laserCloudIn, ConcentricZoneModel_); |
| 401 | |
| 402 | t2 = ros::Time::now().toSec(); |
| 403 | |
| 404 | cloud_out.clear(); |
| 405 | cloud_nonground.clear(); |
| 406 | revert_pc.clear(); |
| 407 | reject_pc.clear(); |
| 408 | |