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 ()));
113 #define AT(POS) ((*input_)[(*indices_)[(POS)]])
117 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
120 return _mm256_andnot_ps (abs_help,
121 _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)),
122 _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))),
123 _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)),
126 #endif // ifdef __AVX__
130 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
133 return _mm_andnot_ps (abs_help,
134 _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)),
135 _mm_mul_ps (b_vec, _mm_set_ps (AT(i ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y))),
136 _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)),
139 #endif // ifdef __SSE__
144 template <
typename Po
intT>
void
146 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
149 if (!isModelValid (model_coefficients))
151 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n");
155 distances.resize (indices_->size ());
158 for (std::size_t i = 0; i < indices_->size (); ++i)
166 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
167 (*input_)[(*indices_)[i]].y,
168 (*input_)[(*indices_)[i]].z,
170 distances[i] = std::abs (model_coefficients.dot (pt));
175 template <
typename Po
intT>
void
177 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
180 if (!isModelValid (model_coefficients))
182 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
187 error_sqr_dists_.clear ();
188 inliers.reserve (indices_->size ());
189 error_sqr_dists_.reserve (indices_->size ());
192 for (std::size_t i = 0; i < indices_->size (); ++i)
196 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
197 (*input_)[(*indices_)[i]].y,
198 (*input_)[(*indices_)[i]].z,
201 float distance = std::abs (model_coefficients.dot (pt));
206 inliers.push_back ((*indices_)[i]);
207 error_sqr_dists_.push_back (
static_cast<double> (
distance));
213 template <
typename Po
intT> std::size_t
215 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
218 if (!isModelValid (model_coefficients))
220 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
223 #if defined (__AVX__) && defined (__AVX2__)
224 return countWithinDistanceAVX (model_coefficients, threshold);
225 #elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
226 return countWithinDistanceSSE (model_coefficients, threshold);
228 return countWithinDistanceStandard (model_coefficients, threshold);
233 template <
typename Po
intT> std::size_t
235 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
237 std::size_t nr_p = 0;
239 for (; i < indices_->size (); ++i)
243 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
244 (*input_)[(*indices_)[i]].y,
245 (*input_)[(*indices_)[i]].z,
247 if (std::abs (model_coefficients.dot (pt)) < threshold)
256 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
257 template <
typename Po
intT> std::size_t
259 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
261 std::size_t nr_p = 0;
262 const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
263 const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
264 const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
265 const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
266 const __m128 threshold_vec = _mm_set1_ps (threshold);
267 const __m128 abs_help = _mm_set1_ps (-0.0F);
268 __m128i res = _mm_set1_epi32(0);
269 for (; (i + 4) <= indices_->size (); i += 4)
271 const __m128 mask = _mm_cmplt_ps (dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec);
272 res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask)));
279 nr_p += _mm_extract_epi32 (res, 0);
280 nr_p += _mm_extract_epi32 (res, 1);
281 nr_p += _mm_extract_epi32 (res, 2);
282 nr_p += _mm_extract_epi32 (res, 3);
285 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
291 #if defined (__AVX__) && defined (__AVX2__)
292 template <
typename Po
intT> std::size_t
294 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
296 std::size_t nr_p = 0;
297 const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
298 const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
299 const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
300 const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
301 const __m256 threshold_vec = _mm256_set1_ps (threshold);
302 const __m256 abs_help = _mm256_set1_ps (-0.0F);
303 __m256i res = _mm256_set1_epi32(0);
304 for (; (i + 8) <= indices_->size (); i += 8)
306 const __m256 mask = _mm256_cmp_ps (dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help), threshold_vec, _CMP_LT_OQ);
307 res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask)));
318 nr_p += _mm256_extract_epi32 (res, 0);
319 nr_p += _mm256_extract_epi32 (res, 1);
320 nr_p += _mm256_extract_epi32 (res, 2);
321 nr_p += _mm256_extract_epi32 (res, 3);
322 nr_p += _mm256_extract_epi32 (res, 4);
323 nr_p += _mm256_extract_epi32 (res, 5);
324 nr_p += _mm256_extract_epi32 (res, 6);
325 nr_p += _mm256_extract_epi32 (res, 7);
328 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
334 template <
typename Po
intT>
void
336 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
339 if (!isModelValid (model_coefficients))
341 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
342 optimized_coefficients = model_coefficients;
347 if (inliers.size () <= sample_size_)
349 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
350 optimized_coefficients = model_coefficients;
354 Eigen::Vector4f plane_parameters;
357 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
358 Eigen::Vector4f xyz_centroid;
363 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
364 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
365 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
368 optimized_coefficients.resize (model_size_);
369 optimized_coefficients[0] = eigen_vector [0];
370 optimized_coefficients[1] = eigen_vector [1];
371 optimized_coefficients[2] = eigen_vector [2];
372 optimized_coefficients[3] = 0.0f;
373 optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid);
376 if (!isModelValid (optimized_coefficients))
378 optimized_coefficients = model_coefficients;
383 template <
typename Po
intT>
void
385 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
388 if (!isModelValid (model_coefficients))
390 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
394 projected_points.
header = input_->header;
395 projected_points.
is_dense = input_->is_dense;
397 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
402 Eigen::Vector4f tmp_mc = model_coefficients;
408 if (copy_data_fields)
411 projected_points.
resize (input_->size ());
412 projected_points.
width = input_->width;
413 projected_points.
height = input_->height;
415 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
417 for (std::size_t i = 0; i < input_->size (); ++i)
422 for (
const auto &inlier : inliers)
425 Eigen::Vector4f p ((*input_)[inlier].x,
430 float distance_to_plane = tmp_mc.dot (p);
433 pp.matrix () = p - mc * distance_to_plane;
439 projected_points.
resize (inliers.size ());
440 projected_points.
width = inliers.size ();
441 projected_points.
height = 1;
443 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
445 for (std::size_t i = 0; i < inliers.size (); ++i)
452 for (std::size_t i = 0; i < inliers.size (); ++i)
455 Eigen::Vector4f p ((*input_)[inliers[i]].x,
456 (*input_)[inliers[i]].y,
457 (*input_)[inliers[i]].z,
460 float distance_to_plane = tmp_mc.dot (p);
463 pp.matrix () = p - mc * distance_to_plane;
469 template <
typename Po
intT>
bool
471 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
474 if (!isModelValid (model_coefficients))
476 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
480 for (
const auto &index : indices)
482 Eigen::Vector4f pt ((*input_)[index].x,
486 if (std::abs (model_coefficients.dot (pt)) > threshold)
495 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
497 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_