Point Cloud Library (PCL)  1.10.0-dev
sac_model_circle3d.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #pragma once
39 
40 #include <pcl/sample_consensus/sac_model.h>
41 #include <pcl/sample_consensus/model_types.h>
42 
43 namespace pcl
44 {
45  /** \brief SampleConsensusModelCircle3D defines a model for 3D circle segmentation.
46  *
47  * The model coefficients are defined as:
48  * - \b center.x : the X coordinate of the circle's center
49  * - \b center.y : the Y coordinate of the circle's center
50  * - \b center.z : the Z coordinate of the circle's center
51  * - \b radius : the circle's radius
52  * - \b normal.x : the X coordinate of the normal's direction
53  * - \b normal.y : the Y coordinate of the normal's direction
54  * - \b normal.z : the Z coordinate of the normal's direction
55  *
56  * \author Raoul Hoffmann, Karol Hausman, Radu B. Rusu
57  * \ingroup sample_consensus
58  */
59  template <typename PointT>
61  {
62  public:
68 
72 
75 
76  /** \brief Constructor for base SampleConsensusModelCircle3D.
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  */
81  bool random = false)
82  : SampleConsensusModel<PointT> (cloud, random)
83  {
84  model_name_ = "SampleConsensusModelCircle3D";
85  sample_size_ = 3;
86  model_size_ = 7;
87  }
88 
89  /** \brief Constructor for base SampleConsensusModelCircle3D.
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  */
95  const std::vector<int> &indices,
96  bool random = false)
97  : SampleConsensusModel<PointT> (cloud, indices, random)
98  {
99  model_name_ = "SampleConsensusModelCircle3D";
100  sample_size_ = 3;
101  model_size_ = 7;
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_ = "SampleConsensusModelCircle3D";
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 2D circle model, compute the model coefficients
128  * from these samples and store them in model_coefficients. The circle coefficients are: x, y, R.
129  * \param[in] samples the point indices found as possible good candidates for creating a valid model
130  * \param[out] model_coefficients the resultant model coefficients
131  */
132  bool
133  computeModelCoefficients (const std::vector<int> &samples,
134  Eigen::VectorXf &model_coefficients) const override;
135 
136  /** \brief Compute all distances from the cloud data to a given 3D circle model.
137  * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to
138  * \param[out] distances the resultant estimated distances
139  */
140  void
141  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
142  std::vector<double> &distances) const override;
143 
144  /** \brief Compute all distances from the cloud data to a given 3D circle model.
145  * \param[in] model_coefficients the coefficients of a 3D circle model that we need to compute distances to
146  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
147  * \param[out] inliers the resultant model inliers
148  */
149  void
150  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
151  const double threshold,
152  std::vector<int> &inliers) override;
153 
154  /** \brief Count all the points which respect the given model coefficients as inliers.
155  *
156  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
157  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
158  * \return the resultant number of inliers
159  */
160  std::size_t
161  countWithinDistance (const Eigen::VectorXf &model_coefficients,
162  const double threshold) const override;
163 
164  /** \brief Recompute the 3d circle coefficients using the given inlier set and return them to the user.
165  * @note: these are the coefficients of the 3d circle model after refinement (e.g. after SVD)
166  * \param[in] inliers the data inliers found as supporting the model
167  * \param[in] model_coefficients the initial guess for the optimization
168  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
169  */
170  void
171  optimizeModelCoefficients (const std::vector<int> &inliers,
172  const Eigen::VectorXf &model_coefficients,
173  Eigen::VectorXf &optimized_coefficients) const override;
174 
175  /** \brief Create a new point cloud with inliers projected onto the 3d circle model.
176  * \param[in] inliers the data inliers that we want to project on the 3d circle model
177  * \param[in] model_coefficients the coefficients of a 3d circle model
178  * \param[out] projected_points the resultant projected points
179  * \param[in] copy_data_fields set to true if we need to copy the other data fields
180  */
181  void
182  projectPoints (const std::vector<int> &inliers,
183  const Eigen::VectorXf &model_coefficients,
184  PointCloud &projected_points,
185  bool copy_data_fields = true) const override;
186 
187  /** \brief Verify whether a subset of indices verifies the given 3d circle model coefficients.
188  * \param[in] indices the data indices that need to be tested against the 3d circle model
189  * \param[in] model_coefficients the 3d circle model coefficients
190  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
191  */
192  bool
193  doSamplesVerifyModel (const std::set<int> &indices,
194  const Eigen::VectorXf &model_coefficients,
195  const double threshold) const override;
196 
197  /** \brief Return a unique id for this model (SACMODEL_CIRCLE3D). */
198  inline pcl::SacModel
199  getModelType () const override { return (SACMODEL_CIRCLE3D); }
200 
201  protected:
204 
205  /** \brief Check whether a model is valid given the user constraints.
206  * \param[in] model_coefficients the set of model coefficients
207  */
208  bool
209  isModelValid (const Eigen::VectorXf &model_coefficients) const override;
210 
211  /** \brief Check if a sample of indices results in a good sample of points indices.
212  * \param[in] samples the resultant index samples
213  */
214  bool
215  isSampleGood(const std::vector<int> &samples) const override;
216 
217  private:
218  /** \brief Functor for the optimization function */
219  struct OptimizationFunctor : pcl::Functor<double>
220  {
221  /** Functor constructor
222  * \param[in] indices the indices of data points to evaluate
223  * \param[in] estimator pointer to the estimator object
224  */
225  OptimizationFunctor (const pcl::SampleConsensusModelCircle3D<PointT> *model, const std::vector<int>& indices) :
226  pcl::Functor<double> (indices.size ()), model_ (model), indices_ (indices) {}
227 
228  /** Cost function to be minimized
229  * \param[in] x the variables array
230  * \param[out] fvec the resultant functions evaluations
231  * \return 0
232  */
233  int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
234  {
235  for (int i = 0; i < values (); ++i)
236  {
237  // what i have:
238  // P : Sample Point
239  Eigen::Vector3d P (model_->input_->points[indices_[i]].x, model_->input_->points[indices_[i]].y, model_->input_->points[indices_[i]].z);
240  // C : Circle Center
241  Eigen::Vector3d C (x[0], x[1], x[2]);
242  // N : Circle (Plane) Normal
243  Eigen::Vector3d N (x[4], x[5], x[6]);
244  // r : Radius
245  double r = x[3];
246 
247  Eigen::Vector3d helperVectorPC = P - C;
248  // 1.1. get line parameter
249  //float lambda = (helperVectorPC.dot(N)) / N.squaredNorm() ;
250  double lambda = (-(helperVectorPC.dot (N))) / N.dot (N);
251  // Projected Point on plane
252  Eigen::Vector3d P_proj = P + lambda * N;
253  Eigen::Vector3d helperVectorP_projC = P_proj - C;
254 
255  // K : Point on Circle
256  Eigen::Vector3d K = C + r * helperVectorP_projC.normalized ();
257  Eigen::Vector3d distanceVector = P - K;
258 
259  fvec[i] = distanceVector.norm ();
260  }
261  return (0);
262  }
263 
265  const std::vector<int> &indices_;
266  };
267  };
268 }
269 
270 #ifdef PCL_NO_PRECOMPILE
271 #include <pcl/sample_consensus/impl/sac_model_circle3d.hpp>
272 #endif
void projectPoints(const std::vector< int > &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 3d circle model.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers) override
Compute all distances from the cloud data to a given 3D circle model.
SampleConsensusModelCircle3D(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCircle3D.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:564
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.
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:642
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:73
SampleConsensusModelCircle3D defines a model for 3D circle segmentation.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid 2D circle model, compute the model coefficient...
bool isSampleGood(const std::vector< int > &samples) const override
Check if a sample of indices results in a good sample of points indices.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:68
std::string model_name_
The model name.
Definition: sac_model.h:523
SampleConsensusModelCircle3D(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCircle3D.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:72
SampleConsensusModelCircle3D(const SampleConsensusModelCircle3D &source)
Copy constructor.
SacModel
Definition: model_types.h:45
shared_ptr< const SampleConsensusModel< pcl::PointXYZRGB > > ConstPtr
Definition: sac_model.h:77
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 3D circle model.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_CIRCLE3D).
Definition: norms.h:54
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given 3d circle model coefficients.
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:529
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 3d circle coefficients using the given inlier set and return them to the user...
A point structure representing Euclidean xyz coordinates, and the RGB color.
shared_ptr< SampleConsensusModel< pcl::PointXYZRGB > > Ptr
Definition: sac_model.h:76
~SampleConsensusModelCircle3D()
Empty destructor.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:561
SampleConsensusModelCircle3D & operator=(const SampleConsensusModelCircle3D &source)
Copy constructor.