Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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:
50
54
55 using Ptr = shared_ptr<SampleConsensusModelEllipse3D<PointT> >;
56 using ConstPtr = shared_ptr<const SampleConsensusModelEllipse3D<PointT> >;
57
58 /** \brief Constructor for base SampleConsensusModelEllipse3D.
59 * \param[in] cloud the input point cloud dataset
60 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
61 */
62 SampleConsensusModelEllipse3D (const PointCloudConstPtr &cloud, bool random = false)
63 : SampleConsensusModel<PointT> (cloud, random)
64 {
65 model_name_ = "SampleConsensusModelEllipse3D";
66 sample_size_ = 6;
67 model_size_ = 11;
68 }
69
70 /** \brief Constructor for base SampleConsensusModelEllipse3D.
71 * \param[in] cloud the input point cloud dataset
72 * \param[in] indices a vector of point indices to be used from \a cloud
73 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
74 */
76 const Indices &indices,
77 bool random = false)
78 : SampleConsensusModel<PointT> (cloud, indices, random)
79 {
80 model_name_ = "SampleConsensusModelEllipse3D";
81 sample_size_ = 6;
82 model_size_ = 11;
83 }
84
85 /** \brief Empty destructor */
86 ~SampleConsensusModelEllipse3D () override = default;
87
88 /** \brief Copy constructor.
89 * \param[in] source the model to copy into this
90 */
93 {
94 *this = source;
95 model_name_ = "SampleConsensusModelEllipse3D";
96 }
97
98 /** \brief Copy constructor.
99 * \param[in] source the model to copy into this
100 */
103 {
105 return (*this);
106 }
107
108 /** \brief Check whether the given index samples can form a valid 3D ellipse model, compute the model coefficients
109 * from these samples and store them in model_coefficients. The ellipse coefficients are: x, y, R.
110 * \param[in] samples the point indices found as possible good candidates for creating a valid model
111 * \param[out] model_coefficients the resultant model coefficients
112 */
113 bool
114 computeModelCoefficients (const Indices &samples,
115 Eigen::VectorXf &model_coefficients) const override;
116
117 /** \brief Compute all distances from the cloud data to a given 3D ellipse model.
118 * \param[in] model_coefficients the coefficients of a 3D ellipse model that we need to compute distances to
119 * \param[out] distances the resultant estimated distances
120 */
121 void
122 getDistancesToModel (const Eigen::VectorXf &model_coefficients,
123 std::vector<double> &distances) const override;
124
125 /** \brief Compute all distances from the cloud data to a given 3D ellipse model.
126 * \param[in] model_coefficients the coefficients of a 3D ellipse model that we need to compute distances to
127 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
128 * \param[out] inliers the resultant model inliers
129 */
130 void
131 selectWithinDistance (const Eigen::VectorXf &model_coefficients,
132 const double threshold,
133 Indices &inliers) override;
134
135 /** \brief Count all the points which respect the given model coefficients as inliers.
136 *
137 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
138 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
139 * \return the resultant number of inliers
140 */
141 std::size_t
142 countWithinDistance (const Eigen::VectorXf &model_coefficients,
143 const double threshold) const override;
144
145 /** \brief Recompute the 3d ellipse coefficients using the given inlier set and return them to the user.
146 * @note: these are the coefficients of the 3d ellipse model after refinement (e.g. after SVD)
147 * \param[in] inliers the data inliers found as supporting the model
148 * \param[in] model_coefficients the initial guess for the optimization
149 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
150 */
151 void
152 optimizeModelCoefficients (const Indices &inliers,
153 const Eigen::VectorXf &model_coefficients,
154 Eigen::VectorXf &optimized_coefficients) const override;
155
156 /** \brief Create a new point cloud with inliers projected onto the 3d ellipse model.
157 * \param[in] inliers the data inliers that we want to project on the 3d ellipse model
158 * \param[in] model_coefficients the coefficients of a 3d ellipse model
159 * \param[out] projected_points the resultant projected points
160 * \param[in] copy_data_fields set to true if we need to copy the other data fields
161 */
162 void
163 projectPoints (const Indices &inliers,
164 const Eigen::VectorXf &model_coefficients,
165 PointCloud &projected_points,
166 bool copy_data_fields = true) const override;
167
168 /** \brief Verify whether a subset of indices verifies the given 3d ellipse model coefficients.
169 * \param[in] indices the data indices that need to be tested against the 3d ellipse model
170 * \param[in] model_coefficients the 3d ellipse model coefficients
171 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
172 */
173 bool
174 doSamplesVerifyModel (const std::set<index_t> &indices,
175 const Eigen::VectorXf &model_coefficients,
176 const double threshold) const override;
177
178 /** \brief Return a unique id for this model (SACMODEL_ELLIPSE3D). */
179 inline pcl::SacModel
180 getModelType () const override { return (SACMODEL_ELLIPSE3D); }
181
182 protected:
185
186 /** \brief Check whether a model is valid given the user constraints.
187 * \param[in] model_coefficients the set of model coefficients
188 */
189 bool
190 isModelValid (const Eigen::VectorXf &model_coefficients) const override;
191
192 /** \brief Check if a sample of indices results in a good sample of points indices.
193 * \param[in] samples the resultant index samples
194 */
195 bool
196 isSampleGood(const Indices &samples) const override;
197
198 private:
199 /** \brief Functor for the optimization function */
200 struct OptimizationFunctor : pcl::Functor<double>
201 {
202 /** Functor constructor
203 * \param[in] indices the indices of data points to evaluate
204 * \param[in] estimator pointer to the estimator object
205 */
206 OptimizationFunctor (const pcl::SampleConsensusModelEllipse3D<PointT> *model, const Indices& indices) :
207 pcl::Functor<double> (indices.size ()), model_ (model), indices_ (indices) {}
208
209 /** Cost function to be minimized
210 * \param[in] x the variables array
211 * \param[out] fvec the resultant functions evaluations
212 * \return 0
213 */
214 int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
215 {
216 // c : Ellipse Center
217 const Eigen::Vector3f c(x[0], x[1], x[2]);
218 // n : Ellipse (Plane) Normal
219 const Eigen::Vector3f n_axis(x[5], x[6], x[7]);
220 // x : Ellipse (Plane) X-Axis
221 const Eigen::Vector3f x_axis(x[8], x[9], x[10]);
222 // y : Ellipse (Plane) Y-Axis
223 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
224 // a : Ellipse semi-major axis (X) length
225 const float par_a(x[3]);
226 // b : Ellipse semi-minor axis (Y) length
227 const float par_b(x[4]);
228
229 // Compute the rotation matrix and its transpose
230 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
231 << x_axis(0), y_axis(0), n_axis(0),
232 x_axis(1), y_axis(1), n_axis(1),
233 x_axis(2), y_axis(2), n_axis(2))
234 .finished();
235 const Eigen::Matrix3f Rot_T = Rot.transpose();
236
237 for (int i = 0; i < values (); ++i)
238 {
239 // what i have:
240 // p : Sample Point
241 const Eigen::Vector3f p = (*model_->input_)[indices_[i]].getVector3fMap().template cast<float>();
242
243 // Local coordinates of sample point p
244 const Eigen::Vector3f p_ = Rot_T * (p - c);
245
246 // k : Point on Ellipse
247 // Calculate the shortest distance from the point to the ellipse which is
248 // given by the norm of a vector that is normal to the ellipse tangent
249 // calculated at the point it intersects the tangent.
250 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
251 float th_opt;
252 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
253 fvec[i] = distanceVector.norm();
254 }
255 return (0);
256 }
257
259 const Indices &indices_;
260 };
261
262 static void
263 get_ellipse_point(const Eigen::VectorXf& par, float th, float& x, float& y);
264
265 static Eigen::Vector2f
266 dvec2ellipse(const Eigen::VectorXf& par, float u, float v, float& th_opt);
267
268 static float
269 golden_section_search(
270 const Eigen::VectorXf& par,
271 float u,
272 float v,
273 float th_min,
274 float th_max,
275 float epsilon);
276 };
277}
278
279#ifdef PCL_NO_PRECOMPILE
280#include <pcl/sample_consensus/impl/sac_model_ellipse3d.hpp>
281#endif
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation.
typename SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
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.
typename SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
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.
shared_ptr< const SampleConsensusModelEllipse3D< PointT > > ConstPtr
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.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
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.
shared_ptr< SampleConsensusModelEllipse3D< PointT > > Ptr
SampleConsensusModel represents the base model class.
Definition sac_model.h:71
double radius_min_
The minimum and maximum radius limits for the model.
Definition sac_model.h:565
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
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition sac_model.h:554
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
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition sac_model.h:586
@ 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
int values() const
Get the number of values.
Definition sac_model.h:704
A point structure representing Euclidean xyz coordinates, and the RGB color.