75 {
78 pcl::copyPointCloud<PointT>(*cloud, *pointcloud_ptr);
79 int iter_num = static_cast<int>(log2(max_grid_size_ / min_grid_size_));
80 if (iter_num == 0) {
81 iter_num = 1;
82 }
83 std::clock_t plane_time;
84 plane_time = std::clock();
85 int total_plane_num = 0;
86 double power2 = 0.5;
87 for (int iter = 0; iter <= iter_num; ++iter) {
88 power2 *= 2;
89 float grid_size = static_cast<float>(max_grid_size_ / power2);
90 VoxelGridCovariance<PointT> vgc;
91 vgc.setInputCloud(pointcloud_ptr);
92 vgc.SetMinPointPerVoxel(min_planepoints_number_);
93 vgc.setLeafSize(grid_size, grid_size, grid_size);
94 vgc.Filter(false);
95
97 int plane_num = 0;
98 typename std::map<size_t, VoxelGridCovariance<PointT>::Leaf>::iterator it;
99 for (it = vgc.GetLeaves().begin(); it != vgc.GetLeaves().end(); it++) {
100 if (it->second.GetPointCount() < min_planepoints_number_) {
101 cloud_tmp += it->second.cloud_;
102 continue;
103 }
105 if (GetPlaneFeaturePoint(it->second.cloud_, &cloud_outlier)) {
106 cloud_tmp += cloud_outlier;
107 plane_num++;
108 } else {
109 cloud_tmp += it->second.cloud_;
110 }
111 }
112 AINFO <<
"the " << iter <<
" interation: plane_num = " << plane_num;
113 total_plane_num += plane_num;
115 *pointcloud_ptr = cloud_tmp;
116 }
117
118 *non_xy_plane_cloud_ = *pointcloud_ptr;
119 plane_time = std::clock() - plane_time;
120 AINFO <<
"plane_patch takes:"
121 << static_cast<double>(plane_time) / CLOCKS_PER_SEC << "sec.";
122 AINFO <<
"total_plane_num = " << total_plane_num;
123 AINFO <<
"total_points_num = " << xy_plane_cloud_->points.size();
124 return;
125 }