Point Cloud Library (PCL) 1.13.0
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
15namespace 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.
SampleConsensusModelEllipse3D & operator=(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(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:70
double radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:564
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:77
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:588
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:73
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:556
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition: sac_model.h:553
std::string model_name_
The model name.
Definition: sac_model.h:550
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:591
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:74
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:78
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:679
A point structure representing Euclidean xyz coordinates, and the RGB color.