44 #include <xmmintrin.h>
47 #include <immintrin.h>
50 #include <pcl/sample_consensus/sac_model.h>
51 #include <pcl/sample_consensus/model_types.h>
61 template <
typename Po
int>
inline void
62 projectPoint (
const Point &p,
const Eigen::Vector4f &model_coefficients, Point &q)
65 Eigen::Vector4f pp (p.x, p.y, p.z, 1);
67 float distance_to_plane = pp.dot(model_coefficients);
74 Eigen::Vector4f q_e = pp - distance_to_plane * model_coefficients;
88 template <
typename Po
int>
inline double
91 return (a * p.x + b * p.y + c * p.z + d);
99 template <
typename Po
int>
inline double
102 return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] );
113 template <
typename Po
int>
inline double
124 template <
typename Po
int>
inline double
141 template <
typename Po
intT>
155 using Ptr = shared_ptr<SampleConsensusModelPlane<PointT> >;
156 using ConstPtr = shared_ptr<const SampleConsensusModelPlane<PointT>>;
196 Eigen::VectorXf &model_coefficients)
const override;
204 std::vector<double> &distances)
const override;
213 const double threshold,
224 const double threshold)
const override;
234 const Eigen::VectorXf &model_coefficients,
235 Eigen::VectorXf &optimized_coefficients)
const override;
245 const Eigen::VectorXf &model_coefficients,
247 bool copy_data_fields =
true)
const override;
256 const Eigen::VectorXf &model_coefficients,
257 const double threshold)
const override;
272 const double threshold,
273 std::size_t i = 0)
const;
275 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
280 countWithinDistanceSSE (
const Eigen::VectorXf &model_coefficients,
281 const double threshold,
282 std::size_t i = 0)
const;
285 #if defined (__AVX__) && defined (__AVX2__)
290 countWithinDistanceAVX (
const Eigen::VectorXf &model_coefficients,
291 const double threshold,
292 std::size_t i = 0)
const;
296 inline __m256 dist8 (
const std::size_t i,
const __m256 &a_vec,
const __m256 &b_vec,
const __m256 &c_vec,
const __m256 &d_vec,
const __m256 &abs_help)
const;
300 inline __m128 dist4 (
const std::size_t i,
const __m128 &a_vec,
const __m128 &b_vec,
const __m128 &c_vec,
const __m128 &d_vec,
const __m128 &abs_help)
const;
309 isSampleGood (
const Indices &samples)
const override;
313 #ifdef PCL_NO_PRECOMPILE
314 #include <pcl/sample_consensus/impl/sac_model_plane.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
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
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
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the plane coefficients using the given inlier set and return them to the user.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given plane 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 plane model coefficients.
~SampleConsensusModelPlane() override=default
Empty destructor.
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.
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 plane model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
std::size_t countWithinDistanceStandard(const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i=0) const
This implementation uses no SIMD instructions.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_PLANE).
SampleConsensusModelPlane(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelPlane.
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.
SampleConsensusModelPlane(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelPlane.
double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
double pointToPlaneDistance(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0.
void projectPoint(const Point &p, const Eigen::Vector4f &model_coefficients, Point &q)
Project a point on a planar model given by a set of normalized coefficients.
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.