12#include <pcl/sample_consensus/sac_model.h>
13#include <pcl/sample_consensus/model_types.h>
40 template <
typename Po
intT>
55 using Ptr = shared_ptr<SampleConsensusModelEllipse3D<PointT> >;
56 using ConstPtr = shared_ptr<const SampleConsensusModelEllipse3D<PointT> >;
115 Eigen::VectorXf &model_coefficients)
const override;
123 std::vector<double> &distances)
const override;
132 const double threshold,
143 const double threshold)
const override;
153 const Eigen::VectorXf &model_coefficients,
154 Eigen::VectorXf &optimized_coefficients)
const override;
164 const Eigen::VectorXf &model_coefficients,
166 bool copy_data_fields =
true)
const override;
175 const Eigen::VectorXf &model_coefficients,
176 const double threshold)
const override;
190 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override;
207 pcl::
Functor<double> (indices.size ()), model_ (model), indices_ (indices) {}
214 int operator() (
const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
const
217 const Eigen::Vector3f c(x[0], x[1], x[2]);
219 const Eigen::Vector3f n_axis(x[5], x[6], x[7]);
221 const Eigen::Vector3f x_axis(x[8], x[9], x[10]);
223 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
225 const float par_a(x[3]);
227 const float par_b(x[4]);
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))
235 const Eigen::Matrix3f Rot_T = Rot.transpose();
237 for (
int i = 0; i <
values (); ++i)
241 const Eigen::Vector3f p = (*model_->input_)[indices_[i]].getVector3fMap().template cast<float>();
244 const Eigen::Vector3f p_ = Rot_T * (p - c);
250 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
252 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
253 fvec[i] = distanceVector.norm();
263 get_ellipse_point(
const Eigen::VectorXf& par,
float th,
float& x,
float& y);
265 static Eigen::Vector2f
266 dvec2ellipse(
const Eigen::VectorXf& par,
float u,
float v,
float& th_opt);
269 golden_section_search(
270 const Eigen::VectorXf& par,
279#ifdef PCL_NO_PRECOMPILE
280#include <pcl/sample_consensus/impl/sac_model_ellipse3d.hpp>
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.
double radius_min_
The minimum and maximum radius limits for the model.
unsigned int sample_size_
The size of a sample from which the model is computed.
typename PointCloud::ConstPtr PointCloudConstPtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
std::string model_name_
The model name.
unsigned int model_size_
The number of coefficients in the model.
typename PointCloud::Ptr PointCloudPtr
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
IndicesAllocator<> Indices
Type used for indices in PCL.
Base functor all the models that need non linear optimization must define their own one and implement...
int values() const
Get the number of values.
A point structure representing Euclidean xyz coordinates, and the RGB color.