Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
sac_model_line.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_LINE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_
43 
44 #include <pcl/sample_consensus/sac_model_line.h>
45 #include <pcl/common/centroid.h>
46 #include <pcl/common/concatenate.h>
47 
48 //////////////////////////////////////////////////////////////////////////
49 template <typename PointT> bool
50 pcl::SampleConsensusModelLine<PointT>::isSampleGood (const std::vector<int> &samples) const
51 {
52  if (
53  (input_->points[samples[0]].x != input_->points[samples[1]].x)
54  &&
55  (input_->points[samples[0]].y != input_->points[samples[1]].y)
56  &&
57  (input_->points[samples[0]].z != input_->points[samples[1]].z))
58  return (true);
59 
60  return (false);
61 }
62 
63 //////////////////////////////////////////////////////////////////////////
64 template <typename PointT> bool
66  const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
67 {
68  // Need 2 samples
69  if (samples.size () != 2)
70  {
71  PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
72  return (false);
73  }
74 
75  if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
76  fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
77  fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
78  {
79  return (false);
80  }
81 
82  model_coefficients.resize (6);
83  model_coefficients[0] = input_->points[samples[0]].x;
84  model_coefficients[1] = input_->points[samples[0]].y;
85  model_coefficients[2] = input_->points[samples[0]].z;
86 
87  model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0];
88  model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1];
89  model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2];
90 
91  model_coefficients.template tail<3> ().normalize ();
92  return (true);
93 }
94 
95 //////////////////////////////////////////////////////////////////////////
96 template <typename PointT> void
98  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
99 {
100  // Needs a valid set of model coefficients
101  if (!isModelValid (model_coefficients))
102  return;
103 
104  distances.resize (indices_->size ());
105 
106  // Obtain the line point and direction
107  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
108  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
109  line_dir.normalize ();
110 
111  // Iterate through the 3d points and calculate the distances from them to the line
112  for (size_t i = 0; i < indices_->size (); ++i)
113  {
114  // Calculate the distance from the point to the line
115  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
116  // Need to estimate sqrt here to keep MSAC and friends general
117  distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
118  }
119 }
120 
121 //////////////////////////////////////////////////////////////////////////
122 template <typename PointT> void
124  const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
125 {
126  // Needs a valid set of model coefficients
127  if (!isModelValid (model_coefficients))
128  return;
129 
130  double sqr_threshold = threshold * threshold;
131 
132  int nr_p = 0;
133  inliers.resize (indices_->size ());
134  error_sqr_dists_.resize (indices_->size ());
135 
136  // Obtain the line point and direction
137  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
138  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
139  line_dir.normalize ();
140 
141  // Iterate through the 3d points and calculate the distances from them to the line
142  for (size_t i = 0; i < indices_->size (); ++i)
143  {
144  // Calculate the distance from the point to the line
145  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
146  double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
147 
148  if (sqr_distance < sqr_threshold)
149  {
150  // Returns the indices of the points whose squared distances are smaller than the threshold
151  inliers[nr_p] = (*indices_)[i];
152  error_sqr_dists_[nr_p] = sqr_distance;
153  ++nr_p;
154  }
155  }
156  inliers.resize (nr_p);
157  error_sqr_dists_.resize (nr_p);
158 }
159 
160 //////////////////////////////////////////////////////////////////////////
161 template <typename PointT> int
163  const Eigen::VectorXf &model_coefficients, const double threshold) const
164 {
165  // Needs a valid set of model coefficients
166  if (!isModelValid (model_coefficients))
167  return (0);
168 
169  double sqr_threshold = threshold * threshold;
170 
171  int nr_p = 0;
172 
173  // Obtain the line point and direction
174  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
175  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
176  line_dir.normalize ();
177 
178  // Iterate through the 3d points and calculate the distances from them to the line
179  for (size_t i = 0; i < indices_->size (); ++i)
180  {
181  // Calculate the distance from the point to the line
182  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
183  double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
184 
185  if (sqr_distance < sqr_threshold)
186  nr_p++;
187  }
188  return (nr_p);
189 }
190 
191 //////////////////////////////////////////////////////////////////////////
192 template <typename PointT> void
194  const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
195 {
196  // Needs a valid set of model coefficients
197  if (!isModelValid (model_coefficients))
198  {
199  optimized_coefficients = model_coefficients;
200  return;
201  }
202 
203  // Need at least 2 points to estimate a line
204  if (inliers.size () <= 2)
205  {
206  PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
207  optimized_coefficients = model_coefficients;
208  return;
209  }
210 
211  optimized_coefficients.resize (6);
212 
213  // Compute the 3x3 covariance matrix
214  Eigen::Vector4f centroid;
215  compute3DCentroid (*input_, inliers, centroid);
216  Eigen::Matrix3f covariance_matrix;
217  computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix);
218  optimized_coefficients[0] = centroid[0];
219  optimized_coefficients[1] = centroid[1];
220  optimized_coefficients[2] = centroid[2];
221 
222  // Extract the eigenvalues and eigenvectors
223  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
224  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
225  pcl::eigen33 (covariance_matrix, eigen_values);
226  pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector);
227  //pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
228 
229  optimized_coefficients.template tail<3> ().matrix () = eigen_vector;
230 }
231 
232 //////////////////////////////////////////////////////////////////////////
233 template <typename PointT> void
235  const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
236 {
237  // Needs a valid model coefficients
238  if (!isModelValid (model_coefficients))
239  return;
240 
241  // Obtain the line point and direction
242  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
243  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
244 
245  projected_points.header = input_->header;
246  projected_points.is_dense = input_->is_dense;
247 
248  // Copy all the data fields from the input cloud to the projected one?
249  if (copy_data_fields)
250  {
251  // Allocate enough space and copy the basics
252  projected_points.points.resize (input_->points.size ());
253  projected_points.width = input_->width;
254  projected_points.height = input_->height;
255 
256  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
257  // Iterate over each point
258  for (size_t i = 0; i < projected_points.points.size (); ++i)
259  // Iterate over each dimension
260  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
261 
262  // Iterate through the 3d points and calculate the distances from them to the line
263  for (size_t i = 0; i < inliers.size (); ++i)
264  {
265  Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
266  // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
267  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
268 
269  Eigen::Vector4f pp = line_pt + k * line_dir;
270  // Calculate the projection of the point on the line (pointProj = A + k * B)
271  projected_points.points[inliers[i]].x = pp[0];
272  projected_points.points[inliers[i]].y = pp[1];
273  projected_points.points[inliers[i]].z = pp[2];
274  }
275  }
276  else
277  {
278  // Allocate enough space and copy the basics
279  projected_points.points.resize (inliers.size ());
280  projected_points.width = static_cast<uint32_t> (inliers.size ());
281  projected_points.height = 1;
282 
283  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
284  // Iterate over each point
285  for (size_t i = 0; i < inliers.size (); ++i)
286  // Iterate over each dimension
287  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
288 
289  // Iterate through the 3d points and calculate the distances from them to the line
290  for (size_t i = 0; i < inliers.size (); ++i)
291  {
292  Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
293  // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
294  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
295 
296  Eigen::Vector4f pp = line_pt + k * line_dir;
297  // Calculate the projection of the point on the line (pointProj = A + k * B)
298  projected_points.points[i].x = pp[0];
299  projected_points.points[i].y = pp[1];
300  projected_points.points[i].z = pp[2];
301  }
302  }
303 }
304 
305 //////////////////////////////////////////////////////////////////////////
306 template <typename PointT> bool
308  const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
309 {
310  // Needs a valid set of model coefficients
311  if (!isModelValid (model_coefficients))
312  return (false);
313 
314  // Obtain the line point and direction
315  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
316  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
317  line_dir.normalize ();
318 
319  double sqr_threshold = threshold * threshold;
320  // Iterate through the 3d points and calculate the distances from them to the line
321  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
322  {
323  // Calculate the distance from the point to the line
324  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
325  if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
326  return (false);
327  }
328 
329  return (true);
330 }
331 
332 #define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>;
333 
334 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_
335 
void computeCorrespondingEigenVector(const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi defin...
Definition: eigen.hpp:219
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
struct pcl::PointXYZIEdge EIGEN_ALIGN16
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid line model, compute the model coefficients fro...
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
Verify whether a subset of indices verifies the given line model coefficients.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all squared distances from the cloud data to a given line model.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
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
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the line 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 projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const
Create a new point cloud with inliers projected onto the line model.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
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.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50
Helper functor structure for concatenate.
Definition: concatenate.h:64
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407