Point Cloud Library (PCL)  1.11.0
 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  if (samples.size () != sample_size_)
52  {
53  PCL_ERROR ("[pcl::SampleConsensusModelSphere::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
54  return (false);
55  }
56  return (true);
57 }
58 
59 //////////////////////////////////////////////////////////////////////////
60 template <typename PointT> bool
62  const Indices &samples, Eigen::VectorXf &model_coefficients) const
63 {
64  // Need 4 samples
65  if (samples.size () != sample_size_)
66  {
67  PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
68  return (false);
69  }
70 
71  Eigen::Matrix4f temp;
72  for (int i = 0; i < 4; i++)
73  {
74  temp (i, 0) = input_->points[samples[i]].x;
75  temp (i, 1) = input_->points[samples[i]].y;
76  temp (i, 2) = input_->points[samples[i]].z;
77  temp (i, 3) = 1;
78  }
79  float m11 = temp.determinant ();
80  if (m11 == 0)
81  {
82  return (false); // the points don't define a sphere!
83  }
84 
85  for (int i = 0; i < 4; ++i)
86  {
87  temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
88  (input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
89  (input_->points[samples[i]].z) * (input_->points[samples[i]].z);
90  }
91  float m12 = temp.determinant ();
92 
93  for (int i = 0; i < 4; ++i)
94  {
95  temp (i, 1) = temp (i, 0);
96  temp (i, 0) = input_->points[samples[i]].x;
97  }
98  float m13 = temp.determinant ();
99 
100  for (int i = 0; i < 4; ++i)
101  {
102  temp (i, 2) = temp (i, 1);
103  temp (i, 1) = input_->points[samples[i]].y;
104  }
105  float m14 = temp.determinant ();
106 
107  for (int i = 0; i < 4; ++i)
108  {
109  temp (i, 0) = temp (i, 2);
110  temp (i, 1) = input_->points[samples[i]].x;
111  temp (i, 2) = input_->points[samples[i]].y;
112  temp (i, 3) = input_->points[samples[i]].z;
113  }
114  float m15 = temp.determinant ();
115 
116  // Center (x , y, z)
117  model_coefficients.resize (model_size_);
118  model_coefficients[0] = 0.5f * m12 / m11;
119  model_coefficients[1] = 0.5f * m13 / m11;
120  model_coefficients[2] = 0.5f * m14 / m11;
121  // Radius
122  model_coefficients[3] = std::sqrt (model_coefficients[0] * model_coefficients[0] +
123  model_coefficients[1] * model_coefficients[1] +
124  model_coefficients[2] * model_coefficients[2] - m15 / m11);
125 
126  return (true);
127 }
128 
129 //////////////////////////////////////////////////////////////////////////
130 template <typename PointT> void
132  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
133 {
134  // Check if the model is valid given the user constraints
135  if (!isModelValid (model_coefficients))
136  {
137  distances.clear ();
138  return;
139  }
140  distances.resize (indices_->size ());
141 
142  // Iterate through the 3d points and calculate the distances from them to the sphere
143  for (std::size_t i = 0; i < indices_->size (); ++i)
144  {
145  // Calculate the distance from the point to the sphere as the difference between
146  //dist(point,sphere_origin) and sphere_radius
147  distances[i] = std::abs (std::sqrt (
148  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
149  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
150 
151  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
152  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
153 
154  ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
155  ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
156  ) - model_coefficients[3]);
157  }
158 }
159 
160 //////////////////////////////////////////////////////////////////////////
161 template <typename PointT> void
163  const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
164 {
165  // Check if the model is valid given the user constraints
166  if (!isModelValid (model_coefficients))
167  {
168  inliers.clear ();
169  return;
170  }
171 
172  inliers.clear ();
173  error_sqr_dists_.clear ();
174  inliers.reserve (indices_->size ());
175  error_sqr_dists_.reserve (indices_->size ());
176 
177  // Iterate through the 3d points and calculate the distances from them to the sphere
178  for (std::size_t i = 0; i < indices_->size (); ++i)
179  {
180  double distance = std::abs (std::sqrt (
181  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
182  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
183 
184  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
185  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
186 
187  ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
188  ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
189  ) - model_coefficients[3]);
190  // Calculate the distance from the point to the sphere as the difference between
191  // dist(point,sphere_origin) and sphere_radius
192  if (distance < threshold)
193  {
194  // Returns the indices of the points whose distances are smaller than the threshold
195  inliers.push_back ((*indices_)[i]);
196  error_sqr_dists_.push_back (static_cast<double> (distance));
197  }
198  }
199 }
200 
201 //////////////////////////////////////////////////////////////////////////
202 template <typename PointT> std::size_t
204  const Eigen::VectorXf &model_coefficients, const double threshold) const
205 {
206  // Check if the model is valid given the user constraints
207  if (!isModelValid (model_coefficients))
208  return (0);
209 
210  std::size_t nr_p = 0;
211 
212  // Iterate through the 3d points and calculate the distances from them to the sphere
213  for (std::size_t i = 0; i < indices_->size (); ++i)
214  {
215  // Calculate the distance from the point to the sphere as the difference between
216  // dist(point,sphere_origin) and sphere_radius
217  if (std::abs (std::sqrt (
218  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
219  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
220 
221  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
222  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
223 
224  ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
225  ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
226  ) - model_coefficients[3]) < threshold)
227  nr_p++;
228  }
229  return (nr_p);
230 }
231 
232 //////////////////////////////////////////////////////////////////////////
233 template <typename PointT> void
235  const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
236 {
237  optimized_coefficients = model_coefficients;
238 
239  // Needs a set of valid model coefficients
240  if (!isModelValid (model_coefficients))
241  {
242  PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Given model is invalid!\n");
243  return;
244  }
245 
246  // Need more than the minimum sample size to make a difference
247  if (inliers.size () <= sample_size_)
248  {
249  PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
250  return;
251  }
252 
253  OptimizationFunctor functor (this, inliers);
254  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
255  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
256  int info = lm.minimize (optimized_coefficients);
257 
258  // Compute the L2 norm of the residuals
259  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",
260  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]);
261 }
262 
263 //////////////////////////////////////////////////////////////////////////
264 template <typename PointT> void
266  const Indices &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const
267 {
268  // Needs a valid model coefficients
269  if (!isModelValid (model_coefficients))
270  {
271  PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Given model is invalid!\n");
272  return;
273  }
274 
275  // Allocate enough space and copy the basics
276  projected_points.points.resize (input_->points.size ());
277  projected_points.header = input_->header;
278  projected_points.width = input_->width;
279  projected_points.height = input_->height;
280  projected_points.is_dense = input_->is_dense;
281 
282  PCL_WARN ("[pcl::SampleConsensusModelSphere::projectPoints] Not implemented yet.\n");
283  projected_points.points = input_->points;
284 }
285 
286 //////////////////////////////////////////////////////////////////////////
287 template <typename PointT> bool
289  const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
290 {
291  // Needs a valid model coefficients
292  if (!isModelValid (model_coefficients))
293  {
294  PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Given model is invalid!\n");
295  return (false);
296  }
297 
298  for (const auto &index : indices)
299  {
300  // Calculate the distance from the point to the sphere as the difference between
301  //dist(point,sphere_origin) and sphere_radius
302  if (std::abs (sqrt (
303  ( input_->points[index].x - model_coefficients[0] ) *
304  ( input_->points[index].x - model_coefficients[0] ) +
305  ( input_->points[index].y - model_coefficients[1] ) *
306  ( input_->points[index].y - model_coefficients[1] ) +
307  ( input_->points[index].z - model_coefficients[2] ) *
308  ( input_->points[index].z - model_coefficients[2] )
309  ) - model_coefficients[3]) > threshold)
310  {
311  return (false);
312  }
313  }
314 
315  return (true);
316 }
317 
318 #define PCL_INSTANTIATE_SampleConsensusModelSphere(T) template class PCL_EXPORTS pcl::SampleConsensusModelSphere<T>;
319 
320 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
321 
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 sphere model.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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 sphere model coefficients.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the sphere 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. ...
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid sphere model, compute the model coefficients f...
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
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.
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
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given sphere model.
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407