44 #include <xmmintrin.h>
47 #include <immintrin.h>
50 #include <pcl/sample_consensus/sac_model.h>
51 #include <pcl/sample_consensus/model_types.h>
65 template <
typename Po
intT>
80 using Ptr = shared_ptr<SampleConsensusModelSphere<PointT> >;
81 using ConstPtr = shared_ptr<const SampleConsensusModelSphere<PointT>>;
142 Eigen::VectorXf &model_coefficients)
const override;
150 std::vector<double> &distances)
const override;
159 const double threshold,
170 const double threshold)
const override;
180 const Eigen::VectorXf &model_coefficients,
181 Eigen::VectorXf &optimized_coefficients)
const override;
192 const Eigen::VectorXf &model_coefficients,
194 bool copy_data_fields =
true)
const override;
203 const Eigen::VectorXf &model_coefficients,
204 const double threshold)
const override;
217 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override
242 const double threshold,
243 std::size_t i = 0)
const;
245 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
250 countWithinDistanceSSE (
const Eigen::VectorXf &model_coefficients,
251 const double threshold,
252 std::size_t i = 0)
const;
255 #if defined (__AVX__) && defined (__AVX2__)
260 countWithinDistanceAVX (
const Eigen::VectorXf &model_coefficients,
261 const double threshold,
262 std::size_t i = 0)
const;
281 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const
283 Eigen::Vector4f cen_t;
285 for (
int i = 0; i < values (); ++i)
288 cen_t.head<3>() = (*model_->input_)[
indices_[i]].getVector3fMap() - x.head<3>();
291 fvec[i] = std::sqrt (cen_t.dot (cen_t)) - x[3];
301 inline __m256 sqr_dist8 (
const std::size_t i,
const __m256 a_vec,
const __m256 b_vec,
const __m256 c_vec)
const;
305 inline __m128 sqr_dist4 (
const std::size_t i,
const __m128 a_vec,
const __m128 b_vec,
const __m128 c_vec)
const;
310 #ifdef PCL_NO_PRECOMPILE
311 #include <pcl/sample_consensus/impl/sac_model_sphere.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModel represents the base model class.
double radius_min_
The minimum and maximum radius limits for the model.
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
SampleConsensusModelSphere defines a model for 3D sphere segmentation.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
SampleConsensusModelSphere & operator=(const SampleConsensusModelSphere &source)
Copy constructor.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_SPHERE).
SampleConsensusModelSphere(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelSphere.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given sphere model.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the sphere coefficients using the given inlier set and return them to the user.
SampleConsensusModelSphere(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelSphere.
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.
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 sphere model coefficients.
std::size_t countWithinDistanceStandard(const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i=0) const
This implementation uses no SIMD instructions.
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 sphere model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid sphere model, compute the model coefficients f...
~SampleConsensusModelSphere() override=default
Empty destructor.
SampleConsensusModelSphere(const SampleConsensusModelSphere &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.
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.