Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
organized_multi_plane_segmentation.hpp
Go to the documentation of this file.
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 Willow Garage, Inc. 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/organized_connected_component_segmentation.h>
44 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
45 #include <pcl/common/centroid.h>
46 #include <pcl/common/eigen.h>
47 #include <boost/make_shared.hpp>
48 
50 Eigen::Vector3f linePlaneIntersection (Eigen::Vector3f& p1, Eigen::Vector3f& p2, Eigen::Vector3f& norm, Eigen::Vector3f& p3)
51 {
52  float u = norm.dot ((p3 - p1)) / norm.dot ((p2 - p1));
53  Eigen::Vector3f intersection (p1 + u * (p2 - p1));
54  return (intersection);
55 }
56 
58 template <typename PointT> pcl::PointCloud<PointT>
59 projectToPlaneFromViewpoint (pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp)
60 {
61  Eigen::Vector3f norm (normal[0], normal[1], normal[2]); //(region.coefficients_[0], region.coefficients_[1], region.coefficients_[2]);
62  pcl::PointCloud<PointT> projected_cloud;
63  projected_cloud.resize (cloud.points.size ());
64  for (size_t i = 0; i < cloud.points.size (); i++)
65  {
66  Eigen::Vector3f pt (cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
67  Eigen::Vector3f intersection = linePlaneIntersection (vp, pt, norm, centroid);
68  projected_cloud[i].x = intersection[0];
69  projected_cloud[i].y = intersection[1];
70  projected_cloud[i].z = intersection[2];
71  }
72 
73  return (projected_cloud);
74 }
75 
77 template<typename PointT, typename PointNT, typename PointLT> void
78 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
79  std::vector<PointIndices>& inlier_indices)
80 {
81  pcl::PointCloud<pcl::Label> labels;
82  std::vector<pcl::PointIndices> label_indices;
83  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
84  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
85  segment (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
86 }
87 
89 template<typename PointT, typename PointNT, typename PointLT> void
90 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
91  std::vector<PointIndices>& inlier_indices,
92  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
93  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
94  pcl::PointCloud<PointLT>& labels,
95  std::vector<pcl::PointIndices>& label_indices)
96 {
97  if (!initCompute ())
98  return;
99 
100  // Check that we got the same number of points and normals
101  if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
102  {
103  PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n",
104  getClassName ().c_str (), input_->points.size (),
105  normals_->points.size ());
106  return;
107  }
108 
109  // Check that the cloud is organized
110  if (!input_->isOrganized ())
111  {
112  PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
113  getClassName ().c_str ());
114  return;
115  }
116 
117  // Calculate range part of planes' hessian normal form
118  std::vector<float> plane_d (input_->points.size ());
119 
120  for (unsigned int i = 0; i < input_->size (); ++i)
121  plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
122 
123  // Make a comparator
124  //PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d);
125  compare_->setPlaneCoeffD (plane_d);
126  compare_->setInputCloud (input_);
127  compare_->setInputNormals (normals_);
128  compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
129  compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true);
130 
131  // Set up the output
132  OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_);
133  connected_component.setInputCloud (input_);
134  connected_component.segment (labels, label_indices);
135 
136  Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
137  Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
138  Eigen::Matrix3f clust_cov;
139  pcl::ModelCoefficients model;
140  model.values.resize (4);
141 
142  // Fit Planes to each cluster
143  for (size_t i = 0; i < label_indices.size (); i++)
144  {
145  if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_)
146  {
147  pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid);
148  Eigen::Vector4f plane_params;
149 
150  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
151  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
152  pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
153  plane_params[0] = eigen_vector[0];
154  plane_params[1] = eigen_vector[1];
155  plane_params[2] = eigen_vector[2];
156  plane_params[3] = 0;
157  plane_params[3] = -1 * plane_params.dot (clust_centroid);
158 
159  vp -= clust_centroid;
160  float cos_theta = vp.dot (plane_params);
161  if (cos_theta < 0)
162  {
163  plane_params *= -1;
164  plane_params[3] = 0;
165  plane_params[3] = -1 * plane_params.dot (clust_centroid);
166  }
167 
168  // Compute the curvature surface change
169  float curvature;
170  float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
171  if (eig_sum != 0)
172  curvature = fabsf (eigen_value / eig_sum);
173  else
174  curvature = 0;
175 
176  if (curvature < maximum_curvature_)
177  {
178  model.values[0] = plane_params[0];
179  model.values[1] = plane_params[1];
180  model.values[2] = plane_params[2];
181  model.values[3] = plane_params[3];
182  model_coefficients.push_back (model);
183  inlier_indices.push_back (label_indices[i]);
184  centroids.push_back (clust_centroid);
185  covariances.push_back (clust_cov);
186  }
187  }
188  }
189  deinitCompute ();
190 }
191 
193 template<typename PointT, typename PointNT, typename PointLT> void
194 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions)
195 {
196  std::vector<ModelCoefficients> model_coefficients;
197  std::vector<PointIndices> inlier_indices;
198  PointCloudLPtr labels (new PointCloudL);
199  std::vector<pcl::PointIndices> label_indices;
200  std::vector<pcl::PointIndices> boundary_indices;
201  pcl::PointCloud<PointT> boundary_cloud;
202  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
203  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
204  segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
205  regions.resize (model_coefficients.size ());
206  boundary_indices.resize (model_coefficients.size ());
207 
208  for (size_t i = 0; i < model_coefficients.size (); i++)
209  {
210  boundary_cloud.resize (0);
211  pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[0], labels, boundary_indices[i]);
212  boundary_cloud.points.resize (boundary_indices[i].indices.size ());
213  for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
214  boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]];
215 
216  Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
217  Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
218  model_coefficients[i].values[1],
219  model_coefficients[i].values[2],
220  model_coefficients[i].values[3]);
221  regions[i] = PlanarRegion<PointT> (centroid,
222  covariances[i],
223  static_cast<unsigned int> (inlier_indices[i].indices.size ()),
224  boundary_cloud.points,
225  model);
226  }
227 }
228 
230 template<typename PointT, typename PointNT, typename PointLT> void
231 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions)
232 {
233  std::vector<ModelCoefficients> model_coefficients;
234  std::vector<PointIndices> inlier_indices;
235  PointCloudLPtr labels (new PointCloudL);
236  std::vector<pcl::PointIndices> label_indices;
237  std::vector<pcl::PointIndices> boundary_indices;
238  pcl::PointCloud<PointT> boundary_cloud;
239  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
240  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
241  segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
242  refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
243  regions.resize (model_coefficients.size ());
244  boundary_indices.resize (model_coefficients.size ());
245 
246  for (size_t i = 0; i < model_coefficients.size (); i++)
247  {
248  boundary_cloud.resize (0);
249  int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
250  pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
251  boundary_cloud.points.resize (boundary_indices[i].indices.size ());
252  for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
253  boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]];
254 
255  Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
256  Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
257  model_coefficients[i].values[1],
258  model_coefficients[i].values[2],
259  model_coefficients[i].values[3]);
260 
261  Eigen::Vector3f vp (0.0, 0.0, 0.0);
262  if (project_points_)
263  boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
264 
265  regions[i] = PlanarRegion<PointT> (centroid,
266  covariances[i],
267  static_cast<unsigned int> (inlier_indices[i].indices.size ()),
268  boundary_cloud.points,
269  model);
270  }
271 }
272 
274 template<typename PointT, typename PointNT, typename PointLT> void
275 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions,
276  std::vector<ModelCoefficients>& model_coefficients,
277  std::vector<PointIndices>& inlier_indices,
278  PointCloudLPtr& labels,
279  std::vector<pcl::PointIndices>& label_indices,
280  std::vector<pcl::PointIndices>& boundary_indices)
281 {
282  pcl::PointCloud<PointT> boundary_cloud;
283  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
284  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
285  segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
286  refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
287  regions.resize (model_coefficients.size ());
288  boundary_indices.resize (model_coefficients.size ());
289 
290  for (size_t i = 0; i < model_coefficients.size (); i++)
291  {
292  boundary_cloud.resize (0);
293  int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
294  pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
295  boundary_cloud.points.resize (boundary_indices[i].indices.size ());
296  for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
297  boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]];
298 
299  Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
300  Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
301  model_coefficients[i].values[1],
302  model_coefficients[i].values[2],
303  model_coefficients[i].values[3]);
304 
305  Eigen::Vector3f vp (0.0, 0.0, 0.0);
306  if (project_points_ && boundary_cloud.points.size () > 0)
307  boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
308 
309  regions[i] = PlanarRegion<PointT> (centroid,
310  covariances[i],
311  static_cast<unsigned int> (inlier_indices[i].indices.size ()),
312  boundary_cloud.points,
313  model);
314  }
315 }
316 
318 template<typename PointT, typename PointNT, typename PointLT> void
319 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::refine (std::vector<ModelCoefficients>& model_coefficients,
320  std::vector<PointIndices>& inlier_indices,
321  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&,
322  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >&,
323  PointCloudLPtr& labels,
324  std::vector<pcl::PointIndices>& label_indices)
325 {
326  //List of lables to grow, and index of model corresponding to each label
327  std::vector<bool> grow_labels;
328  std::vector<int> label_to_model;
329  grow_labels.resize (label_indices.size (), false);
330  label_to_model.resize (label_indices.size (), 0);
331 
332  for (size_t i = 0; i < model_coefficients.size (); i++)
333  {
334  int model_label = (*labels)[inlier_indices[i].indices[0]].label;
335  label_to_model[model_label] = static_cast<int> (i);
336  grow_labels[model_label] = true;
337  }
338 
339  //refinement_compare_->setDistanceThreshold (0.015f, true);
340  refinement_compare_->setInputCloud (input_);
341  refinement_compare_->setLabels (labels);
342  refinement_compare_->setModelCoefficients (model_coefficients);
343  refinement_compare_->setRefineLabels (grow_labels);
344  refinement_compare_->setLabelToModel (label_to_model);
345 
346  //Do a first pass over the image, top to bottom, left to right
347  unsigned int current_row = 0;
348  unsigned int next_row = labels->width;
349  for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
350  {
351 
352  for (unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
353  {
354  int current_label = (*labels)[current_row+colIdx].label;
355  int right_label = (*labels)[current_row+colIdx+1].label;
356  if (current_label < 0 || right_label < 0)
357  continue;
358 
359  //Check right
360  //bool test1 = false;
361  if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
362  {
363  //test1 = true;
364  labels->points[current_row+colIdx+1].label = current_label;
365  label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
366  inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
367  }
368 
369  int lower_label = (*labels)[next_row+colIdx].label;
370  if (lower_label < 0)
371  continue;
372 
373  //Check down
374  if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
375  {
376  labels->points[next_row+colIdx].label = current_label;
377  label_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
378  inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
379  }
380 
381  }//col
382  }//row
383 
384  //Do a second pass over the image
385  current_row = labels->width * (labels->height - 1);
386  unsigned int prev_row = current_row - labels->width;
387  for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
388  {
389  for (int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
390  {
391  int current_label = (*labels)[current_row+colIdx].label;
392  int left_label = (*labels)[current_row+colIdx-1].label;
393  if (current_label < 0 || left_label < 0)
394  continue;
395 
396  //Check left
397  if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
398  {
399  labels->points[current_row+colIdx-1].label = current_label;
400  label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
401  inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
402  }
403 
404  int upper_label = (*labels)[prev_row+colIdx].label;
405  if (upper_label < 0)
406  continue;
407  //Check up
408  if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
409  {
410  labels->points[prev_row+colIdx].label = current_label;
411  label_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
412  inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
413  }
414  }//col
415  }//row
416 }
417 
418 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>;
419 
420 #endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_
Generated on Fri Mar 8 2013 12:50:56 for Point Cloud Library (PCL) by   doxygen 1.8.3.1