40 #include <pcl/sample_consensus/sac_model.h>
41 #include <pcl/sample_consensus/model_types.h>
59 template <
typename Po
intT>
73 using Ptr = shared_ptr<SampleConsensusModelCircle3D<PointT> >;
74 using ConstPtr = shared_ptr<const SampleConsensusModelCircle3D<PointT> >;
95 const Indices &indices,
134 Eigen::VectorXf &model_coefficients)
const override;
142 std::vector<double> &distances)
const override;
151 const double threshold,
152 Indices &inliers)
override;
162 const double threshold)
const override;
172 const Eigen::VectorXf &model_coefficients,
173 Eigen::VectorXf &optimized_coefficients)
const override;
183 const Eigen::VectorXf &model_coefficients,
185 bool copy_data_fields =
true)
const override;
194 const Eigen::VectorXf &model_coefficients,
195 const double threshold)
const override;
209 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override;
233 int operator() (
const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
const
235 for (
int i = 0; i < values (); ++i)
239 Eigen::Vector3d P (model_->input_->points[
indices_[i]].x, model_->input_->points[
indices_[i]].y, model_->input_->points[
indices_[i]].z);
241 Eigen::Vector3d C (x[0], x[1], x[2]);
243 Eigen::Vector3d N (x[4], x[5], x[6]);
247 Eigen::Vector3d helperVectorPC = P - C;
250 double lambda = (-(helperVectorPC.dot (N))) / N.dot (N);
252 Eigen::Vector3d P_proj = P + lambda * N;
253 Eigen::Vector3d helperVectorP_projC = P_proj - C;
256 Eigen::Vector3d
K = C + r * helperVectorP_projC.normalized ();
257 Eigen::Vector3d distanceVector = P -
K;
259 fvec[i] = distanceVector.norm ();
270 #ifdef PCL_NO_PRECOMPILE
271 #include <pcl/sample_consensus/impl/sac_model_circle3d.hpp>
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...
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...
DeviceArray< int > Indices
unsigned int model_size_
The number of coefficients in the 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.
Base functor all the models that need non linear optimization must define their own one and implement...
SampleConsensusModelCircle3D(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelCircle3D.
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.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
typename PointCloud::Ptr PointCloudPtr
SampleConsensusModelCircle3D defines a model for 3D circle segmentation.
SampleConsensusModel represents the base model class.
std::string model_name_
The model name.
SampleConsensusModelCircle3D(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCircle3D.
typename PointCloud::ConstPtr PointCloudConstPtr
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
SampleConsensusModelCircle3D(const SampleConsensusModelCircle3D &source)
Copy constructor.
shared_ptr< const SampleConsensusModel< pcl::PointXYZRGB > > ConstPtr
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.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_CIRCLE3D).
IndicesPtr indices_
A pointer to the vector of point indices to use.
A point structure representing Euclidean xyz coordinates, and the RGB color.
shared_ptr< SampleConsensusModel< pcl::PointXYZRGB > > Ptr
~SampleConsensusModelCircle3D()
Empty destructor.
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 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.
unsigned int sample_size_
The size of a sample from which the model is computed.
SampleConsensusModelCircle3D & operator=(const SampleConsensusModelCircle3D &source)
Copy constructor.