41#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
44#include <pcl/sample_consensus/sac_model_plane.h>
46#include <pcl/common/eigen.h>
47#include <pcl/common/concatenate.h>
50template <
typename Po
intT>
bool
53 if (
samples.size () != sample_size_)
55 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n",
samples.size (), sample_size_);
69template <
typename Po
intT>
bool
76 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n",
samples.
size ());
85 Eigen::Array4f
p1p0 = p1 -
p0;
87 Eigen::Array4f
p2p0 = p2 -
p0;
110 PCL_DEBUG (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
115#define AT(POS) ((*input_)[(*indices_)[(POS)]])
132template <
typename Po
intT>
inline __m128
pcl::SampleConsensusModelPlane<PointT>::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
135 return _mm_andnot_ps (abs_help,
136 _mm_add_ps (_mm_add_ps (_mm_mul_ps (a_vec, _mm_set_ps (AT(i ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x)),
137 _mm_mul_ps (b_vec, _mm_set_ps (AT(i ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y))),
138 _mm_add_ps (_mm_mul_ps (c_vec, _mm_set_ps (AT(i ).z, AT(i+1).z, AT(i+2).z, AT(i+3).z)),
146template <
typename Po
intT>
void
153 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n");
157 distances.resize (indices_->size ());
160 for (std::size_t i = 0; i < indices_->size (); ++i)
168 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x,
169 (*input_)[(*indices_)[i]].y,
170 (*input_)[(*indices_)[i]].z,
177template <
typename Po
intT>
void
184 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
189 error_sqr_dists_.clear ();
190 inliers.reserve (indices_->size ());
191 error_sqr_dists_.reserve (indices_->size ());
194 for (std::size_t i = 0; i < indices_->size (); ++i)
198 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x,
199 (*input_)[(*indices_)[i]].y,
200 (*input_)[(*indices_)[i]].z,
205 if (distance < threshold)
208 inliers.push_back ((*indices_)[i]);
209 error_sqr_dists_.push_back (
static_cast<double> (distance));
215template <
typename Po
intT> std::size_t
222 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
225#if defined (__AVX__) && defined (__AVX2__)
227#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
235template <
typename Po
intT> std::size_t
237 const Eigen::VectorXf &
model_coefficients,
const double threshold, std::size_t i)
const
239 std::size_t
nr_p = 0;
241 for (; i < indices_->size (); ++i)
245 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x,
246 (*input_)[(*indices_)[i]].y,
247 (*input_)[(*indices_)[i]].z,
258#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
259template <
typename Po
intT> std::size_t
261 const Eigen::VectorXf &
model_coefficients,
const double threshold, std::size_t i)
const
263 std::size_t
nr_p = 0;
271 for (; (i + 4) <= indices_->size (); i += 4)
281 nr_p += _mm_extract_epi32 (res, 0);
282 nr_p += _mm_extract_epi32 (res, 1);
283 nr_p += _mm_extract_epi32 (res, 2);
284 nr_p += _mm_extract_epi32 (res, 3);
287 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
293#if defined (__AVX__) && defined (__AVX2__)
294template <
typename Po
intT> std::size_t
296 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
298 std::size_t nr_p = 0;
299 const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
300 const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
301 const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
302 const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
303 const __m256 threshold_vec = _mm256_set1_ps (threshold);
304 const __m256 abs_help = _mm256_set1_ps (-0.0F);
305 __m256i res = _mm256_set1_epi32(0);
306 for (; (i + 8) <= indices_->size (); i += 8)
308 const __m256 mask = _mm256_cmp_ps (dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec, _CMP_LT_OQ);
309 res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask)));
320 nr_p += _mm256_extract_epi32 (res, 0);
321 nr_p += _mm256_extract_epi32 (res, 1);
322 nr_p += _mm256_extract_epi32 (res, 2);
323 nr_p += _mm256_extract_epi32 (res, 3);
324 nr_p += _mm256_extract_epi32 (res, 4);
325 nr_p += _mm256_extract_epi32 (res, 5);
326 nr_p += _mm256_extract_epi32 (res, 6);
327 nr_p += _mm256_extract_epi32 (res, 7);
330 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
336template <
typename Po
intT>
void
343 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
351 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n",
inliers.
size ());
360 Eigen::Vector4f xyz_centroid;
385template <
typename Po
intT>
void
392 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
417 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
419 for (std::size_t i = 0; i < input_->size (); ++i)
427 Eigen::Vector4f p ((*input_)[
inlier].x,
445 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
457 Eigen::Vector4f p ((*input_)[
inliers[i]].x,
471template <
typename Po
intT>
bool
473 const std::set<index_t> &indices,
const Eigen::VectorXf &
model_coefficients,
const double threshold)
const
478 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
482 for (
const auto &index : indices)
484 Eigen::Vector4f
pt ((*input_)[index].x,
497#define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
Define methods for centroid estimation and covariance matrix calculus.
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
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.
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.
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.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.