41#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
44#include <pcl/sample_consensus/sac_model_normal_plane.h>
45#include <pcl/sample_consensus/impl/sac_model_plane.hpp>
49template <
typename Po
intT,
typename Po
intNT>
void
55 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
72 error_sqr_dists_.clear ();
73 inliers.reserve (indices_->size ());
74 error_sqr_dists_.reserve (indices_->size ());
77 for (std::size_t i = 0; i < indices_->size (); ++i)
79 const PointT &
pt = (*input_)[(*indices_)[i]];
80 const PointNT &
nt = (*normals_)[(*indices_)[i]];
83 Eigen::Vector4f p (
pt.x,
pt.y,
pt.z, 0.0f);
84 Eigen::Vector4f n (
nt.normal_x,
nt.normal_y,
nt.normal_z, 0.0f);
92 double weight = normal_distance_weight_ * (1.0 -
nt.curvature);
95 if (distance < threshold)
98 inliers.push_back ((*indices_)[i]);
99 error_sqr_dists_.push_back (distance);
105template <
typename Po
intT,
typename Po
intNT> std::size_t
111 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
119#if defined (__AVX__) && defined (__AVX2__)
121#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
129template <
typename Po
intT,
typename Po
intNT> std::size_t
131 const Eigen::VectorXf &
model_coefficients,
const double threshold, std::size_t i)
const
133 std::size_t
nr_p = 0;
140 for (; i < indices_->size (); ++i)
142 const PointT &
pt = (*input_)[(*indices_)[i]];
143 const PointNT &
nt = (*normals_)[(*indices_)[i]];
146 const Eigen::Vector4f p (
pt.x,
pt.y,
pt.z, 0.0f);
147 const Eigen::Vector4f n (
nt.normal_x,
nt.normal_y,
nt.normal_z, 0.0f);
155 const double weight = normal_distance_weight_ * (1.0 -
nt.curvature);
166#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
167template <
typename Po
intT,
typename Po
intNT> std::size_t
169 const Eigen::VectorXf &
model_coefficients,
const double threshold, std::size_t i)
const
171 std::size_t
nr_p = 0;
180 for (; (i + 4) <= indices_->size (); i += 4)
185 _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_x,
186 (*normals_)[(*indices_)[i+1]].normal_x,
187 (*normals_)[(*indices_)[i+2]].normal_x,
188 (*normals_)[(*indices_)[i+3]].normal_x),
189 _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_y,
190 (*normals_)[(*indices_)[i+1]].normal_y,
191 (*normals_)[(*indices_)[i+2]].normal_y,
192 (*normals_)[(*indices_)[i+3]].normal_y),
193 _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_z,
194 (*normals_)[(*indices_)[i+1]].normal_z,
195 (*normals_)[(*indices_)[i+2]].normal_z,
196 (*normals_)[(*indices_)[i+3]].normal_z),
199 _mm_set_ps ((*normals_)[(*indices_)[i ]].curvature,
200 (*normals_)[(*indices_)[i+1]].curvature,
201 (*normals_)[(*indices_)[i+2]].curvature,
202 (*normals_)[(*indices_)[i+3]].curvature)));
207 nr_p += _mm_extract_epi32 (res, 0);
208 nr_p += _mm_extract_epi32 (res, 1);
209 nr_p += _mm_extract_epi32 (res, 2);
210 nr_p += _mm_extract_epi32 (res, 3);
213 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
219#if defined (__AVX__) && defined (__AVX2__)
220template <
typename Po
intT,
typename Po
intNT> std::size_t
222 const Eigen::VectorXf &model_coefficients,
const double threshold, std::size_t i)
const
224 std::size_t nr_p = 0;
225 const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
226 const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
227 const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
228 const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
229 const __m256 threshold_vec = _mm256_set1_ps (threshold);
230 const __m256 normal_distance_weight_vec = _mm256_set1_ps (normal_distance_weight_);
231 const __m256 abs_help = _mm256_set1_ps (-0.0F);
232 __m256i res = _mm256_set1_epi32(0);
233 for (; (i + 8) <= indices_->size (); i += 8)
237 const __m256 d_normal_vec = getAcuteAngle3DAVX (
238 _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_x,
239 (*normals_)[(*indices_)[i+1]].normal_x,
240 (*normals_)[(*indices_)[i+2]].normal_x,
241 (*normals_)[(*indices_)[i+3]].normal_x,
242 (*normals_)[(*indices_)[i+4]].normal_x,
243 (*normals_)[(*indices_)[i+5]].normal_x,
244 (*normals_)[(*indices_)[i+6]].normal_x,
245 (*normals_)[(*indices_)[i+7]].normal_x),
246 _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_y,
247 (*normals_)[(*indices_)[i+1]].normal_y,
248 (*normals_)[(*indices_)[i+2]].normal_y,
249 (*normals_)[(*indices_)[i+3]].normal_y,
250 (*normals_)[(*indices_)[i+4]].normal_y,
251 (*normals_)[(*indices_)[i+5]].normal_y,
252 (*normals_)[(*indices_)[i+6]].normal_y,
253 (*normals_)[(*indices_)[i+7]].normal_y),
254 _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_z,
255 (*normals_)[(*indices_)[i+1]].normal_z,
256 (*normals_)[(*indices_)[i+2]].normal_z,
257 (*normals_)[(*indices_)[i+3]].normal_z,
258 (*normals_)[(*indices_)[i+4]].normal_z,
259 (*normals_)[(*indices_)[i+5]].normal_z,
260 (*normals_)[(*indices_)[i+6]].normal_z,
261 (*normals_)[(*indices_)[i+7]].normal_z),
262 a_vec, b_vec, c_vec);
263 const __m256 weight_vec = _mm256_mul_ps (normal_distance_weight_vec, _mm256_sub_ps (_mm256_set1_ps (1.0f),
264 _mm256_set_ps ((*normals_)[(*indices_)[i ]].curvature,
265 (*normals_)[(*indices_)[i+1]].curvature,
266 (*normals_)[(*indices_)[i+2]].curvature,
267 (*normals_)[(*indices_)[i+3]].curvature,
268 (*normals_)[(*indices_)[i+4]].curvature,
269 (*normals_)[(*indices_)[i+5]].curvature,
270 (*normals_)[(*indices_)[i+6]].curvature,
271 (*normals_)[(*indices_)[i+7]].curvature)));
272 const __m256 dist = _mm256_andnot_ps (abs_help, _mm256_add_ps (_mm256_mul_ps (weight_vec, d_normal_vec), _mm256_mul_ps (_mm256_sub_ps (_mm256_set1_ps (1.0f), weight_vec), d_euclid_vec)));
273 const __m256 mask = _mm256_cmp_ps (dist, threshold_vec, _CMP_LT_OQ);
274 res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask)));
276 nr_p += _mm256_extract_epi32 (res, 0);
277 nr_p += _mm256_extract_epi32 (res, 1);
278 nr_p += _mm256_extract_epi32 (res, 2);
279 nr_p += _mm256_extract_epi32 (res, 3);
280 nr_p += _mm256_extract_epi32 (res, 4);
281 nr_p += _mm256_extract_epi32 (res, 5);
282 nr_p += _mm256_extract_epi32 (res, 6);
283 nr_p += _mm256_extract_epi32 (res, 7);
286 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
292template <
typename Po
intT,
typename Po
intNT>
void
298 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
313 distances.resize (indices_->size ());
316 for (std::size_t i = 0; i < indices_->size (); ++i)
318 const PointT &
pt = (*input_)[(*indices_)[i]];
319 const PointNT &
nt = (*normals_)[(*indices_)[i]];
322 Eigen::Vector4f p (
pt.x,
pt.y,
pt.z, 0.0f);
323 Eigen::Vector4f n (
nt.normal_x,
nt.normal_y,
nt.normal_z, 0.0f);
331 double weight = normal_distance_weight_ * (1.0 -
nt.curvature);
337#define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>;
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
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.
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 getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given plane model.
std::size_t countWithinDistanceStandard(const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i=0) const
This implementation uses no SIMD instructions.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
Define standard C methods and C++ classes that are common to all methods.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.