12 #include <pcl/sample_consensus/sac_model.h>
13 #include <pcl/sample_consensus/model_types.h>
40 template <
typename Po
intT>
54 using Ptr = shared_ptr<SampleConsensusModelEllipse3D<PointT> >;
55 using ConstPtr = shared_ptr<const SampleConsensusModelEllipse3D<PointT> >;
114 Eigen::VectorXf &model_coefficients)
const override;
122 std::vector<double> &distances)
const override;
131 const double threshold,
142 const double threshold)
const override;
152 const Eigen::VectorXf &model_coefficients,
153 Eigen::VectorXf &optimized_coefficients)
const override;
163 const Eigen::VectorXf &model_coefficients,
165 bool copy_data_fields =
true)
const override;
174 const Eigen::VectorXf &model_coefficients,
175 const double threshold)
const override;
189 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override;
213 int operator() (
const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
const
216 const Eigen::Vector3f c(x[0], x[1], x[2]);
218 const Eigen::Vector3f n_axis(x[5], x[6], x[7]);
220 const Eigen::Vector3f x_axis(x[8], x[9], x[10]);
222 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
224 const float par_a(x[3]);
226 const float par_b(x[4]);
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))
234 const Eigen::Matrix3f Rot_T = Rot.transpose();
236 for (
int i = 0; i < values (); ++i)
240 const Eigen::Vector3f p = (*model_->input_)[
indices_[i]].getVector3fMap().template cast<float>();
243 const Eigen::Vector3f p_ = Rot_T * (p - c);
249 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
251 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
252 fvec[i] = distanceVector.norm();
262 get_ellipse_point(
const Eigen::VectorXf& par,
float th,
float& x,
float& y);
264 static Eigen::Vector2f
265 dvec2ellipse(
const Eigen::VectorXf& par,
float u,
float v,
float& th_opt);
268 golden_section_search(
269 const Eigen::VectorXf& par,
278 #ifdef PCL_NO_PRECOMPILE
279 #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.
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.
shared_ptr< SampleConsensusModel< PointT > > Ptr
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.
std::string model_name_
The model name.
unsigned int model_size_
The number of coefficients in the model.
typename PointCloud::Ptr PointCloudPtr
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
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...
A point structure representing Euclidean xyz coordinates, and the RGB color.