Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
sac_model_sphere.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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_MODEL_SPHERE_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_SPHERE_H_
43 
44 #include <pcl/sample_consensus/sac_model.h>
45 #include <pcl/sample_consensus/model_types.h>
46 
47 namespace pcl
48 {
49  /** \brief SampleConsensusModelSphere defines a model for 3D sphere segmentation.
50  * The model coefficients are defined as:
51  * - \b center.x : the X coordinate of the sphere's center
52  * - \b center.y : the Y coordinate of the sphere's center
53  * - \b center.z : the Z coordinate of the sphere's center
54  * - \b radius : the sphere's radius
55  *
56  * \author Radu B. Rusu
57  * \ingroup sample_consensus
58  */
59  template <typename PointT>
61  {
62  public:
69 
73 
74  typedef boost::shared_ptr<SampleConsensusModelSphere> Ptr;
75 
76  /** \brief Constructor for base SampleConsensusModelSphere.
77  * \param[in] cloud the input point cloud dataset
78  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
79  */
80  SampleConsensusModelSphere (const PointCloudConstPtr &cloud,
81  bool random = false)
82  : SampleConsensusModel<PointT> (cloud, random)
83  {
84  model_name_ = "SampleConsensusModelSphere";
85  sample_size_ = 4;
86  model_size_ = 4;
87  }
88 
89  /** \brief Constructor for base SampleConsensusModelSphere.
90  * \param[in] cloud the input point cloud dataset
91  * \param[in] indices a vector of point indices to be used from \a cloud
92  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
93  */
94  SampleConsensusModelSphere (const PointCloudConstPtr &cloud,
95  const std::vector<int> &indices,
96  bool random = false)
97  : SampleConsensusModel<PointT> (cloud, indices, random)
98  {
99  model_name_ = "SampleConsensusModelSphere";
100  sample_size_ = 4;
101  model_size_ = 4;
102  }
103 
104  /** \brief Empty destructor */
106 
107  /** \brief Copy constructor.
108  * \param[in] source the model to copy into this
109  */
112  {
113  *this = source;
114  model_name_ = "SampleConsensusModelSphere";
115  }
116 
117  /** \brief Copy constructor.
118  * \param[in] source the model to copy into this
119  */
122  {
124  return (*this);
125  }
126 
127  /** \brief Check whether the given index samples can form a valid sphere model, compute the model
128  * coefficients from these samples and store them internally in model_coefficients.
129  * The sphere coefficients are: x, y, z, R.
130  * \param[in] samples the point indices found as possible good candidates for creating a valid model
131  * \param[out] model_coefficients the resultant model coefficients
132  */
133  bool
134  computeModelCoefficients (const std::vector<int> &samples,
135  Eigen::VectorXf &model_coefficients) const;
136 
137  /** \brief Compute all distances from the cloud data to a given sphere model.
138  * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to
139  * \param[out] distances the resultant estimated distances
140  */
141  void
142  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
143  std::vector<double> &distances) const;
144 
145  /** \brief Select all the points which respect the given model coefficients as inliers.
146  * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to
147  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
148  * \param[out] inliers the resultant model inliers
149  */
150  void
151  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
152  const double threshold,
153  std::vector<int> &inliers);
154 
155  /** \brief Count all the points which respect the given model coefficients as inliers.
156  *
157  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
158  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
159  * \return the resultant number of inliers
160  */
161  virtual int
162  countWithinDistance (const Eigen::VectorXf &model_coefficients,
163  const double threshold) const;
164 
165  /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user.
166  * @note: these are the coefficients of the sphere model after refinement (e.g. after SVD)
167  * \param[in] inliers the data inliers found as supporting the model
168  * \param[in] model_coefficients the initial guess for the optimization
169  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
170  */
171  void
172  optimizeModelCoefficients (const std::vector<int> &inliers,
173  const Eigen::VectorXf &model_coefficients,
174  Eigen::VectorXf &optimized_coefficients) const;
175 
176  /** \brief Create a new point cloud with inliers projected onto the sphere model.
177  * \param[in] inliers the data inliers that we want to project on the sphere model
178  * \param[in] model_coefficients the coefficients of a sphere model
179  * \param[out] projected_points the resultant projected points
180  * \param[in] copy_data_fields set to true if we need to copy the other data fields
181  * \todo implement this.
182  */
183  void
184  projectPoints (const std::vector<int> &inliers,
185  const Eigen::VectorXf &model_coefficients,
186  PointCloud &projected_points,
187  bool copy_data_fields = true) const;
188 
189  /** \brief Verify whether a subset of indices verifies the given sphere model coefficients.
190  * \param[in] indices the data indices that need to be tested against the sphere model
191  * \param[in] model_coefficients the sphere model coefficients
192  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
193  */
194  bool
195  doSamplesVerifyModel (const std::set<int> &indices,
196  const Eigen::VectorXf &model_coefficients,
197  const double threshold) const;
198 
199  /** \brief Return an unique id for this model (SACMODEL_SPHERE). */
200  inline pcl::SacModel getModelType () const { return (SACMODEL_SPHERE); }
201 
202  protected:
205 
206  /** \brief Check whether a model is valid given the user constraints.
207  * \param[in] model_coefficients the set of model coefficients
208  */
209  virtual bool
210  isModelValid (const Eigen::VectorXf &model_coefficients) const
211  {
212  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
213  return (false);
214 
215  if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_)
216  return (false);
217  if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_)
218  return (false);
219 
220  return (true);
221  }
222 
223  /** \brief Check if a sample of indices results in a good sample of points
224  * indices.
225  * \param[in] samples the resultant index samples
226  */
227  bool
228  isSampleGood(const std::vector<int> &samples) const;
229 
230  private:
231 
232 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
233 #pragma GCC diagnostic ignored "-Weffc++"
234 #endif
235  struct OptimizationFunctor : pcl::Functor<float>
236  {
237  /** Functor constructor
238  * \param[in] indices the indices of data points to evaluate
239  * \param[in] estimator pointer to the estimator object
240  */
241  OptimizationFunctor (const pcl::SampleConsensusModelSphere<PointT> *model, const std::vector<int>& indices) :
242  pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
243 
244  /** Cost function to be minimized
245  * \param[in] x the variables array
246  * \param[out] fvec the resultant functions evaluations
247  * \return 0
248  */
249  int
250  operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
251  {
252  Eigen::Vector4f cen_t;
253  cen_t[3] = 0;
254  for (int i = 0; i < values (); ++i)
255  {
256  // Compute the difference between the center of the sphere and the datapoint X_i
257  cen_t[0] = model_->input_->points[indices_[i]].x - x[0];
258  cen_t[1] = model_->input_->points[indices_[i]].y - x[1];
259  cen_t[2] = model_->input_->points[indices_[i]].z - x[2];
260 
261  // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R
262  fvec[i] = std::sqrt (cen_t.dot (cen_t)) - x[3];
263  }
264  return (0);
265  }
266 
268  const std::vector<int> &indices_;
269  };
270 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
271 #pragma GCC diagnostic warning "-Weffc++"
272 #endif
273  };
274 }
275 
276 #ifdef PCL_NO_PRECOMPILE
277 #include <pcl/sample_consensus/impl/sac_model_sphere.hpp>
278 #endif
279 
280 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_SPHERE_H_
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.
double radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:548
virtual ~SampleConsensusModelSphere()
Empty destructor.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::shared_ptr< SampleConsensusModelSphere > Ptr
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:575
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...
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:653
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:540
SampleConsensusModel< PointT >::PointCloud PointCloud
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
SampleConsensusModelSphere(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelSphere.
std::string model_name_
The model name.
Definition: sac_model.h:534
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
SampleConsensusModelSphere & operator=(const SampleConsensusModelSphere &source)
Copy constructor.
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.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModelSphere(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelSphere.
SacModel
Definition: model_types.h:46
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_SPHERE).
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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...
SampleConsensusModelSphere defines a model for 3D sphere segmentation.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given sphere model.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:572
SampleConsensusModelSphere(const SampleConsensusModelSphere &source)
Copy constructor.