Point Cloud Library (PCL)  1.8.1-dev
organized_multi_plane_segmentation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  *
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
41 #define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
42 
43 #include <pcl/segmentation/boost.h>
44 #include <pcl/segmentation/organized_connected_component_segmentation.h>
45 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
46 #include <pcl/common/centroid.h>
47 #include <pcl/common/eigen.h>
48 
49 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointT> pcl::PointCloud<PointT>
51 projectToPlaneFromViewpoint (pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp)
52 {
53  Eigen::Vector3f norm (normal[0], normal[1], normal[2]); //(region.coefficients_[0], region.coefficients_[1], region.coefficients_[2]);
54  pcl::PointCloud<PointT> projected_cloud;
55  projected_cloud.resize (cloud.points.size ());
56  for (size_t i = 0; i < cloud.points.size (); i++)
57  {
58  Eigen::Vector3f pt (cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
59  //Eigen::Vector3f intersection = (vp, pt, norm, centroid);
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];
65  }
66 
67  return (projected_cloud);
68 }
69 
70 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71 template<typename PointT, typename PointNT, typename PointLT> void
72 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
73  std::vector<PointIndices>& inlier_indices)
74 {
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);
80 }
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
83 template<typename PointT, typename PointNT, typename PointLT> void
84 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
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)
90 {
91  if (!initCompute ())
92  return;
93 
94  // Check that we got the same number of points and normals
95  if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
96  {
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 ());
100  return;
101  }
102 
103  // Check that the cloud is organized
104  if (!input_->isOrganized ())
105  {
106  PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
107  getClassName ().c_str ());
108  return;
109  }
110 
111  // Calculate range part of planes' hessian normal form
112  std::vector<float> plane_d (input_->points.size ());
113 
114  for (unsigned int i = 0; i < input_->size (); ++i)
115  plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
116 
117  // Make a comparator
118  //PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d);
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);
124 
125  // Set up the output
126  OrganizedConnectedComponentSegmentation<PointT,PointLT> connected_component (compare_);
127  connected_component.setInputCloud (input_);
128  connected_component.segment (labels, label_indices);
129 
130  Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
131  Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
132  Eigen::Matrix3f clust_cov;
134  model.values.resize (4);
135 
136  // Fit Planes to each cluster
137  for (size_t i = 0; i < label_indices.size (); i++)
138  {
139  if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_)
140  {
141  pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid);
142  Eigen::Vector4f plane_params;
143 
144  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
145  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
146  pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
147  plane_params[0] = eigen_vector[0];
148  plane_params[1] = eigen_vector[1];
149  plane_params[2] = eigen_vector[2];
150  plane_params[3] = 0;
151  plane_params[3] = -1 * plane_params.dot (clust_centroid);
152 
153  vp -= clust_centroid;
154  float cos_theta = vp.dot (plane_params);
155  if (cos_theta < 0)
156  {
157  plane_params *= -1;
158  plane_params[3] = 0;
159  plane_params[3] = -1 * plane_params.dot (clust_centroid);
160  }
161 
162  // Compute the curvature surface change
163  float curvature;
164  float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
165  if (eig_sum != 0)
166  curvature = fabsf (eigen_value / eig_sum);
167  else
168  curvature = 0;
169 
170  if (curvature < maximum_curvature_)
171  {
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_indices[i]);
178  centroids.push_back (clust_centroid);
179  covariances.push_back (clust_cov);
180  }
181  }
182  }
183  deinitCompute ();
184 }
185 
186 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
187 template<typename PointT, typename PointNT, typename PointLT> void
189 {
190  std::vector<ModelCoefficients> model_coefficients;
191  std::vector<PointIndices> inlier_indices;
192  PointCloudLPtr labels (new PointCloudL);
193  std::vector<pcl::PointIndices> label_indices;
194  std::vector<pcl::PointIndices> boundary_indices;
195  pcl::PointCloud<PointT> boundary_cloud;
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 ());
201 
202  for (size_t i = 0; i < model_coefficients.size (); i++)
203  {
204  boundary_cloud.resize (0);
205  pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[0], labels, boundary_indices[i]);
206  boundary_cloud.points.resize (boundary_indices[i].indices.size ());
207  for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
208  boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]];
209 
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]);
215  regions[i] = PlanarRegion<PointT> (centroid,
216  covariances[i],
217  static_cast<unsigned int> (inlier_indices[i].indices.size ()),
218  boundary_cloud.points,
219  model);
220  }
221 }
222 
223 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
224 template<typename PointT, typename PointNT, typename PointLT> void
226 {
227  std::vector<ModelCoefficients> model_coefficients;
228  std::vector<PointIndices> inlier_indices;
229  PointCloudLPtr labels (new PointCloudL);
230  std::vector<pcl::PointIndices> label_indices;
231  std::vector<pcl::PointIndices> boundary_indices;
232  pcl::PointCloud<PointT> boundary_cloud;
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, centroids, covariances, labels, label_indices);
237  regions.resize (model_coefficients.size ());
238  boundary_indices.resize (model_coefficients.size ());
239 
240  for (size_t i = 0; i < model_coefficients.size (); i++)
241  {
242  boundary_cloud.resize (0);
243  int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
244  pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
245  boundary_cloud.points.resize (boundary_indices[i].indices.size ());
246  for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
247  boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]];
248 
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]);
254 
255  Eigen::Vector3f vp (0.0, 0.0, 0.0);
256  if (project_points_)
257  boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
258 
259  regions[i] = PlanarRegion<PointT> (centroid,
260  covariances[i],
261  static_cast<unsigned int> (inlier_indices[i].indices.size ()),
262  boundary_cloud.points,
263  model);
264  }
265 }
266 
267 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
268 template<typename PointT, typename PointNT, typename PointLT> void
270  std::vector<ModelCoefficients>& model_coefficients,
271  std::vector<PointIndices>& inlier_indices,
272  PointCloudLPtr& labels,
273  std::vector<pcl::PointIndices>& label_indices,
274  std::vector<pcl::PointIndices>& boundary_indices)
275 {
276  pcl::PointCloud<PointT> boundary_cloud;
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, centroids, covariances, labels, label_indices);
281  regions.resize (model_coefficients.size ());
282  boundary_indices.resize (model_coefficients.size ());
283 
284  for (size_t i = 0; i < model_coefficients.size (); i++)
285  {
286  boundary_cloud.resize (0);
287  int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
288  pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
289  boundary_cloud.points.resize (boundary_indices[i].indices.size ());
290  for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
291  boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]];
292 
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]);
298 
299  Eigen::Vector3f vp (0.0, 0.0, 0.0);
300  if (project_points_ && boundary_cloud.points.size () > 0)
301  boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
302 
303  regions[i] = PlanarRegion<PointT> (centroid,
304  covariances[i],
305  static_cast<unsigned int> (inlier_indices[i].indices.size ()),
306  boundary_cloud.points,
307  model);
308  }
309 }
310 
311 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
312 template<typename PointT, typename PointNT, typename PointLT> void
313 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::refine (std::vector<ModelCoefficients>& model_coefficients,
314  std::vector<PointIndices>& inlier_indices,
315  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&,
316  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >&,
317  PointCloudLPtr& labels,
318  std::vector<pcl::PointIndices>& label_indices)
319 {
320  //List of lables to grow, and index of model corresponding to each label
321  std::vector<bool> grow_labels;
322  std::vector<int> label_to_model;
323  grow_labels.resize (label_indices.size (), false);
324  label_to_model.resize (label_indices.size (), 0);
325 
326  for (size_t i = 0; i < model_coefficients.size (); i++)
327  {
328  int model_label = (*labels)[inlier_indices[i].indices[0]].label;
329  label_to_model[model_label] = static_cast<int> (i);
330  grow_labels[model_label] = true;
331  }
332 
333  //refinement_compare_->setDistanceThreshold (0.015f, true);
334  refinement_compare_->setInputCloud (input_);
335  refinement_compare_->setLabels (labels);
336  refinement_compare_->setModelCoefficients (model_coefficients);
337  refinement_compare_->setRefineLabels (grow_labels);
338  refinement_compare_->setLabelToModel (label_to_model);
339 
340  //Do a first pass over the image, top to bottom, left to right
341  unsigned int current_row = 0;
342  unsigned int next_row = labels->width;
343  for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
344  {
345 
346  for (unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
347  {
348  int current_label = (*labels)[current_row+colIdx].label;
349  int right_label = (*labels)[current_row+colIdx+1].label;
350  if (current_label < 0 || right_label < 0)
351  continue;
352 
353  //Check right
354  //bool test1 = false;
355  if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
356  {
357  //test1 = true;
358  labels->points[current_row+colIdx+1].label = current_label;
359  label_indices[current_label].indices.push_back (current_row+colIdx+1);
360  inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
361  }
362 
363  int lower_label = (*labels)[next_row+colIdx].label;
364  if (lower_label < 0)
365  continue;
366 
367  //Check down
368  if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
369  {
370  labels->points[next_row+colIdx].label = current_label;
371  label_indices[current_label].indices.push_back (next_row+colIdx);
372  inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
373  }
374 
375  }//col
376  }//row
377 
378  //Do a second pass over the image
379  current_row = labels->width * (labels->height - 1);
380  unsigned int prev_row = current_row - labels->width;
381  for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
382  {
383  for (int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
384  {
385  int current_label = (*labels)[current_row+colIdx].label;
386  int left_label = (*labels)[current_row+colIdx-1].label;
387  if (current_label < 0 || left_label < 0)
388  continue;
389 
390  //Check left
391  if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
392  {
393  labels->points[current_row+colIdx-1].label = current_label;
394  label_indices[current_label].indices.push_back (current_row+colIdx-1);
395  inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
396  }
397 
398  int upper_label = (*labels)[prev_row+colIdx].label;
399  if (upper_label < 0)
400  continue;
401  //Check up
402  if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
403  {
404  labels->points[prev_row+colIdx].label = current_label;
405  label_indices[current_label].indices.push_back (prev_row+colIdx);
406  inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
407  }
408  }//col
409  }//row
410 }
411 
412 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>;
413 
414 #endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_
void segment(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &centroids, std::vector< Eigen::Matrix3f, Eigen::aligned_allocator< Eigen::Matrix3f > > &covariances, pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices)
Segmentation of all planes in a point cloud given by setInputCloud(), setIndices() ...
struct pcl::PointXYZIEdge EIGEN_ALIGN16
OrganizedConnectedComponentSegmentation allows connected components to be found within organized poin...
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition: centroid.hpp:489
void segmentAndRefine(std::vector< PlanarRegion< PointT >, Eigen::aligned_allocator< PlanarRegion< PointT > > > &regions)
Perform a segmentation, as well as an additional refinement step.
std::vector< float > values
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
static void findLabeledRegionBoundary(int start_idx, PointCloudLPtr labels, pcl::PointIndices &boundary_indices)
Find the boundary points / contour of a connected component.
void segment(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the connected component segmentation.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:251
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
void refine(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &centroids, std::vector< Eigen::Matrix3f, Eigen::aligned_allocator< Eigen::Matrix3f > > &covariances, PointCloudLPtr &labels, std::vector< pcl::PointIndices > &label_indices)
Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by ...
PlanarRegion represents a set of points that lie in a plane.
Definition: planar_region.h:50