40#include <pcl/sample_consensus/sac_model.h>
41#include <pcl/sample_consensus/model_types.h>
59 template <
typename Po
intT>
74 using Ptr = shared_ptr<SampleConsensusModelCircle3D<PointT> >;
75 using ConstPtr = shared_ptr<const SampleConsensusModelCircle3D<PointT> >;
135 Eigen::VectorXf &model_coefficients)
const override;
143 std::vector<double> &distances)
const override;
152 const double threshold,
163 const double threshold)
const override;
173 const Eigen::VectorXf &model_coefficients,
174 Eigen::VectorXf &optimized_coefficients)
const override;
184 const Eigen::VectorXf &model_coefficients,
186 bool copy_data_fields =
true)
const override;
195 const Eigen::VectorXf &model_coefficients,
196 const double threshold)
const override;
210 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override;
227 pcl::
Functor<double> (indices.size ()), model_ (model), indices_ (indices) {}
234 int operator() (
const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
const
238 const Eigen::Vector3d C (x[0], x[1], x[2]);
240 const Eigen::Vector3d N (x[4], x[5], x[6]);
242 const double r = x[3];
243 for (
int i = 0; i <
values (); ++i)
248 (*model_->input_)[indices_[i]].getVector3fMap().template cast<double>();
250 Eigen::Vector3d helperVectorPC = P - C;
253 double lambda = (-(helperVectorPC.dot (N))) / N.dot (N);
255 Eigen::Vector3d P_proj = P + lambda * N;
256 Eigen::Vector3d helperVectorP_projC = P_proj - C;
259 Eigen::Vector3d
K = C + r * helperVectorP_projC.normalized ();
260 Eigen::Vector3d distanceVector = P -
K;
262 fvec[i] = distanceVector.norm ();
273#ifdef PCL_NO_PRECOMPILE
274#include <pcl/sample_consensus/impl/sac_model_circle3d.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModelCircle3D defines a model for 3D circle segmentation.
typename SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
SampleConsensusModelCircle3D(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelCircle3D.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
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 circle model coefficients.
typename SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
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 circle model.
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.
SampleConsensusModelCircle3D(const SampleConsensusModelCircle3D &source)
Copy constructor.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_CIRCLE3D).
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 3D circle model.
shared_ptr< SampleConsensusModelCircle3D< PointT > > Ptr
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 3d circle coefficients using the given inlier set and return them to the user.
shared_ptr< const SampleConsensusModelCircle3D< PointT > > ConstPtr
~SampleConsensusModelCircle3D() override=default
Empty destructor.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Compute all distances from the cloud data to a given 3D circle model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid 2D circle model, compute the model coefficient...
SampleConsensusModelCircle3D(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCircle3D.
SampleConsensusModelCircle3D & operator=(const SampleConsensusModelCircle3D &source)
Copy constructor.
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.