Point Cloud Library (PCL)  1.11.1-dev
sac_model_stick.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: sac_model_line.hpp 2328 2011-08-31 08:11:00Z rusu $
38  *
39  */
40 
41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_
43 
44 #include <pcl/sample_consensus/sac_model_stick.h>
45 #include <pcl/common/centroid.h>
46 #include <pcl/common/concatenate.h>
47 #include <pcl/common/eigen.h> // for eigen33
48 
49 //////////////////////////////////////////////////////////////////////////
50 template <typename PointT> bool
52 {
53  if (samples.size () != sample_size_)
54  {
55  PCL_ERROR ("[pcl::SampleConsensusModelStick::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
56  return (false);
57  }
58  if (
59  ((*input_)[samples[0]].x != (*input_)[samples[1]].x)
60  &&
61  ((*input_)[samples[0]].y != (*input_)[samples[1]].y)
62  &&
63  ((*input_)[samples[0]].z != (*input_)[samples[1]].z))
64  {
65  return (true);
66  }
67 
68  return (false);
69 }
70 
71 //////////////////////////////////////////////////////////////////////////
72 template <typename PointT> bool
74  const Indices &samples, Eigen::VectorXf &model_coefficients) const
75 {
76  // Need 2 samples
77  if (samples.size () != sample_size_)
78  {
79  PCL_ERROR ("[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
80  return (false);
81  }
82 
83  model_coefficients.resize (model_size_);
84  model_coefficients[0] = (*input_)[samples[0]].x;
85  model_coefficients[1] = (*input_)[samples[0]].y;
86  model_coefficients[2] = (*input_)[samples[0]].z;
87 
88  model_coefficients[3] = (*input_)[samples[1]].x;
89  model_coefficients[4] = (*input_)[samples[1]].y;
90  model_coefficients[5] = (*input_)[samples[1]].z;
91 
92 // model_coefficients[3] = (*input_)[samples[1]].x - model_coefficients[0];
93 // model_coefficients[4] = (*input_)[samples[1]].y - model_coefficients[1];
94 // model_coefficients[5] = (*input_)[samples[1]].z - model_coefficients[2];
95 
96 // model_coefficients.template segment<3> (3).normalize ();
97  // We don't care about model_coefficients[6] which is the width (radius) of the stick
98 
99  return (true);
100 }
101 
102 //////////////////////////////////////////////////////////////////////////
103 template <typename PointT> void
105  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
106 {
107  // Needs a valid set of model coefficients
108  if (!isModelValid (model_coefficients))
109  {
110  PCL_ERROR ("[pcl::SampleConsensusModelStick::getDistancesToModel] Given model is invalid!\n");
111  return;
112  }
113 
114  float sqr_threshold = static_cast<float> (radius_max_ * radius_max_);
115  distances.resize (indices_->size ());
116 
117  // Obtain the line point and direction
118  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
119  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
120  line_dir.normalize ();
121 
122  // Iterate through the 3d points and calculate the distances from them to the line
123  for (std::size_t i = 0; i < indices_->size (); ++i)
124  {
125  // Calculate the distance from the point to the line
126  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
127  float sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
128 
129  if (sqr_distance < sqr_threshold)
130  {
131  // Need to estimate sqrt here to keep MSAC and friends general
132  distances[i] = sqrt (sqr_distance);
133  }
134  else
135  {
136  // Penalize outliers by doubling the distance
137  distances[i] = 2 * sqrt (sqr_distance);
138  }
139  }
140 }
141 
142 //////////////////////////////////////////////////////////////////////////
143 template <typename PointT> void
145  const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
146 {
147  // Needs a valid set of model coefficients
148  if (!isModelValid (model_coefficients))
149  {
150  PCL_ERROR ("[pcl::SampleConsensusModelStick::selectWithinDistance] Given model is invalid!\n");
151  return;
152  }
153 
154  float sqr_threshold = static_cast<float> (threshold * threshold);
155 
156  inliers.clear ();
157  error_sqr_dists_.clear ();
158  inliers.reserve (indices_->size ());
159  error_sqr_dists_.reserve (indices_->size ());
160 
161  // Obtain the line point and direction
162  Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
163  Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
164  Eigen::Vector4f line_dir = line_pt2 - line_pt1;
165  //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
166  //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
167  line_dir.normalize ();
168  //float norm = line_dir.squaredNorm ();
169 
170  // Iterate through the 3d points and calculate the distances from them to the line
171  for (std::size_t i = 0; i < indices_->size (); ++i)
172  {
173  // Calculate the distance from the point to the line
174  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
175  Eigen::Vector4f dir = (*input_)[(*indices_)[i]].getVector4fMap () - line_pt1;
176  //float u = dir.dot (line_dir);
177 
178  // If the point falls outside of the segment, ignore it
179  //if (u < 0.0f || u > 1.0f)
180  // continue;
181 
182  float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
183  if (sqr_distance < sqr_threshold)
184  {
185  // Returns the indices of the points whose squared distances are smaller than the threshold
186  inliers.push_back ((*indices_)[i]);
187  error_sqr_dists_.push_back (static_cast<double> (sqr_distance));
188  }
189  }
190 }
191 
192 ///////////////////////////////////////////////////////////////////////////
193 template <typename PointT> std::size_t
195  const Eigen::VectorXf &model_coefficients, const double threshold) const
196 {
197  // Needs a valid set of model coefficients
198  if (!isModelValid (model_coefficients))
199  {
200  PCL_ERROR ("[pcl::SampleConsensusModelStick::countWithinDistance] Given model is invalid!\n");
201  return (0);
202  }
203 
204  float sqr_threshold = static_cast<float> (threshold * threshold);
205 
206  std::size_t nr_i = 0, nr_o = 0;
207 
208  // Obtain the line point and direction
209  Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
210  Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
211  Eigen::Vector4f line_dir = line_pt2 - line_pt1;
212  line_dir.normalize ();
213 
214  //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
215  //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
216 
217  // Iterate through the 3d points and calculate the distances from them to the line
218  for (std::size_t i = 0; i < indices_->size (); ++i)
219  {
220  // Calculate the distance from the point to the line
221  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
222  Eigen::Vector4f dir = (*input_)[(*indices_)[i]].getVector4fMap () - line_pt1;
223  //float u = dir.dot (line_dir);
224 
225  // If the point falls outside of the segment, ignore it
226  //if (u < 0.0f || u > 1.0f)
227  // continue;
228 
229  float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
230  // Use a larger threshold (4 times the radius) to get more points in
231  if (sqr_distance < sqr_threshold)
232  {
233  nr_i++;
234  }
235  else if (sqr_distance < 4.0f * sqr_threshold)
236  {
237  nr_o++;
238  }
239  }
240 
241  return (nr_i <= nr_o ? 0 : nr_i - nr_o);
242 }
243 
244 //////////////////////////////////////////////////////////////////////////
245 template <typename PointT> void
247  const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
248 {
249  // Needs a valid set of model coefficients
250  if (!isModelValid (model_coefficients))
251  {
252  optimized_coefficients = model_coefficients;
253  return;
254  }
255 
256  // Need more than the minimum sample size to make a difference
257  if (inliers.size () <= sample_size_)
258  {
259  PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
260  optimized_coefficients = model_coefficients;
261  return;
262  }
263 
264  optimized_coefficients.resize (model_size_);
265 
266  // Compute the 3x3 covariance matrix
267  Eigen::Vector4f centroid;
268  Eigen::Matrix3f covariance_matrix;
269 
270  computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, centroid);
271 
272  optimized_coefficients[0] = centroid[0];
273  optimized_coefficients[1] = centroid[1];
274  optimized_coefficients[2] = centroid[2];
275 
276  // Extract the eigenvalues and eigenvectors
277  Eigen::Vector3f eigen_values;
278  Eigen::Vector3f eigen_vector;
279  pcl::eigen33 (covariance_matrix, eigen_values);
280  pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector);
281 
282  optimized_coefficients.template segment<3> (3).matrix () = eigen_vector;
283 }
284 
285 //////////////////////////////////////////////////////////////////////////
286 template <typename PointT> void
288  const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
289 {
290  // Needs a valid model coefficients
291  if (!isModelValid (model_coefficients))
292  {
293  PCL_ERROR ("[pcl::SampleConsensusModelStick::projectPoints] Given model is invalid!\n");
294  return;
295  }
296 
297  // Obtain the line point and direction
298  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
299  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
300 
301  projected_points.header = input_->header;
302  projected_points.is_dense = input_->is_dense;
303 
304  // Copy all the data fields from the input cloud to the projected one?
305  if (copy_data_fields)
306  {
307  // Allocate enough space and copy the basics
308  projected_points.resize (input_->size ());
309  projected_points.width = input_->width;
310  projected_points.height = input_->height;
311 
312  using FieldList = typename pcl::traits::fieldList<PointT>::type;
313  // Iterate over each point
314  for (std::size_t i = 0; i < projected_points.size (); ++i)
315  {
316  // Iterate over each dimension
317  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
318  }
319 
320  // Iterate through the 3d points and calculate the distances from them to the line
321  for (const auto &inlier : inliers)
322  {
323  Eigen::Vector4f pt ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z, 0.0f);
324  // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
325  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
326 
327  Eigen::Vector4f pp = line_pt + k * line_dir;
328  // Calculate the projection of the point on the line (pointProj = A + k * B)
329  projected_points[inlier].x = pp[0];
330  projected_points[inlier].y = pp[1];
331  projected_points[inlier].z = pp[2];
332  }
333  }
334  else
335  {
336  // Allocate enough space and copy the basics
337  projected_points.resize (inliers.size ());
338  projected_points.width = inliers.size ();
339  projected_points.height = 1;
340 
341  using FieldList = typename pcl::traits::fieldList<PointT>::type;
342  // Iterate over each point
343  for (std::size_t i = 0; i < inliers.size (); ++i)
344  {
345  // Iterate over each dimension
346  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
347  }
348 
349  // Iterate through the 3d points and calculate the distances from them to the line
350  for (std::size_t i = 0; i < inliers.size (); ++i)
351  {
352  Eigen::Vector4f pt ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z, 0.0f);
353  // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
354  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
355 
356  Eigen::Vector4f pp = line_pt + k * line_dir;
357  // Calculate the projection of the point on the line (pointProj = A + k * B)
358  projected_points[i].x = pp[0];
359  projected_points[i].y = pp[1];
360  projected_points[i].z = pp[2];
361  }
362  }
363 }
364 
365 //////////////////////////////////////////////////////////////////////////
366 template <typename PointT> bool
368  const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
369 {
370  // Needs a valid set of model coefficients
371  if (!isModelValid (model_coefficients))
372  {
373  PCL_ERROR ("[pcl::SampleConsensusModelStick::doSamplesVerifyModel] Given model is invalid!\n");
374  return (false);
375  }
376 
377  // Obtain the line point and direction
378  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
379  Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0.0f);
380  //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
381  line_dir.normalize ();
382 
383  float sqr_threshold = static_cast<float> (threshold * threshold);
384  // Iterate through the 3d points and calculate the distances from them to the line
385  for (const auto &index : indices)
386  {
387  // Calculate the distance from the point to the line
388  // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
389  if ((line_pt - (*input_)[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
390  {
391  return (false);
392  }
393  }
394 
395  return (true);
396 }
397 
398 #define PCL_INSTANTIATE_SampleConsensusModelStick(T) template class PCL_EXPORTS pcl::SampleConsensusModelStick<T>;
399 
400 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_
401 
pcl::computeMeanAndCovarianceMatrix
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:485
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::SampleConsensusModelStick::selectWithinDistance
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
Definition: sac_model_stick.hpp:144
pcl::SampleConsensusModelStick::doSamplesVerifyModel
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given stick model coefficients.
Definition: sac_model_stick.hpp:367
pcl::SampleConsensusModelStick::countWithinDistance
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
Definition: sac_model_stick.hpp:194
pcl::SampleConsensusModelStick::computeModelCoefficients
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid stick model, compute the model coefficients fr...
Definition: sac_model_stick.hpp:73
pcl::NdConcatenateFunctor
Helper functor structure for concatenate.
Definition: concatenate.h:49
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::eigen33
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:296
pcl::SampleConsensusModelStick::isSampleGood
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
Definition: sac_model_stick.hpp:51
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::PointCloud::is_dense
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:397
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:386
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::computeCorrespondingEigenVector
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:226
pcl::SampleConsensusModelStick::getDistancesToModel
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all squared distances from the cloud data to a given stick model.
Definition: sac_model_stick.hpp:104
pcl::SampleConsensusModelStick::projectPoints
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the stick model.
Definition: sac_model_stick.hpp:287
centroid.h
pcl::SampleConsensusModelStick::optimizeModelCoefficients
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the stick coefficients using the given inlier set and return them to the user.
Definition: sac_model_stick.hpp:246