Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
sac_model_sphere.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_SPHERE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
43 
44 #include <pcl/sample_consensus/eigen.h>
45 #include <pcl/sample_consensus/sac_model_sphere.h>
46 
47 //////////////////////////////////////////////////////////////////////////
48 template <typename PointT> bool
50 {
51  return (true);
52 }
53 
54 //////////////////////////////////////////////////////////////////////////
55 template <typename PointT> bool
57  const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
58 {
59  // Need 4 samples
60  if (samples.size () != 4)
61  {
62  PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
63  return (false);
64  }
65 
66  Eigen::Matrix4f temp;
67  for (int i = 0; i < 4; i++)
68  {
69  temp (i, 0) = input_->points[samples[i]].x;
70  temp (i, 1) = input_->points[samples[i]].y;
71  temp (i, 2) = input_->points[samples[i]].z;
72  temp (i, 3) = 1;
73  }
74  float m11 = temp.determinant ();
75  if (m11 == 0)
76  return (false); // the points don't define a sphere!
77 
78  for (int i = 0; i < 4; ++i)
79  temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
80  (input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
81  (input_->points[samples[i]].z) * (input_->points[samples[i]].z);
82  float m12 = temp.determinant ();
83 
84  for (int i = 0; i < 4; ++i)
85  {
86  temp (i, 1) = temp (i, 0);
87  temp (i, 0) = input_->points[samples[i]].x;
88  }
89  float m13 = temp.determinant ();
90 
91  for (int i = 0; i < 4; ++i)
92  {
93  temp (i, 2) = temp (i, 1);
94  temp (i, 1) = input_->points[samples[i]].y;
95  }
96  float m14 = temp.determinant ();
97 
98  for (int i = 0; i < 4; ++i)
99  {
100  temp (i, 0) = temp (i, 2);
101  temp (i, 1) = input_->points[samples[i]].x;
102  temp (i, 2) = input_->points[samples[i]].y;
103  temp (i, 3) = input_->points[samples[i]].z;
104  }
105  float m15 = temp.determinant ();
106 
107  // Center (x , y, z)
108  model_coefficients.resize (4);
109  model_coefficients[0] = 0.5f * m12 / m11;
110  model_coefficients[1] = 0.5f * m13 / m11;
111  model_coefficients[2] = 0.5f * m14 / m11;
112  // Radius
113  model_coefficients[3] = std::sqrt (model_coefficients[0] * model_coefficients[0] +
114  model_coefficients[1] * model_coefficients[1] +
115  model_coefficients[2] * model_coefficients[2] - m15 / m11);
116 
117  return (true);
118 }
119 
120 //////////////////////////////////////////////////////////////////////////
121 template <typename PointT> void
123  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
124 {
125  // Check if the model is valid given the user constraints
126  if (!isModelValid (model_coefficients))
127  {
128  distances.clear ();
129  return;
130  }
131  distances.resize (indices_->size ());
132 
133  // Iterate through the 3d points and calculate the distances from them to the sphere
134  for (size_t i = 0; i < indices_->size (); ++i)
135  // Calculate the distance from the point to the sphere as the difference between
136  //dist(point,sphere_origin) and sphere_radius
137  distances[i] = fabs (std::sqrt (
138  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
139  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
140 
141  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
142  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
143 
144  ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
145  ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
146  ) - model_coefficients[3]);
147 }
148 
149 //////////////////////////////////////////////////////////////////////////
150 template <typename PointT> void
152  const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
153 {
154  // Check if the model is valid given the user constraints
155  if (!isModelValid (model_coefficients))
156  {
157  inliers.clear ();
158  return;
159  }
160 
161  int nr_p = 0;
162  inliers.resize (indices_->size ());
163  error_sqr_dists_.resize (indices_->size ());
164 
165  // Iterate through the 3d points and calculate the distances from them to the sphere
166  for (size_t i = 0; i < indices_->size (); ++i)
167  {
168  double distance = fabs (std::sqrt (
169  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
170  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
171 
172  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
173  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
174 
175  ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
176  ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
177  ) - model_coefficients[3]);
178  // Calculate the distance from the point to the sphere as the difference between
179  // dist(point,sphere_origin) and sphere_radius
180  if (distance < threshold)
181  {
182  // Returns the indices of the points whose distances are smaller than the threshold
183  inliers[nr_p] = (*indices_)[i];
184  error_sqr_dists_[nr_p] = static_cast<double> (distance);
185  ++nr_p;
186  }
187  }
188  inliers.resize (nr_p);
189  error_sqr_dists_.resize (nr_p);
190 }
191 
192 //////////////////////////////////////////////////////////////////////////
193 template <typename PointT> int
195  const Eigen::VectorXf &model_coefficients, const double threshold) const
196 {
197  // Check if the model is valid given the user constraints
198  if (!isModelValid (model_coefficients))
199  return (0);
200 
201  int nr_p = 0;
202 
203  // Iterate through the 3d points and calculate the distances from them to the sphere
204  for (size_t i = 0; i < indices_->size (); ++i)
205  {
206  // Calculate the distance from the point to the sphere as the difference between
207  // dist(point,sphere_origin) and sphere_radius
208  if (fabs (std::sqrt (
209  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
210  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
211 
212  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
213  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
214 
215  ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
216  ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
217  ) - model_coefficients[3]) < threshold)
218  nr_p++;
219  }
220  return (nr_p);
221 }
222 
223 //////////////////////////////////////////////////////////////////////////
224 template <typename PointT> void
226  const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
227 {
228  optimized_coefficients = model_coefficients;
229 
230  // Needs a set of valid model coefficients
231  if (model_coefficients.size () != 4)
232  {
233  PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
234  return;
235  }
236 
237  // Need at least 4 samples
238  if (inliers.size () <= 4)
239  {
240  PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
241  return;
242  }
243 
244  OptimizationFunctor functor (this, inliers);
245  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
246  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
247  int info = lm.minimize (optimized_coefficients);
248 
249  // Compute the L2 norm of the residuals
250  PCL_DEBUG ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g \nFinal solution: %g %g %g %g\n",
251  info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3]);
252 }
253 
254 //////////////////////////////////////////////////////////////////////////
255 template <typename PointT> void
257  const std::vector<int> &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const
258 {
259  // Needs a valid model coefficients
260  if (model_coefficients.size () != 4)
261  {
262  PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
263  return;
264  }
265 
266  // Allocate enough space and copy the basics
267  projected_points.points.resize (input_->points.size ());
268  projected_points.header = input_->header;
269  projected_points.width = input_->width;
270  projected_points.height = input_->height;
271  projected_points.is_dense = input_->is_dense;
272 
273  PCL_WARN ("[pcl::SampleConsensusModelSphere::projectPoints] Not implemented yet.\n");
274  projected_points.points = input_->points;
275 }
276 
277 //////////////////////////////////////////////////////////////////////////
278 template <typename PointT> bool
280  const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
281 {
282  // Needs a valid model coefficients
283  if (model_coefficients.size () != 4)
284  {
285  PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
286  return (false);
287  }
288 
289  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
290  // Calculate the distance from the point to the sphere as the difference between
291  //dist(point,sphere_origin) and sphere_radius
292  if (fabs (sqrt (
293  ( input_->points[*it].x - model_coefficients[0] ) *
294  ( input_->points[*it].x - model_coefficients[0] ) +
295  ( input_->points[*it].y - model_coefficients[1] ) *
296  ( input_->points[*it].y - model_coefficients[1] ) +
297  ( input_->points[*it].z - model_coefficients[2] ) *
298  ( input_->points[*it].z - model_coefficients[2] )
299  ) - model_coefficients[3]) > threshold)
300  return (false);
301 
302  return (true);
303 }
304 
305 #define PCL_INSTANTIATE_SampleConsensusModelSphere(T) template class PCL_EXPORTS pcl::SampleConsensusModelSphere<T>;
306 
307 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
308 
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 sphere model coefficients.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the sphere coefficients using the given inlier set and return them to the user...
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.
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 sphere model.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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 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
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid sphere model, compute the model coefficients f...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given sphere model.
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407