Point Cloud Library (PCL)  1.7.1
sac_model_plane.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
43 
44 #include <pcl/sample_consensus/sac_model_plane.h>
45 #include <pcl/common/centroid.h>
46 #include <pcl/common/eigen.h>
47 #include <pcl/common/concatenate.h>
48 #include <pcl/point_types.h>
49 
50 //////////////////////////////////////////////////////////////////////////
51 template <typename PointT> bool
52 pcl::SampleConsensusModelPlane<PointT>::isSampleGood (const std::vector<int> &samples) const
53 {
54  // Need an extra check in case the sample selection is empty
55  if (samples.empty ())
56  return (false);
57  // Get the values at the two points
58  pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap ();
59  pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap ();
60  pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap ();
61 
62  Eigen::Array4f dy1dy2 = (p1-p0) / (p2-p0);
63 
64  return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) );
65 }
66 
67 //////////////////////////////////////////////////////////////////////////
68 template <typename PointT> bool
70  const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
71 {
72  // Need 3 samples
73  if (samples.size () != 3)
74  {
75  PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
76  return (false);
77  }
78 
79  pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap ();
80  pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap ();
81  pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap ();
82 
83  // Compute the segment values (in 3d) between p1 and p0
84  Eigen::Array4f p1p0 = p1 - p0;
85  // Compute the segment values (in 3d) between p2 and p0
86  Eigen::Array4f p2p0 = p2 - p0;
87 
88  // Avoid some crashes by checking for collinearity here
89  Eigen::Array4f dy1dy2 = p1p0 / p2p0;
90  if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ) // Check for collinearity
91  return (false);
92 
93  // Compute the plane coefficients from the 3 given points in a straightforward manner
94  // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
95  model_coefficients.resize (4);
96  model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
97  model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
98  model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
99  model_coefficients[3] = 0;
100 
101  // Normalize
102  model_coefficients.normalize ();
103 
104  // ... + d = 0
105  model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ()));
106 
107  return (true);
108 }
109 
110 //////////////////////////////////////////////////////////////////////////
111 template <typename PointT> void
113  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
114 {
115  // Needs a valid set of model coefficients
116  if (model_coefficients.size () != 4)
117  {
118  PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
119  return;
120  }
121 
122  distances.resize (indices_->size ());
123 
124  // Iterate through the 3d points and calculate the distances from them to the plane
125  for (size_t i = 0; i < indices_->size (); ++i)
126  {
127  // Calculate the distance from the point to the plane normal as the dot product
128  // D = (P-A).N/|N|
129  /*distances[i] = fabs (model_coefficients[0] * input_->points[(*indices_)[i]].x +
130  model_coefficients[1] * input_->points[(*indices_)[i]].y +
131  model_coefficients[2] * input_->points[(*indices_)[i]].z +
132  model_coefficients[3]);*/
133  Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
134  input_->points[(*indices_)[i]].y,
135  input_->points[(*indices_)[i]].z,
136  1);
137  distances[i] = fabs (model_coefficients.dot (pt));
138  }
139 }
140 
141 //////////////////////////////////////////////////////////////////////////
142 template <typename PointT> void
144  const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
145 {
146  // Needs a valid set of model coefficients
147  if (model_coefficients.size () != 4)
148  {
149  PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
150  return;
151  }
152 
153  int nr_p = 0;
154  inliers.resize (indices_->size ());
155  error_sqr_dists_.resize (indices_->size ());
156 
157  // Iterate through the 3d points and calculate the distances from them to the plane
158  for (size_t i = 0; i < indices_->size (); ++i)
159  {
160  // Calculate the distance from the point to the plane normal as the dot product
161  // D = (P-A).N/|N|
162  Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
163  input_->points[(*indices_)[i]].y,
164  input_->points[(*indices_)[i]].z,
165  1);
166 
167  float distance = fabsf (model_coefficients.dot (pt));
168 
169  if (distance < threshold)
170  {
171  // Returns the indices of the points whose distances are smaller than the threshold
172  inliers[nr_p] = (*indices_)[i];
173  error_sqr_dists_[nr_p] = static_cast<double> (distance);
174  ++nr_p;
175  }
176  }
177  inliers.resize (nr_p);
178  error_sqr_dists_.resize (nr_p);
179 }
180 
181 //////////////////////////////////////////////////////////////////////////
182 template <typename PointT> int
184  const Eigen::VectorXf &model_coefficients, const double threshold)
185 {
186  // Needs a valid set of model coefficients
187  if (model_coefficients.size () != 4)
188  {
189  PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
190  return (0);
191  }
192 
193  int nr_p = 0;
194 
195  // Iterate through the 3d points and calculate the distances from them to the plane
196  for (size_t i = 0; i < indices_->size (); ++i)
197  {
198  // Calculate the distance from the point to the plane normal as the dot product
199  // D = (P-A).N/|N|
200  Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
201  input_->points[(*indices_)[i]].y,
202  input_->points[(*indices_)[i]].z,
203  1);
204  if (fabs (model_coefficients.dot (pt)) < threshold)
205  nr_p++;
206  }
207  return (nr_p);
208 }
209 
210 //////////////////////////////////////////////////////////////////////////
211 template <typename PointT> void
213  const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
214 {
215  // Needs a valid set of model coefficients
216  if (model_coefficients.size () != 4)
217  {
218  PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
219  optimized_coefficients = model_coefficients;
220  return;
221  }
222 
223  // Need at least 3 points to estimate a plane
224  if (inliers.size () < 4)
225  {
226  PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ());
227  optimized_coefficients = model_coefficients;
228  return;
229  }
230 
231  Eigen::Vector4f plane_parameters;
232 
233  // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients
234  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
235  Eigen::Vector4f xyz_centroid;
236 
237  computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, xyz_centroid);
238 
239  // Compute the model coefficients
240  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
241  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
242  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
243 
244  // Hessian form (D = nc . p_plane (centroid here) + p)
245  optimized_coefficients.resize (4);
246  optimized_coefficients[0] = eigen_vector [0];
247  optimized_coefficients[1] = eigen_vector [1];
248  optimized_coefficients[2] = eigen_vector [2];
249  optimized_coefficients[3] = 0;
250  optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
251 
252  // Make sure it results in a valid model
253  if (!isModelValid (optimized_coefficients))
254  {
255  optimized_coefficients = model_coefficients;
256  }
257 }
258 
259 //////////////////////////////////////////////////////////////////////////
260 template <typename PointT> void
262  const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
263 {
264  // Needs a valid set of model coefficients
265  if (model_coefficients.size () != 4)
266  {
267  PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
268  return;
269  }
270 
271  projected_points.header = input_->header;
272  projected_points.is_dense = input_->is_dense;
273 
274  Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
275 
276  // normalize the vector perpendicular to the plane...
277  mc.normalize ();
278  // ... and store the resulting normal as a local copy of the model coefficients
279  Eigen::Vector4f tmp_mc = model_coefficients;
280  tmp_mc[0] = mc[0];
281  tmp_mc[1] = mc[1];
282  tmp_mc[2] = mc[2];
283 
284  // Copy all the data fields from the input cloud to the projected one?
285  if (copy_data_fields)
286  {
287  // Allocate enough space and copy the basics
288  projected_points.points.resize (input_->points.size ());
289  projected_points.width = input_->width;
290  projected_points.height = input_->height;
291 
292  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
293  // Iterate over each point
294  for (size_t i = 0; i < input_->points.size (); ++i)
295  // Iterate over each dimension
296  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
297 
298  // Iterate through the 3d points and calculate the distances from them to the plane
299  for (size_t i = 0; i < inliers.size (); ++i)
300  {
301  // Calculate the distance from the point to the plane
302  Eigen::Vector4f p (input_->points[inliers[i]].x,
303  input_->points[inliers[i]].y,
304  input_->points[inliers[i]].z,
305  1);
306  // use normalized coefficients to calculate the scalar projection
307  float distance_to_plane = tmp_mc.dot (p);
308 
309  pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
310  pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
311  }
312  }
313  else
314  {
315  // Allocate enough space and copy the basics
316  projected_points.points.resize (inliers.size ());
317  projected_points.width = static_cast<uint32_t> (inliers.size ());
318  projected_points.height = 1;
319 
320  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
321  // Iterate over each point
322  for (size_t i = 0; i < inliers.size (); ++i)
323  // Iterate over each dimension
324  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
325 
326  // Iterate through the 3d points and calculate the distances from them to the plane
327  for (size_t i = 0; i < inliers.size (); ++i)
328  {
329  // Calculate the distance from the point to the plane
330  Eigen::Vector4f p (input_->points[inliers[i]].x,
331  input_->points[inliers[i]].y,
332  input_->points[inliers[i]].z,
333  1);
334  // use normalized coefficients to calculate the scalar projection
335  float distance_to_plane = tmp_mc.dot (p);
336 
337  pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
338  pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
339  }
340  }
341 }
342 
343 //////////////////////////////////////////////////////////////////////////
344 template <typename PointT> bool
346  const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
347 {
348  // Needs a valid set of model coefficients
349  if (model_coefficients.size () != 4)
350  {
351  PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
352  return (false);
353  }
354 
355  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
356  {
357  Eigen::Vector4f pt (input_->points[*it].x,
358  input_->points[*it].y,
359  input_->points[*it].z,
360  1);
361  if (fabs (model_coefficients.dot (pt)) > threshold)
362  return (false);
363  }
364 
365  return (true);
366 }
367 
368 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
369 
370 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
371 
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given plane model coefficients.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
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:490
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)
Create a new point cloud with inliers projected onto the plane model.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
struct pcl::_PointXYZHSV EIGEN_ALIGN16
SampleConsensusModelPlane defines a model for 3D plane segmentation.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the plane coefficients using the given inlier set and return them to the user...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.h:304
Helper functor structure for concatenate.
Definition: concatenate.h:64
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given plane model.