Point Cloud Library (PCL)  1.7.1
sac_model_circle.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_CIRCLE2D_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_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 SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane.
50  *
51  * The model coefficients are defined as:
52  * - \b center.x : the X coordinate of the circle's center
53  * - \b center.y : the Y coordinate of the circle's center
54  * - \b radius : the circle's radius
55  *
56  * \author Radu B. Rusu
57  * \ingroup sample_consensus
58  */
59  template <typename PointT>
61  {
62  public:
68 
72 
73  typedef boost::shared_ptr<SampleConsensusModelCircle2D> Ptr;
74 
75  /** \brief Constructor for base SampleConsensusModelCircle2D.
76  * \param[in] cloud the input point cloud dataset
77  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
78  */
79  SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, bool random = false)
80  : SampleConsensusModel<PointT> (cloud, random), tmp_inliers_ ()
81  {};
82 
83  /** \brief Constructor for base SampleConsensusModelCircle2D.
84  * \param[in] cloud the input point cloud dataset
85  * \param[in] indices a vector of point indices to be used from \a cloud
86  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
87  */
88  SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud,
89  const std::vector<int> &indices,
90  bool random = false)
91  : SampleConsensusModel<PointT> (cloud, indices, random), tmp_inliers_ ()
92  {};
93 
94  /** \brief Copy constructor.
95  * \param[in] source the model to copy into this
96  */
98  SampleConsensusModel<PointT> (), tmp_inliers_ ()
99  {
100  *this = source;
101  }
102 
103  /** \brief Empty destructor */
105 
106  /** \brief Copy constructor.
107  * \param[in] source the model to copy into this
108  */
111  {
113  tmp_inliers_ = source.tmp_inliers_;
114  return (*this);
115  }
116 
117  /** \brief Check whether the given index samples can form a valid 2D circle model, compute the model coefficients
118  * from these samples and store them in model_coefficients. The circle coefficients are: x, y, R.
119  * \param[in] samples the point indices found as possible good candidates for creating a valid model
120  * \param[out] model_coefficients the resultant model coefficients
121  */
122  bool
123  computeModelCoefficients (const std::vector<int> &samples,
124  Eigen::VectorXf &model_coefficients);
125 
126  /** \brief Compute all distances from the cloud data to a given 2D circle model.
127  * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to
128  * \param[out] distances the resultant estimated distances
129  */
130  void
131  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
132  std::vector<double> &distances);
133 
134  /** \brief Compute all distances from the cloud data to a given 2D circle model.
135  * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to
136  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
137  * \param[out] inliers the resultant model inliers
138  */
139  void
140  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
141  const double threshold,
142  std::vector<int> &inliers);
143 
144  /** \brief Count all the points which respect the given model coefficients as inliers.
145  *
146  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
147  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
148  * \return the resultant number of inliers
149  */
150  virtual int
151  countWithinDistance (const Eigen::VectorXf &model_coefficients,
152  const double threshold);
153 
154  /** \brief Recompute the 2d circle coefficients using the given inlier set and return them to the user.
155  * @note: these are the coefficients of the 2d circle model after refinement (eg. after SVD)
156  * \param[in] inliers the data inliers found as supporting the model
157  * \param[in] model_coefficients the initial guess for the optimization
158  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
159  */
160  void
161  optimizeModelCoefficients (const std::vector<int> &inliers,
162  const Eigen::VectorXf &model_coefficients,
163  Eigen::VectorXf &optimized_coefficients);
164 
165  /** \brief Create a new point cloud with inliers projected onto the 2d circle model.
166  * \param[in] inliers the data inliers that we want to project on the 2d circle model
167  * \param[in] model_coefficients the coefficients of a 2d circle model
168  * \param[out] projected_points the resultant projected points
169  * \param[in] copy_data_fields set to true if we need to copy the other data fields
170  */
171  void
172  projectPoints (const std::vector<int> &inliers,
173  const Eigen::VectorXf &model_coefficients,
174  PointCloud &projected_points,
175  bool copy_data_fields = true);
176 
177  /** \brief Verify whether a subset of indices verifies the given 2d circle model coefficients.
178  * \param[in] indices the data indices that need to be tested against the 2d circle model
179  * \param[in] model_coefficients the 2d circle model coefficients
180  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
181  */
182  bool
183  doSamplesVerifyModel (const std::set<int> &indices,
184  const Eigen::VectorXf &model_coefficients,
185  const double threshold);
186 
187  /** \brief Return an unique id for this model (SACMODEL_CIRCLE2D). */
188  inline pcl::SacModel
189  getModelType () const { return (SACMODEL_CIRCLE2D); }
190 
191  protected:
192  /** \brief Check whether a model is valid given the user constraints.
193  * \param[in] model_coefficients the set of model coefficients
194  */
195  bool
196  isModelValid (const Eigen::VectorXf &model_coefficients);
197 
198  /** \brief Check if a sample of indices results in a good sample of points indices.
199  * \param[in] samples the resultant index samples
200  */
201  bool
202  isSampleGood(const std::vector<int> &samples) const;
203 
204  private:
205  /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */
206  const std::vector<int> *tmp_inliers_;
207 
208 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
209 #pragma GCC diagnostic ignored "-Weffc++"
210 #endif
211  /** \brief Functor for the optimization function */
212  struct OptimizationFunctor : pcl::Functor<float>
213  {
214  /** \brief Functor constructor
215  * \param[in] m_data_points the number of data points to evaluate
216  * \param[in] estimator pointer to the estimator object
217  * \param[in] distance distance computation function pointer
218  */
219  OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCircle2D<PointT> *model) :
220  pcl::Functor<float>(m_data_points), model_ (model) {}
221 
222  /** Cost function to be minimized
223  * \param[in] x the variables array
224  * \param[out] fvec the resultant functions evaluations
225  * \return 0
226  */
227  int
228  operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
229  {
230  for (int i = 0; i < values (); ++i)
231  {
232  // Compute the difference between the center of the circle and the datapoint X_i
233  float xt = model_->input_->points[(*model_->tmp_inliers_)[i]].x - x[0];
234  float yt = model_->input_->points[(*model_->tmp_inliers_)[i]].y - x[1];
235 
236  // g = sqrt ((x-a)^2 + (y-b)^2) - R
237  fvec[i] = sqrtf (xt * xt + yt * yt) - x[2];
238  }
239  return (0);
240  }
241 
243  };
244 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
245 #pragma GCC diagnostic warning "-Weffc++"
246 #endif
247  };
248 }
249 
250 #ifdef PCL_NO_PRECOMPILE
251 #include <pcl/sample_consensus/impl/sac_model_circle.hpp>
252 #endif
253 
254 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
SampleConsensusModel< PointT >::PointCloud PointCloud
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given 2d circle model coefficients.
bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given 2D circle model.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModelCircle2D(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCircle2D.
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the 2d circle coefficients using the given inlier set and return them to the user...
SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane...
SampleConsensusModelCircle2D(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCircle2D.
SacModel
Definition: model_types.h:48
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)
Create a new point cloud with inliers projected onto the 2d circle model.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid 2D circle model, compute the model coefficient...
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::shared_ptr< SampleConsensusModelCircle2D > Ptr
SampleConsensusModelCircle2D & operator=(const SampleConsensusModelCircle2D &source)
Copy constructor.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CIRCLE2D).
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Compute all distances from the cloud data to a given 2D circle model.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:618
virtual ~SampleConsensusModelCircle2D()
Empty destructor.
SampleConsensusModelCircle2D(const SampleConsensusModelCircle2D &source)
Copy constructor.