62 projectPoint (
const Point &p,
const Eigen::Vector4f &model_coefficients, Point &q)
65 Eigen::Vector4f pp (p.x, p.y, p.z, 1);
67 float distance_to_plane = pp.dot(model_coefficients);
74 Eigen::Vector4f q_e = pp - distance_to_plane * model_coefficients;
155 using Ptr = shared_ptr<SampleConsensusModelPlane<PointT> >;
156 using ConstPtr = shared_ptr<const SampleConsensusModelPlane<PointT>>;
165 model_name_ =
"SampleConsensusModelPlane";
180 model_name_ =
"SampleConsensusModelPlane";
195 computeModelCoefficients (
const Indices &samples,
196 Eigen::VectorXf &model_coefficients)
const override;
203 getDistancesToModel (
const Eigen::VectorXf &model_coefficients,
204 std::vector<double> &distances)
const override;
212 selectWithinDistance (
const Eigen::VectorXf &model_coefficients,
213 const double threshold,
223 countWithinDistance (
const Eigen::VectorXf &model_coefficients,
224 const double threshold)
const override;
233 optimizeModelCoefficients (
const Indices &inliers,
234 const Eigen::VectorXf &model_coefficients,
235 Eigen::VectorXf &optimized_coefficients)
const override;
244 projectPoints (
const Indices &inliers,
245 const Eigen::VectorXf &model_coefficients,
247 bool copy_data_fields =
true)
const override;
255 doSamplesVerifyModel (
const std::set<index_t> &indices,
256 const Eigen::VectorXf &model_coefficients,
257 const double threshold)
const override;
271 countWithinDistanceStandard (
const Eigen::VectorXf &model_coefficients,
272 const double threshold,
273 std::size_t i = 0)
const;
275#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
280 countWithinDistanceSSE (
const Eigen::VectorXf &model_coefficients,
281 const double threshold,
282 std::size_t i = 0)
const;
285#if defined (__AVX__) && defined (__AVX2__)
290 countWithinDistanceAVX (
const Eigen::VectorXf &model_coefficients,
291 const double threshold,
292 std::size_t i = 0)
const;
295#define PCLAT(POS) ((*input_)[(*indices_)[(POS)]])
299inline __m256 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
302 return _mm256_andnot_ps (abs_help,
303 _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (a_vec, _mm256_set_ps (PCLAT(i ).x, PCLAT(i+1).x, PCLAT(i+2).x, PCLAT(i+3).x, PCLAT(i+4).x, PCLAT(i+5).x, PCLAT(i+6).x, PCLAT(i+7).x)),
304 _mm256_mul_ps (b_vec, _mm256_set_ps (PCLAT(i ).y, PCLAT(i+1).y, PCLAT(i+2).y, PCLAT(i+3).y, PCLAT(i+4).y, PCLAT(i+5).y, PCLAT(i+6).y, PCLAT(i+7).y))),
305 _mm256_add_ps (_mm256_mul_ps (c_vec, _mm256_set_ps (PCLAT(i ).z, PCLAT(i+1).z, PCLAT(i+2).z, PCLAT(i+3).z, PCLAT(i+4).z, PCLAT(i+5).z, PCLAT(i+6).z, PCLAT(i+7).z)),
312inline __m128 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
315 return _mm_andnot_ps (abs_help,
316 _mm_add_ps (_mm_add_ps (_mm_mul_ps (a_vec, _mm_set_ps (PCLAT(i ).x, PCLAT(i+1).x, PCLAT(i+2).x, PCLAT(i+3).x)),
317 _mm_mul_ps (b_vec, _mm_set_ps (PCLAT(i ).y, PCLAT(i+1).y, PCLAT(i+2).y, PCLAT(i+3).y))),
318 _mm_add_ps (_mm_mul_ps (c_vec, _mm_set_ps (PCLAT(i ).z, PCLAT(i+1).z, PCLAT(i+2).z, PCLAT(i+3).z)),
331 isSampleGood (
const Indices &samples)
const override;
double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
double pointToPlaneDistance(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0.