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_);
63 Eigen::Array4f dy1dy2 = (p1-p0) / (p2-p0);
65 return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) );
69 template <
typename Po
intT>
bool
71 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
74 if (samples.size () != sample_size_)
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;
90 Eigen::Array4f dy1dy2 = p1p0 / p2p0;
91 if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) )
98 model_coefficients.resize (model_size_);
99 model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
100 model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
101 model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
102 model_coefficients[3] = 0.0f;
105 model_coefficients.normalize ();
108 model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
110 PCL_DEBUG (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
111 model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);
115 #define AT(POS) ((*input_)[(*indices_)[(POS)]])
119 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
122 return _mm256_andnot_ps (abs_help,
123 _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)),
124 _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))),
125 _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)),
132 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
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)),
146 template <
typename Po
intT>
void
148 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
151 if (!isModelValid (model_coefficients))
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,
172 distances[i] = std::abs (model_coefficients.dot (pt));
177 template <
typename Po
intT>
void
179 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
182 if (!isModelValid (model_coefficients))
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,
203 float distance = std::abs (model_coefficients.dot (pt));
208 inliers.push_back ((*indices_)[i]);
209 error_sqr_dists_.push_back (
static_cast<double> (
distance));
215 template <
typename Po
intT> std::size_t
217 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
220 if (!isModelValid (model_coefficients))
222 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
225 #if defined (__AVX__) && defined (__AVX2__)
226 return countWithinDistanceAVX (model_coefficients, threshold);
227 #elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
228 return countWithinDistanceSSE (model_coefficients, threshold);
230 return countWithinDistanceStandard (model_coefficients, threshold);
235 template <
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,
249 if (std::abs (model_coefficients.dot (pt)) < threshold)
258 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
259 template <
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;
264 const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
265 const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
266 const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
267 const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
268 const __m128 threshold_vec = _mm_set1_ps (threshold);
269 const __m128 abs_help = _mm_set1_ps (-0.0F);
270 __m128i res = _mm_set1_epi32(0);
271 for (; (i + 4) <= indices_->size (); i += 4)
273 const __m128 mask = _mm_cmplt_ps (dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec);
274 res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask)));
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__)
294 template <
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);
336 template <
typename Po
intT>
void
338 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
341 if (!isModelValid (model_coefficients))
343 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
344 optimized_coefficients = model_coefficients;
349 if (inliers.size () <= sample_size_)
351 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
352 optimized_coefficients = model_coefficients;
356 Eigen::Vector4f plane_parameters;
359 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
360 Eigen::Vector4f xyz_centroid;
365 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
366 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
367 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
370 optimized_coefficients.resize (model_size_);
371 optimized_coefficients[0] = eigen_vector [0];
372 optimized_coefficients[1] = eigen_vector [1];
373 optimized_coefficients[2] = eigen_vector [2];
374 optimized_coefficients[3] = 0.0f;
375 optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid);
378 if (!isModelValid (optimized_coefficients))
380 optimized_coefficients = model_coefficients;
385 template <
typename Po
intT>
void
387 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
390 if (!isModelValid (model_coefficients))
392 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
396 projected_points.
header = input_->header;
397 projected_points.
is_dense = input_->is_dense;
399 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
404 Eigen::Vector4f tmp_mc = model_coefficients;
410 if (copy_data_fields)
413 projected_points.
resize (input_->size ());
414 projected_points.
width = input_->width;
415 projected_points.
height = input_->height;
417 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
419 for (std::size_t i = 0; i < input_->size (); ++i)
424 for (
const auto &inlier : inliers)
427 Eigen::Vector4f p ((*input_)[inlier].x,
432 float distance_to_plane = tmp_mc.dot (p);
435 pp.matrix () = p - mc * distance_to_plane;
441 projected_points.
resize (inliers.size ());
442 projected_points.
width = inliers.size ();
443 projected_points.
height = 1;
445 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
447 for (std::size_t i = 0; i < inliers.size (); ++i)
454 for (std::size_t i = 0; i < inliers.size (); ++i)
457 Eigen::Vector4f p ((*input_)[inliers[i]].x,
458 (*input_)[inliers[i]].y,
459 (*input_)[inliers[i]].z,
462 float distance_to_plane = tmp_mc.dot (p);
465 pp.matrix () = p - mc * distance_to_plane;
471 template <
typename Po
intT>
bool
473 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
476 if (!isModelValid (model_coefficients))
478 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
482 for (
const auto &index : indices)
484 Eigen::Vector4f pt ((*input_)[index].x,
488 if (std::abs (model_coefficients.dot (pt)) > threshold)
497 #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::Array4f, Eigen::Aligned > Array4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Helper functor structure for concatenate.