Point Cloud Library (PCL)  1.14.0-dev
sac_model_ellipse3d.h
1 /*
2  * SPDX-License-Identifier: BSD-3-Clause
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception Inc.
6  *
7  * All rights reserved
8  */
9 
10 #pragma once
11 
12 #include <pcl/sample_consensus/sac_model.h>
13 #include <pcl/sample_consensus/model_types.h>
14 
15 namespace pcl
16 {
17  /** \brief SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation.
18  *
19  * The model coefficients are defined as:
20  * - \b center.x : the X coordinate of the ellipse's center
21  * - \b center.y : the Y coordinate of the ellipse's center
22  * - \b center.z : the Z coordinate of the ellipse's center
23  * - \b semi_axis.u : semi-major axis length along the local u-axis of the ellipse
24  * - \b semi_axis.v : semi-minor axis length along the local v-axis of the ellipse
25  * - \b normal.x : the X coordinate of the normal's direction
26  * - \b normal.y : the Y coordinate of the normal's direction
27  * - \b normal.z : the Z coordinate of the normal's direction
28  * - \b u.x : the X coordinate of the local u-axis of the ellipse
29  * - \b u.y : the Y coordinate of the local u-axis of the ellipse
30  * - \b u.z : the Z coordinate of the local u-axis of the ellipse
31  *
32  * For more details please refer to the following manuscript:
33  * "Semi-autonomous Prosthesis Control Using Minimal Depth Information and Vibrotactile Feedback",
34  * Miguel N. Castro & Strahinja Dosen. IEEE Transactions on Human-Machine Systems [under review]. arXiv:2210.00541.
35  * (@ github.com/mnobrecastro/pcl-ellipse-fitting)
36  *
37  * \author Miguel Nobre Castro (mnobrecastro@gmail.com)
38  * \ingroup sample_consensus
39  */
40  template <typename PointT>
42  {
43  public:
49 
53 
54  using Ptr = shared_ptr<SampleConsensusModelEllipse3D<PointT> >;
55  using ConstPtr = shared_ptr<const SampleConsensusModelEllipse3D<PointT> >;
56 
57  /** \brief Constructor for base SampleConsensusModelEllipse3D.
58  * \param[in] cloud the input point cloud dataset
59  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
60  */
61  SampleConsensusModelEllipse3D (const PointCloudConstPtr &cloud, bool random = false)
62  : SampleConsensusModel<PointT> (cloud, random)
63  {
64  model_name_ = "SampleConsensusModelEllipse3D";
65  sample_size_ = 6;
66  model_size_ = 11;
67  }
68 
69  /** \brief Constructor for base SampleConsensusModelEllipse3D.
70  * \param[in] cloud the input point cloud dataset
71  * \param[in] indices a vector of point indices to be used from \a cloud
72  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
73  */
75  const Indices &indices,
76  bool random = false)
77  : SampleConsensusModel<PointT> (cloud, indices, random)
78  {
79  model_name_ = "SampleConsensusModelEllipse3D";
80  sample_size_ = 6;
81  model_size_ = 11;
82  }
83 
84  /** \brief Empty destructor */
85  ~SampleConsensusModelEllipse3D () override = default;
86 
87  /** \brief Copy constructor.
88  * \param[in] source the model to copy into this
89  */
92  {
93  *this = source;
94  model_name_ = "SampleConsensusModelEllipse3D";
95  }
96 
97  /** \brief Copy constructor.
98  * \param[in] source the model to copy into this
99  */
102  {
104  return (*this);
105  }
106 
107  /** \brief Check whether the given index samples can form a valid 3D ellipse model, compute the model coefficients
108  * from these samples and store them in model_coefficients. The ellipse coefficients are: x, y, R.
109  * \param[in] samples the point indices found as possible good candidates for creating a valid model
110  * \param[out] model_coefficients the resultant model coefficients
111  */
112  bool
113  computeModelCoefficients (const Indices &samples,
114  Eigen::VectorXf &model_coefficients) const override;
115 
116  /** \brief Compute all distances from the cloud data to a given 3D ellipse model.
117  * \param[in] model_coefficients the coefficients of a 3D ellipse model that we need to compute distances to
118  * \param[out] distances the resultant estimated distances
119  */
120  void
121  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
122  std::vector<double> &distances) const override;
123 
124  /** \brief Compute all distances from the cloud data to a given 3D ellipse model.
125  * \param[in] model_coefficients the coefficients of a 3D ellipse model that we need to compute distances to
126  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
127  * \param[out] inliers the resultant model inliers
128  */
129  void
130  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
131  const double threshold,
132  Indices &inliers) override;
133 
134  /** \brief Count all the points which respect the given model coefficients as inliers.
135  *
136  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
137  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
138  * \return the resultant number of inliers
139  */
140  std::size_t
141  countWithinDistance (const Eigen::VectorXf &model_coefficients,
142  const double threshold) const override;
143 
144  /** \brief Recompute the 3d ellipse coefficients using the given inlier set and return them to the user.
145  * @note: these are the coefficients of the 3d ellipse model after refinement (e.g. after SVD)
146  * \param[in] inliers the data inliers found as supporting the model
147  * \param[in] model_coefficients the initial guess for the optimization
148  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
149  */
150  void
151  optimizeModelCoefficients (const Indices &inliers,
152  const Eigen::VectorXf &model_coefficients,
153  Eigen::VectorXf &optimized_coefficients) const override;
154 
155  /** \brief Create a new point cloud with inliers projected onto the 3d ellipse model.
156  * \param[in] inliers the data inliers that we want to project on the 3d ellipse model
157  * \param[in] model_coefficients the coefficients of a 3d ellipse model
158  * \param[out] projected_points the resultant projected points
159  * \param[in] copy_data_fields set to true if we need to copy the other data fields
160  */
161  void
162  projectPoints (const Indices &inliers,
163  const Eigen::VectorXf &model_coefficients,
164  PointCloud &projected_points,
165  bool copy_data_fields = true) const override;
166 
167  /** \brief Verify whether a subset of indices verifies the given 3d ellipse model coefficients.
168  * \param[in] indices the data indices that need to be tested against the 3d ellipse model
169  * \param[in] model_coefficients the 3d ellipse model coefficients
170  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
171  */
172  bool
173  doSamplesVerifyModel (const std::set<index_t> &indices,
174  const Eigen::VectorXf &model_coefficients,
175  const double threshold) const override;
176 
177  /** \brief Return a unique id for this model (SACMODEL_ELLIPSE3D). */
178  inline pcl::SacModel
179  getModelType () const override { return (SACMODEL_ELLIPSE3D); }
180 
181  protected:
184 
185  /** \brief Check whether a model is valid given the user constraints.
186  * \param[in] model_coefficients the set of model coefficients
187  */
188  bool
189  isModelValid (const Eigen::VectorXf &model_coefficients) const override;
190 
191  /** \brief Check if a sample of indices results in a good sample of points indices.
192  * \param[in] samples the resultant index samples
193  */
194  bool
195  isSampleGood(const Indices &samples) const override;
196 
197  private:
198  /** \brief Functor for the optimization function */
199  struct OptimizationFunctor : pcl::Functor<double>
200  {
201  /** Functor constructor
202  * \param[in] indices the indices of data points to evaluate
203  * \param[in] estimator pointer to the estimator object
204  */
205  OptimizationFunctor (const pcl::SampleConsensusModelEllipse3D<PointT> *model, const Indices& indices) :
206  pcl::Functor<double> (indices.size ()), model_ (model), indices_ (indices) {}
207 
208  /** Cost function to be minimized
209  * \param[in] x the variables array
210  * \param[out] fvec the resultant functions evaluations
211  * \return 0
212  */
213  int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
214  {
215  // c : Ellipse Center
216  const Eigen::Vector3f c(x[0], x[1], x[2]);
217  // n : Ellipse (Plane) Normal
218  const Eigen::Vector3f n_axis(x[5], x[6], x[7]);
219  // x : Ellipse (Plane) X-Axis
220  const Eigen::Vector3f x_axis(x[8], x[9], x[10]);
221  // y : Ellipse (Plane) Y-Axis
222  const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
223  // a : Ellipse semi-major axis (X) length
224  const float par_a(x[3]);
225  // b : Ellipse semi-minor axis (Y) length
226  const float par_b(x[4]);
227 
228  // Compute the rotation matrix and its transpose
229  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
230  << x_axis(0), y_axis(0), n_axis(0),
231  x_axis(1), y_axis(1), n_axis(1),
232  x_axis(2), y_axis(2), n_axis(2))
233  .finished();
234  const Eigen::Matrix3f Rot_T = Rot.transpose();
235 
236  for (int i = 0; i < values (); ++i)
237  {
238  // what i have:
239  // p : Sample Point
240  const Eigen::Vector3f p = (*model_->input_)[indices_[i]].getVector3fMap().template cast<float>();
241 
242  // Local coordinates of sample point p
243  const Eigen::Vector3f p_ = Rot_T * (p - c);
244 
245  // k : Point on Ellipse
246  // Calculate the shortest distance from the point to the ellipse which is
247  // given by the norm of a vector that is normal to the ellipse tangent
248  // calculated at the point it intersects the tangent.
249  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
250  float th_opt;
251  const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
252  fvec[i] = distanceVector.norm();
253  }
254  return (0);
255  }
256 
258  const Indices &indices_;
259  };
260 
261  static void
262  get_ellipse_point(const Eigen::VectorXf& par, float th, float& x, float& y);
263 
264  static Eigen::Vector2f
265  dvec2ellipse(const Eigen::VectorXf& par, float u, float v, float& th_opt);
266 
267  static float
268  golden_section_search(
269  const Eigen::VectorXf& par,
270  float u,
271  float v,
272  float th_min,
273  float th_max,
274  float epsilon);
275  };
276 }
277 
278 #ifdef PCL_NO_PRECOMPILE
279 #include <pcl/sample_consensus/impl/sac_model_ellipse3d.hpp>
280 #endif
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 3d ellipse coefficients using the given inlier set and return them to the user.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_ELLIPSE3D).
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.
SampleConsensusModelEllipse3D(const SampleConsensusModelEllipse3D &source)
Copy constructor.
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 3d ellipse model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid 3D ellipse model, compute the model coefficien...
SampleConsensusModelEllipse3D(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelEllipse3D.
~SampleConsensusModelEllipse3D() override=default
Empty destructor.
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 3d ellipse model coefficients.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Compute all distances from the cloud data to a given 3D ellipse model.
SampleConsensusModelEllipse3D & operator=(const SampleConsensusModelEllipse3D &source)
Copy constructor.
SampleConsensusModelEllipse3D(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelEllipse3D.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 3D ellipse model.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:71
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:78
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:589
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:74
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:557
std::string model_name_
The model name.
Definition: sac_model.h:551
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:592
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:75
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:79
SacModel
Definition: model_types.h:46
@ SACMODEL_ELLIPSE3D
Definition: model_types.h:65
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:680
A point structure representing Euclidean xyz coordinates, and the RGB color.