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