40 #ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
41 #define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
43 #include <pcl/segmentation/boost.h>
44 #include <pcl/segmentation/organized_connected_component_segmentation.h>
45 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
47 #include <pcl/common/eigen.h>
51 projectToPlaneFromViewpoint (
pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp)
53 Eigen::Vector3f
norm (normal[0], normal[1], normal[2]);
56 for (std::size_t i = 0; i < cloud.
points.size (); i++)
60 float u =
norm.dot ((centroid - vp)) /
norm.dot ((pt - vp));
61 Eigen::Vector3f intersection (vp + u * (pt - vp));
62 projected_cloud[i].x = intersection[0];
63 projected_cloud[i].y = intersection[1];
64 projected_cloud[i].z = intersection[2];
67 return (projected_cloud);
71 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
73 std::vector<PointIndices>& inlier_indices)
76 std::vector<pcl::PointIndices> label_indices;
77 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
78 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
79 segment (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
83 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
85 std::vector<PointIndices>& inlier_indices,
86 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
87 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
89 std::vector<pcl::PointIndices>& label_indices)
95 if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
97 PCL_ERROR (
"[pcl::%s::segment] Number of points in input cloud (%lu) and normal cloud (%lu) do not match!\n",
98 getClassName ().c_str (), input_->points.size (),
99 normals_->points.size ());
104 if (!input_->isOrganized ())
106 PCL_ERROR (
"[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
107 getClassName ().c_str ());
112 std::vector<float> plane_d (input_->points.size ());
114 for (std::size_t i = 0; i < input_->size (); ++i)
115 plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
119 compare_->setPlaneCoeffD (plane_d);
120 compare_->setInputCloud (input_);
121 compare_->setInputNormals (normals_);
122 compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
123 compare_->setDistanceThreshold (static_cast<float> (distance_threshold_),
true);
128 connected_component.
segment (labels, label_indices);
130 Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
131 Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
132 Eigen::Matrix3f clust_cov;
137 for (
const auto &label_index : label_indices)
139 if (static_cast<unsigned> (label_index.indices.size ()) > min_inliers_)
142 Eigen::Vector4f plane_params;
144 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
145 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
147 plane_params[0] = eigen_vector[0];
148 plane_params[1] = eigen_vector[1];
149 plane_params[2] = eigen_vector[2];
151 plane_params[3] = -1 * plane_params.dot (clust_centroid);
153 vp -= clust_centroid;
154 float cos_theta = vp.dot (plane_params);
159 plane_params[3] = -1 * plane_params.dot (clust_centroid);
164 float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
166 curvature = std::abs (eigen_value / eig_sum);
170 if (curvature < maximum_curvature_)
172 model.
values[0] = plane_params[0];
173 model.
values[1] = plane_params[1];
174 model.
values[2] = plane_params[2];
175 model.
values[3] = plane_params[3];
176 model_coefficients.push_back (model);
177 inlier_indices.push_back (label_index);
178 centroids.push_back (clust_centroid);
179 covariances.push_back (clust_cov);
187 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
190 std::vector<ModelCoefficients> model_coefficients;
191 std::vector<PointIndices> inlier_indices;
193 std::vector<pcl::PointIndices> label_indices;
194 std::vector<pcl::PointIndices> boundary_indices;
196 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
197 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
198 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
199 regions.resize (model_coefficients.size ());
200 boundary_indices.resize (model_coefficients.size ());
202 for (std::size_t i = 0; i < model_coefficients.size (); i++)
204 boundary_cloud.
resize (0);
206 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
207 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
208 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
210 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
211 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
212 model_coefficients[i].values[1],
213 model_coefficients[i].values[2],
214 model_coefficients[i].values[3]);
217 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
224 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
227 std::vector<ModelCoefficients> model_coefficients;
228 std::vector<PointIndices> inlier_indices;
230 std::vector<pcl::PointIndices> label_indices;
231 std::vector<pcl::PointIndices> boundary_indices;
233 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
234 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
235 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
236 refine (model_coefficients, inlier_indices, labels, label_indices);
237 regions.resize (model_coefficients.size ());
238 boundary_indices.resize (model_coefficients.size ());
240 for (std::size_t i = 0; i < model_coefficients.size (); i++)
242 boundary_cloud.
resize (0);
243 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
245 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
246 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
247 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
249 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
250 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
251 model_coefficients[i].values[1],
252 model_coefficients[i].values[2],
253 model_coefficients[i].values[3]);
255 Eigen::Vector3f vp (0.0, 0.0, 0.0);
257 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
261 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
268 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
270 std::vector<ModelCoefficients>& model_coefficients,
271 std::vector<PointIndices>& inlier_indices,
273 std::vector<pcl::PointIndices>& label_indices,
274 std::vector<pcl::PointIndices>& boundary_indices)
277 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
278 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
279 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
280 refine (model_coefficients, inlier_indices, labels, label_indices);
281 regions.resize (model_coefficients.size ());
282 boundary_indices.resize (model_coefficients.size ());
284 for (std::size_t i = 0; i < model_coefficients.size (); i++)
286 boundary_cloud.
resize (0);
287 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
289 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
290 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
291 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
293 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
294 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
295 model_coefficients[i].values[1],
296 model_coefficients[i].values[2],
297 model_coefficients[i].values[3]);
299 Eigen::Vector3f vp (0.0, 0.0, 0.0);
300 if (project_points_ && !boundary_cloud.
points.empty ())
301 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
305 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
312 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
314 std::vector<PointIndices>& inlier_indices,
316 std::vector<pcl::PointIndices>& label_indices)
319 std::vector<bool> grow_labels;
320 std::vector<int> label_to_model;
321 grow_labels.resize (label_indices.size (),
false);
322 label_to_model.resize (label_indices.size (), 0);
324 for (std::size_t i = 0; i < model_coefficients.size (); i++)
326 int model_label = (*labels)[inlier_indices[i].indices[0]].label;
327 label_to_model[model_label] = static_cast<int> (i);
328 grow_labels[model_label] =
true;
332 refinement_compare_->setInputCloud (input_);
333 refinement_compare_->setLabels (labels);
334 refinement_compare_->setModelCoefficients (model_coefficients);
335 refinement_compare_->setRefineLabels (grow_labels);
336 refinement_compare_->setLabelToModel (label_to_model);
339 unsigned int current_row = 0;
340 unsigned int next_row = labels->width;
341 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
344 for (
unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
346 int current_label = (*labels)[current_row+colIdx].label;
347 int right_label = (*labels)[current_row+colIdx+1].label;
348 if (current_label < 0 || right_label < 0)
353 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
356 labels->points[current_row+colIdx+1].label = current_label;
357 label_indices[current_label].indices.push_back (current_row+colIdx+1);
358 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
361 int lower_label = (*labels)[next_row+colIdx].label;
366 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
368 labels->points[next_row+colIdx].label = current_label;
369 label_indices[current_label].indices.push_back (next_row+colIdx);
370 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
377 current_row = labels->width * (labels->height - 1);
378 unsigned int prev_row = current_row - labels->width;
379 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
381 for (
int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
383 int current_label = (*labels)[current_row+colIdx].label;
384 int left_label = (*labels)[current_row+colIdx-1].label;
385 if (current_label < 0 || left_label < 0)
389 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
391 labels->points[current_row+colIdx-1].label = current_label;
392 label_indices[current_label].indices.push_back (current_row+colIdx-1);
393 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
396 int upper_label = (*labels)[prev_row+colIdx].label;
400 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
402 labels->points[prev_row+colIdx].label = current_label;
403 label_indices[current_label].indices.push_back (prev_row+colIdx);
404 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
410 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>;
412 #endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_