44#include <pcl/sample_consensus/model_types.h>
45#include <pcl/sample_consensus/sac_model.h>
63template <
typename Po
intT,
typename Po
intNT>
81 using Ptr = shared_ptr<SampleConsensusModelTorus<PointT, PointNT>>;
82 using ConstPtr = shared_ptr<const SampleConsensusModelTorus<PointT, PointNT>>;
145 Eigen::VectorXf& model_coefficients)
const override;
153 std::vector<double>& distances)
const override;
164 const double threshold,
175 const double threshold)
const override;
186 const Eigen::VectorXf& model_coefficients,
187 Eigen::VectorXf& optimized_coefficients)
const override;
197 const Eigen::VectorXf& model_coefficients,
199 bool copy_data_fields =
true)
const override;
210 const Eigen::VectorXf& model_coefficients,
211 const double threshold)
const override;
232 const Eigen::Vector3f& pt_n,
233 const Eigen::VectorXf& model_coefficients,
234 Eigen::Vector3f& pt_proj)
const;
240 isModelValid(
const Eigen::VectorXf& model_coefficients)
const override;
257 :
pcl::
Functor<double>(indices.size()), model_(model), indices_(indices)
266 operator()(
const Eigen::VectorXd& xs, Eigen::VectorXd& fvec)
const
269 const double& R = xs[0];
270 const double& r = xs[1];
272 const double& x0 = xs[2];
273 const double& y0 = xs[3];
274 const double& z0 = xs[4];
276 const Eigen::Vector3d centroid{x0, y0, z0};
278 const double& nx = xs[5];
279 const double& ny = xs[6];
280 const double& nz = xs[7];
282 const Eigen::Vector3d n1{0.0, 0.0, 1.0};
283 const Eigen::Vector3d n2 = Eigen::Vector3d{nx, ny, nz}.normalized();
285 for (
size_t j = 0; j < indices_.size(); j++) {
287 size_t i = indices_[j];
288 const Eigen::Vector3d pt =
289 (*model_->input_)[i].getVector3fMap().template cast<double>();
291 Eigen::Vector3d pte{pt - centroid};
295 pte = Eigen::Quaterniond()
296 .setFromTwoVectors(n1, n2)
301 const double& x = pte[0];
302 const double& y = pte[1];
303 const double& z = pte[2];
305 fvec[j] = (std::pow(sqrt(x * x + y * y) - R, 2) + z * z - r * r);
316#ifdef PCL_NO_PRECOMPILE
317#include <pcl/sample_consensus/impl/sac_model_torus.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
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.
SampleConsensusModelTorus defines a model for 3D torus segmentation.
void projectPointToTorus(const Eigen::Vector3f &pt, const Eigen::Vector3f &pt_n, const Eigen::VectorXf &model_coefficients, Eigen::Vector3f &pt_proj) const
Project a point onto a torus given by its model coefficients (radii, torus_center_point,...
SampleConsensusModelTorus(const SampleConsensusModelTorus &source)
Copy constructor.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
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 torus model coefficients.
SampleConsensusModelTorus & operator=(const SampleConsensusModelTorus &source)
Copy constructor.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_TORUS).
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid torus model, compute the model coefficients fr...
shared_ptr< SampleConsensusModelTorus< PointT, PointNT > > Ptr
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the torus coefficients using the given inlier set and return them to the user.
~SampleConsensusModelTorus() override=default
Empty destructor.
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.
shared_ptr< const SampleConsensusModelTorus< PointT, PointNT > > ConstPtr
SampleConsensusModelTorus(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelTorus.
SampleConsensusModelTorus(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelTorus.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given torus model.
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 torus model.
Define standard C methods to do distance calculations.
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.