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>
50 template <
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_);
70 if ((p1 - p0).cross(p2 - p0).stableNorm() < Eigen::NumTraits<float>::dummy_precision ())
72 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::isSampleGood] Sample points too similar or collinear!\n");
80 template <
typename Po
intT>
bool
82 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
87 if (samples.size () != sample_size_)
89 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
97 const Eigen::Vector3f cross = (p1 - p0).cross(p2 - p0);
98 const float crossNorm = cross.stableNorm();
101 if (crossNorm < Eigen::NumTraits<float>::dummy_precision ())
103 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Chosen samples are collinear!\n");
109 model_coefficients.resize (model_size_);
110 model_coefficients.template head<3>() = cross / crossNorm;
113 model_coefficients[3] = -1.0f * (model_coefficients.template head<3>().dot (p0));
115 PCL_DEBUG (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
116 model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);
120 #define AT(POS) ((*input_)[(*indices_)[(POS)]])
124 template <
typename Po
intT>
inline __m256
pcl::SampleConsensusModelPlane<PointT>::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
127 return _mm256_andnot_ps (abs_help,
128 _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (a_vec, _mm256_set_ps (AT(i ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x, AT(i+4).x, AT(i+5).x, AT(i+6).x, AT(i+7).x)),
129 _mm256_mul_ps (b_vec, _mm256_set_ps (AT(i ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y, AT(i+4).y, AT(i+5).y, AT(i+6).y, AT(i+7).y))),
130 _mm256_add_ps (_mm256_mul_ps (c_vec, _mm256_set_ps (AT(i ).z, AT(i+1).z, AT(i+2).z, AT(i+3).z, AT(i+4).z, AT(i+5).z, AT(i+6).z, AT(i+7).z)),
137 template <
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
140 return _mm_andnot_ps (abs_help,
141 _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)),
142 _mm_mul_ps (b_vec, _mm_set_ps (AT(i ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y))),
143 _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)),
151 template <
typename Po
intT>
void
153 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
156 if (!isModelValid (model_coefficients))
158 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n");
162 distances.resize (indices_->size ());
165 for (std::size_t i = 0; i < indices_->size (); ++i)
173 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
174 (*input_)[(*indices_)[i]].y,
175 (*input_)[(*indices_)[i]].z,
177 distances[i] = std::abs (model_coefficients.dot (pt));
182 template <
typename Po
intT>
void
184 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
187 if (!isModelValid (model_coefficients))
189 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
194 error_sqr_dists_.clear ();
195 inliers.reserve (indices_->size ());
196 error_sqr_dists_.reserve (indices_->size ());
199 for (std::size_t i = 0; i < indices_->size (); ++i)
203 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
204 (*input_)[(*indices_)[i]].y,
205 (*input_)[(*indices_)[i]].z,
208 float distance = std::abs (model_coefficients.dot (pt));
213 inliers.push_back ((*indices_)[i]);
214 error_sqr_dists_.push_back (
static_cast<double> (
distance));
220 template <
typename Po
intT> std::size_t
222 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
225 if (!isModelValid (model_coefficients))
227 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
230 #if defined (__AVX__) && defined (__AVX2__)
231 return countWithinDistanceAVX (model_coefficients, threshold);
232 #elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
233 return countWithinDistanceSSE (model_coefficients, threshold);
235 return countWithinDistanceStandard (model_coefficients, threshold);
240 template <
typename Po
intT> std::size_t
242 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
244 std::size_t nr_p = 0;
246 for (; i < indices_->size (); ++i)
250 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
251 (*input_)[(*indices_)[i]].y,
252 (*input_)[(*indices_)[i]].z,
254 if (std::abs (model_coefficients.dot (pt)) < threshold)
263 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
264 template <
typename Po
intT> std::size_t
266 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
268 std::size_t nr_p = 0;
269 const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
270 const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
271 const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
272 const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
273 const __m128 threshold_vec = _mm_set1_ps (threshold);
274 const __m128 abs_help = _mm_set1_ps (-0.0F);
275 __m128i res = _mm_set1_epi32(0);
276 for (; (i + 4) <= indices_->size (); i += 4)
278 const __m128 mask = _mm_cmplt_ps (dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec);
279 res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask)));
286 nr_p += _mm_extract_epi32 (res, 0);
287 nr_p += _mm_extract_epi32 (res, 1);
288 nr_p += _mm_extract_epi32 (res, 2);
289 nr_p += _mm_extract_epi32 (res, 3);
292 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
298 #if defined (__AVX__) && defined (__AVX2__)
299 template <
typename Po
intT> std::size_t
301 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
303 std::size_t nr_p = 0;
304 const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
305 const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
306 const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
307 const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
308 const __m256 threshold_vec = _mm256_set1_ps (threshold);
309 const __m256 abs_help = _mm256_set1_ps (-0.0F);
310 __m256i res = _mm256_set1_epi32(0);
311 for (; (i + 8) <= indices_->size (); i += 8)
313 const __m256 mask = _mm256_cmp_ps (dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec, _CMP_LT_OQ);
314 res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask)));
325 nr_p += _mm256_extract_epi32 (res, 0);
326 nr_p += _mm256_extract_epi32 (res, 1);
327 nr_p += _mm256_extract_epi32 (res, 2);
328 nr_p += _mm256_extract_epi32 (res, 3);
329 nr_p += _mm256_extract_epi32 (res, 4);
330 nr_p += _mm256_extract_epi32 (res, 5);
331 nr_p += _mm256_extract_epi32 (res, 6);
332 nr_p += _mm256_extract_epi32 (res, 7);
335 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
341 template <
typename Po
intT>
void
343 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
346 if (!isModelValid (model_coefficients))
348 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
349 optimized_coefficients = model_coefficients;
354 if (inliers.size () <= sample_size_)
356 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
357 optimized_coefficients = model_coefficients;
361 Eigen::Vector4f plane_parameters;
364 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
365 Eigen::Vector4f xyz_centroid;
369 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] computeMeanAndCovarianceMatrix failed (returned 0) because there are no valid inliers.\n");
370 optimized_coefficients = model_coefficients;
375 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
376 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
377 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
380 optimized_coefficients.resize (model_size_);
381 optimized_coefficients[0] = eigen_vector [0];
382 optimized_coefficients[1] = eigen_vector [1];
383 optimized_coefficients[2] = eigen_vector [2];
384 optimized_coefficients[3] = 0.0f;
385 optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid);
388 if (!isModelValid (optimized_coefficients))
390 optimized_coefficients = model_coefficients;
395 template <
typename Po
intT>
void
397 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
400 if (!isModelValid (model_coefficients))
402 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
406 projected_points.
header = input_->header;
407 projected_points.
is_dense = input_->is_dense;
409 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
414 Eigen::Vector4f tmp_mc = model_coefficients;
420 if (copy_data_fields)
423 projected_points.
resize (input_->size ());
424 projected_points.
width = input_->width;
425 projected_points.
height = input_->height;
427 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
429 for (std::size_t i = 0; i < input_->size (); ++i)
434 for (
const auto &inlier : inliers)
437 Eigen::Vector4f p ((*input_)[inlier].x,
442 float distance_to_plane = tmp_mc.dot (p);
445 pp.matrix () = p - mc * distance_to_plane;
451 projected_points.
resize (inliers.size ());
452 projected_points.
width = inliers.size ();
453 projected_points.
height = 1;
455 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
457 for (std::size_t i = 0; i < inliers.size (); ++i)
464 for (std::size_t i = 0; i < inliers.size (); ++i)
467 Eigen::Vector4f p ((*input_)[inliers[i]].x,
468 (*input_)[inliers[i]].y,
469 (*input_)[inliers[i]].z,
472 float distance_to_plane = tmp_mc.dot (p);
475 pp.matrix () = p - mc * distance_to_plane;
481 template <
typename Po
intT>
bool
483 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
486 if (!isModelValid (model_coefficients))
488 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
492 for (
const auto &index : indices)
494 Eigen::Vector4f pt ((*input_)[index].x,
498 if (std::abs (model_coefficients.dot (pt)) > threshold)
507 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
Define methods for centroid estimation and covariance matrix calculus.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
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...
float distance(const PointT &p1, const PointT &p2)
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Helper functor structure for concatenate.