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 p2p0 = p2 - p0;
64 Eigen::Array4f dy1dy2 = (p1-p0) / p2p0;
66 return ( ((dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1])) && (!p2p0.isZero()) );
70 template <
typename Po
intT>
bool
72 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
75 if (samples.size () != sample_size_)
77 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
86 Eigen::Array4f p1p0 = p1 - p0;
88 Eigen::Array4f p2p0 = p2 - p0;
91 Eigen::Array4f dy1dy2 = p1p0 / p2p0;
92 if ( p2p0.isZero() || ((dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1])) )
99 model_coefficients.resize (model_size_);
100 model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
101 model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
102 model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
103 model_coefficients[3] = 0.0f;
106 model_coefficients.normalize ();
109 model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
111 PCL_DEBUG (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
112 model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);
116 #define AT(POS) ((*input_)[(*indices_)[(POS)]])
120 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
123 return _mm256_andnot_ps (abs_help,
124 _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)),
125 _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))),
126 _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)),
129 #endif // ifdef __AVX__
133 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
136 return _mm_andnot_ps (abs_help,
137 _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)),
138 _mm_mul_ps (b_vec, _mm_set_ps (AT(i ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y))),
139 _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)),
142 #endif // ifdef __SSE__
147 template <
typename Po
intT>
void
149 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
152 if (!isModelValid (model_coefficients))
154 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n");
158 distances.resize (indices_->size ());
161 for (std::size_t i = 0; i < indices_->size (); ++i)
169 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
170 (*input_)[(*indices_)[i]].y,
171 (*input_)[(*indices_)[i]].z,
173 distances[i] = std::abs (model_coefficients.dot (pt));
178 template <
typename Po
intT>
void
180 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
183 if (!isModelValid (model_coefficients))
185 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
190 error_sqr_dists_.clear ();
191 inliers.reserve (indices_->size ());
192 error_sqr_dists_.reserve (indices_->size ());
195 for (std::size_t i = 0; i < indices_->size (); ++i)
199 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
200 (*input_)[(*indices_)[i]].y,
201 (*input_)[(*indices_)[i]].z,
204 float distance = std::abs (model_coefficients.dot (pt));
209 inliers.push_back ((*indices_)[i]);
210 error_sqr_dists_.push_back (
static_cast<double> (
distance));
216 template <
typename Po
intT> std::size_t
218 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
221 if (!isModelValid (model_coefficients))
223 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
226 #if defined (__AVX__) && defined (__AVX2__)
227 return countWithinDistanceAVX (model_coefficients, threshold);
228 #elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
229 return countWithinDistanceSSE (model_coefficients, threshold);
231 return countWithinDistanceStandard (model_coefficients, threshold);
236 template <
typename Po
intT> std::size_t
238 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
240 std::size_t nr_p = 0;
242 for (; i < indices_->size (); ++i)
246 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
247 (*input_)[(*indices_)[i]].y,
248 (*input_)[(*indices_)[i]].z,
250 if (std::abs (model_coefficients.dot (pt)) < threshold)
259 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
260 template <
typename Po
intT> std::size_t
262 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
264 std::size_t nr_p = 0;
265 const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
266 const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
267 const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
268 const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
269 const __m128 threshold_vec = _mm_set1_ps (threshold);
270 const __m128 abs_help = _mm_set1_ps (-0.0F);
271 __m128i res = _mm_set1_epi32(0);
272 for (; (i + 4) <= indices_->size (); i += 4)
274 const __m128 mask = _mm_cmplt_ps (dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec);
275 res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask)));
282 nr_p += _mm_extract_epi32 (res, 0);
283 nr_p += _mm_extract_epi32 (res, 1);
284 nr_p += _mm_extract_epi32 (res, 2);
285 nr_p += _mm_extract_epi32 (res, 3);
288 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
294 #if defined (__AVX__) && defined (__AVX2__)
295 template <
typename Po
intT> std::size_t
297 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
299 std::size_t nr_p = 0;
300 const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
301 const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
302 const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
303 const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
304 const __m256 threshold_vec = _mm256_set1_ps (threshold);
305 const __m256 abs_help = _mm256_set1_ps (-0.0F);
306 __m256i res = _mm256_set1_epi32(0);
307 for (; (i + 8) <= indices_->size (); i += 8)
309 const __m256 mask = _mm256_cmp_ps (dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec, _CMP_LT_OQ);
310 res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask)));
321 nr_p += _mm256_extract_epi32 (res, 0);
322 nr_p += _mm256_extract_epi32 (res, 1);
323 nr_p += _mm256_extract_epi32 (res, 2);
324 nr_p += _mm256_extract_epi32 (res, 3);
325 nr_p += _mm256_extract_epi32 (res, 4);
326 nr_p += _mm256_extract_epi32 (res, 5);
327 nr_p += _mm256_extract_epi32 (res, 6);
328 nr_p += _mm256_extract_epi32 (res, 7);
331 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
337 template <
typename Po
intT>
void
339 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
342 if (!isModelValid (model_coefficients))
344 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
345 optimized_coefficients = model_coefficients;
350 if (inliers.size () <= sample_size_)
352 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
353 optimized_coefficients = model_coefficients;
357 Eigen::Vector4f plane_parameters;
360 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
361 Eigen::Vector4f xyz_centroid;
366 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
367 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
368 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
371 optimized_coefficients.resize (model_size_);
372 optimized_coefficients[0] = eigen_vector [0];
373 optimized_coefficients[1] = eigen_vector [1];
374 optimized_coefficients[2] = eigen_vector [2];
375 optimized_coefficients[3] = 0.0f;
376 optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid);
379 if (!isModelValid (optimized_coefficients))
381 optimized_coefficients = model_coefficients;
386 template <
typename Po
intT>
void
388 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
391 if (!isModelValid (model_coefficients))
393 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
397 projected_points.
header = input_->header;
398 projected_points.
is_dense = input_->is_dense;
400 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
405 Eigen::Vector4f tmp_mc = model_coefficients;
411 if (copy_data_fields)
414 projected_points.
resize (input_->size ());
415 projected_points.
width = input_->width;
416 projected_points.
height = input_->height;
418 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
420 for (std::size_t i = 0; i < input_->size (); ++i)
425 for (
const auto &inlier : inliers)
428 Eigen::Vector4f p ((*input_)[inlier].x,
433 float distance_to_plane = tmp_mc.dot (p);
436 pp.matrix () = p - mc * distance_to_plane;
442 projected_points.
resize (inliers.size ());
443 projected_points.
width = inliers.size ();
444 projected_points.
height = 1;
446 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
448 for (std::size_t i = 0; i < inliers.size (); ++i)
455 for (std::size_t i = 0; i < inliers.size (); ++i)
458 Eigen::Vector4f p ((*input_)[inliers[i]].x,
459 (*input_)[inliers[i]].y,
460 (*input_)[inliers[i]].z,
463 float distance_to_plane = tmp_mc.dot (p);
466 pp.matrix () = p - mc * distance_to_plane;
472 template <
typename Po
intT>
bool
474 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
477 if (!isModelValid (model_coefficients))
479 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
483 for (
const auto &index : indices)
485 Eigen::Vector4f pt ((*input_)[index].x,
489 if (std::abs (model_coefficients.dot (pt)) > threshold)
498 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
500 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_