39#include <pcl/features/integral_image_normal.h>
44template <
typename Po
intInT,
typename Po
intOutT>
50 delete[] distance_map_;
54template <
typename Po
intInT,
typename Po
intOutT>
void
57 if (border_policy_ != BORDER_POLICY_IGNORE &&
58 border_policy_ != BORDER_POLICY_MIRROR)
60 "[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
62 if (normal_estimation_method_ != COVARIANCE_MATRIX &&
63 normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
64 normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
65 normal_estimation_method_ != SIMPLE_3D_GRADIENT)
67 "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
73 delete[] distance_map_;
76 depth_data_ =
nullptr;
77 distance_map_ =
nullptr;
79 if (normal_estimation_method_ == COVARIANCE_MATRIX)
80 initCovarianceMatrixMethod ();
81 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
82 initAverage3DGradientMethod ();
83 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
84 initAverageDepthChangeMethod ();
85 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
86 initSimple3DGradientMethod ();
91template <
typename Po
intInT,
typename Po
intOutT>
void
95 rect_width_2_ = width/2;
96 rect_width_4_ = width/4;
97 rect_height_ = height;
98 rect_height_2_ = height/2;
99 rect_height_4_ = height/4;
103template <
typename Po
intInT,
typename Po
intOutT>
void
107 int element_stride =
sizeof (PointInT) /
sizeof (
float);
109 int row_stride = element_stride * input_->width;
111 const float *data_ =
reinterpret_cast<const float*
> (&(*input_)[0]);
113 integral_image_XYZ_.setSecondOrderComputation (
false);
114 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
116 init_simple_3d_gradient_ =
true;
117 init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ =
false;
121template <
typename Po
intInT,
typename Po
intOutT>
void
125 int element_stride =
sizeof (PointInT) /
sizeof (
float);
127 int row_stride = element_stride * input_->width;
129 const float *data_ =
reinterpret_cast<const float*
> (&(*input_)[0]);
131 integral_image_XYZ_.setSecondOrderComputation (
true);
132 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
134 init_covariance_matrix_ =
true;
135 init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ =
false;
139template <
typename Po
intInT,
typename Po
intOutT>
void
144 std::size_t data_size = (input_->size () << 2);
145 diff_x_ =
new float[data_size]{};
146 diff_y_ =
new float[data_size]{};
151 const PointInT* point_up = &(input_->points [1]);
152 const PointInT* point_dn = point_up + (input_->width << 1);
153 const PointInT* point_lf = &(input_->points [input_->width]);
154 const PointInT* point_rg = point_lf + 2;
155 float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
156 float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
157 unsigned diff_skip = 8;
159 for (std::size_t ri = 1; ri < input_->height - 1; ++ri
160 , point_up += input_->width
161 , point_dn += input_->width
162 , point_lf += input_->width
163 , point_rg += input_->width
164 , diff_x_ptr += diff_skip
165 , diff_y_ptr += diff_skip)
167 for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
169 diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
170 diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
171 diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
173 diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
174 diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
175 diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
180 integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
181 integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
182 init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ =
false;
183 init_average_3d_gradient_ =
true;
187template <
typename Po
intInT,
typename Po
intOutT>
void
191 int element_stride =
sizeof (PointInT) /
sizeof (
float);
193 int row_stride = element_stride * input_->width;
195 const float *data_ =
reinterpret_cast<const float*
> (&(*input_)[0]);
198 integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
199 init_depth_change_ =
true;
200 init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ =
false;
204template <
typename Po
intInT,
typename Po
intOutT>
void
206 const int pos_x,
const int pos_y,
const unsigned point_index, PointOutT &normal)
208 float bad_point = std::numeric_limits<float>::quiet_NaN ();
210 if (normal_estimation_method_ == COVARIANCE_MATRIX)
212 if (!init_covariance_matrix_)
213 initCovarianceMatrixMethod ();
215 unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
220 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
224 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
225 Eigen::Vector3f center;
227 center = integral_image_XYZ_.
getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
228 so_elements = integral_image_XYZ_.
getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
230 covariance_matrix.coeffRef (0) =
static_cast<float> (so_elements [0]);
231 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) =
static_cast<float> (so_elements [1]);
232 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) =
static_cast<float> (so_elements [2]);
233 covariance_matrix.coeffRef (4) =
static_cast<float> (so_elements [3]);
234 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) =
static_cast<float> (so_elements [4]);
235 covariance_matrix.coeffRef (8) =
static_cast<float> (so_elements [5]);
236 covariance_matrix -= (center * center.transpose ()) /
static_cast<float> (count);
238 Eigen::Vector3f eigen_vector;
239 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
241 normal.getNormalVector3fMap () = eigen_vector;
244 if (eigen_value > 0.0)
245 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
247 normal.curvature = 0;
251 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
253 if (!init_average_3d_gradient_)
254 initAverage3DGradientMethod ();
256 unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
257 unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
258 if (count_x == 0 || count_y == 0)
260 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
263 Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
264 Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
266 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
267 double normal_length = normal_vector.squaredNorm ();
268 if (normal_length == 0.0f)
270 normal.getNormalVector3fMap ().setConstant (bad_point);
271 normal.curvature = bad_point;
275 normal_vector /= sqrt (normal_length);
277 float nx =
static_cast<float> (normal_vector [0]);
278 float ny =
static_cast<float> (normal_vector [1]);
279 float nz =
static_cast<float> (normal_vector [2]);
283 normal.normal_x = nx;
284 normal.normal_y = ny;
285 normal.normal_z = nz;
286 normal.curvature = bad_point;
289 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
291 if (!init_depth_change_)
292 initAverageDepthChangeMethod ();
295 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
296 unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
297 unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
298 unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
300 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
302 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
306 float mean_L_z =
static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_L_z);
307 float mean_R_z =
static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_R_z);
308 float mean_U_z =
static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_) / count_U_z);
309 float mean_D_z =
static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_) / count_D_z);
311 PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
312 PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
313 PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
314 PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
316 const float mean_x_z = mean_R_z - mean_L_z;
317 const float mean_y_z = mean_D_z - mean_U_z;
319 const float mean_x_x = pointR.x - pointL.x;
320 const float mean_x_y = pointR.y - pointL.y;
321 const float mean_y_x = pointD.x - pointU.x;
322 const float mean_y_y = pointD.y - pointU.y;
324 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
325 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
326 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
328 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
330 if (normal_length == 0.0f)
332 normal.getNormalVector3fMap ().setConstant (bad_point);
333 normal.curvature = bad_point;
339 const float scale = 1.0f / std::sqrt (normal_length);
341 normal.normal_x = normal_x * scale;
342 normal.normal_y = normal_y * scale;
343 normal.normal_z = normal_z * scale;
344 normal.curvature = bad_point;
348 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
350 if (!init_simple_3d_gradient_)
351 initSimple3DGradientMethod ();
354 Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
355 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
357 Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
358 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
359 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
360 double normal_length = normal_vector.squaredNorm ();
361 if (normal_length == 0.0f)
363 normal.getNormalVector3fMap ().setConstant (bad_point);
364 normal.curvature = bad_point;
368 normal_vector /= sqrt (normal_length);
370 float nx =
static_cast<float> (normal_vector [0]);
371 float ny =
static_cast<float> (normal_vector [1]);
372 float nz =
static_cast<float> (normal_vector [2]);
376 normal.normal_x = nx;
377 normal.normal_y = ny;
378 normal.normal_z = nz;
379 normal.curvature = bad_point;
383 normal.getNormalVector3fMap ().setConstant (bad_point);
384 normal.curvature = bad_point;
391sumArea (
int start_x,
int start_y,
int end_x,
int end_y,
const int width,
const int height,
392 const std::function<T(
unsigned,
unsigned,
unsigned,
unsigned)> &f,
399 result += f (0, 0, end_x, end_y);
400 result += f (0, 0, -start_x, -start_y);
401 result += f (0, 0, -start_x, end_y);
402 result += f (0, 0, end_x, -start_y);
404 else if (end_y >= height)
406 result += f (0, start_y, end_x, height-1);
407 result += f (0, start_y, -start_x, height-1);
408 result += f (0, height-(end_y-(height-1)), end_x, height-1);
409 result += f (0, height-(end_y-(height-1)), -start_x, height-1);
413 result += f (0, start_y, end_x, end_y);
414 result += f (0, start_y, -start_x, end_y);
417 else if (start_y < 0)
421 result += f (start_x, 0, width-1, end_y);
422 result += f (start_x, 0, width-1, -start_y);
423 result += f (width-(end_x-(width-1)), 0, width-1, end_y);
424 result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
428 result += f (start_x, 0, end_x, end_y);
429 result += f (start_x, 0, end_x, -start_y);
432 else if (end_x >= width)
436 result += f (start_x, start_y, width-1, height-1);
437 result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
438 result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
439 result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
443 result += f (start_x, start_y, width-1, end_y);
444 result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
447 else if (end_y >= height)
449 result += f (start_x, start_y, end_x, height-1);
450 result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
454 result += f (start_x, start_y, end_x, end_y);
459template <
typename Po
intInT,
typename Po
intOutT>
void
461 const int pos_x,
const int pos_y,
const unsigned point_index, PointOutT &normal)
463 float bad_point = std::numeric_limits<float>::quiet_NaN ();
465 const int width = input_->width;
466 const int height = input_->height;
469 if (normal_estimation_method_ == COVARIANCE_MATRIX)
471 if (!init_covariance_matrix_)
472 initCovarianceMatrixMethod ();
474 const int start_x = pos_x - rect_width_2_;
475 const int start_y = pos_y - rect_height_2_;
476 const int end_x = start_x + rect_width_;
477 const int end_y = start_y + rect_height_;
480 auto cb_xyz_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
481 sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
486 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
490 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
491 Eigen::Vector3f center;
509 auto cb_xyz_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.
getFirstOrderSumSE (p1, p2, p3, p4); };
510 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
511 auto cb_xyz_sosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
512 sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
514 center[0] = float (tmp_center[0]);
515 center[1] = float (tmp_center[1]);
516 center[2] = float (tmp_center[2]);
518 covariance_matrix.coeffRef (0) =
static_cast<float> (so_elements [0]);
519 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) =
static_cast<float> (so_elements [1]);
520 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) =
static_cast<float> (so_elements [2]);
521 covariance_matrix.coeffRef (4) =
static_cast<float> (so_elements [3]);
522 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) =
static_cast<float> (so_elements [4]);
523 covariance_matrix.coeffRef (8) =
static_cast<float> (so_elements [5]);
524 covariance_matrix -= (center * center.transpose ()) /
static_cast<float> (count);
526 Eigen::Vector3f eigen_vector;
527 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
529 normal.getNormalVector3fMap () = eigen_vector;
532 if (eigen_value > 0.0)
533 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
535 normal.curvature = 0;
540 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
542 if (!init_average_3d_gradient_)
543 initAverage3DGradientMethod ();
545 const int start_x = pos_x - rect_width_2_;
546 const int start_y = pos_y - rect_height_2_;
547 const int end_x = start_x + rect_width_;
548 const int end_y = start_y + rect_height_;
550 unsigned count_x = 0;
551 unsigned count_y = 0;
553 auto cb_dx_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
554 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
555 auto cb_dy_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
556 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
559 if (count_x == 0 || count_y == 0)
561 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
564 Eigen::Vector3d gradient_x (0, 0, 0);
565 Eigen::Vector3d gradient_y (0, 0, 0);
567 auto cb_dx_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
568 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
569 auto cb_dy_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
570 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
573 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
574 double normal_length = normal_vector.squaredNorm ();
575 if (normal_length == 0.0f)
577 normal.getNormalVector3fMap ().setConstant (bad_point);
578 normal.curvature = bad_point;
582 normal_vector /= sqrt (normal_length);
584 float nx =
static_cast<float> (normal_vector [0]);
585 float ny =
static_cast<float> (normal_vector [1]);
586 float nz =
static_cast<float> (normal_vector [2]);
590 normal.normal_x = nx;
591 normal.normal_y = ny;
592 normal.normal_z = nz;
593 normal.curvature = bad_point;
597 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
599 if (!init_depth_change_)
600 initAverageDepthChangeMethod ();
602 int point_index_L_x = pos_x - rect_width_4_ - 1;
603 int point_index_L_y = pos_y;
604 int point_index_R_x = pos_x + rect_width_4_ + 1;
605 int point_index_R_y = pos_y;
606 int point_index_U_x = pos_x - 1;
607 int point_index_U_y = pos_y - rect_height_4_;
608 int point_index_D_x = pos_x + 1;
609 int point_index_D_y = pos_y + rect_height_4_;
611 if (point_index_L_x < 0)
612 point_index_L_x = -point_index_L_x;
613 if (point_index_U_x < 0)
614 point_index_U_x = -point_index_U_x;
615 if (point_index_U_y < 0)
616 point_index_U_y = -point_index_U_y;
618 if (point_index_R_x >= width)
619 point_index_R_x = width-(point_index_R_x-(width-1));
620 if (point_index_D_x >= width)
621 point_index_D_x = width-(point_index_D_x-(width-1));
622 if (point_index_D_y >= height)
623 point_index_D_y = height-(point_index_D_y-(height-1));
625 const int start_x_L = pos_x - rect_width_2_;
626 const int start_y_L = pos_y - rect_height_4_;
627 const int end_x_L = start_x_L + rect_width_2_;
628 const int end_y_L = start_y_L + rect_height_2_;
630 const int start_x_R = pos_x + 1;
631 const int start_y_R = pos_y - rect_height_4_;
632 const int end_x_R = start_x_R + rect_width_2_;
633 const int end_y_R = start_y_R + rect_height_2_;
635 const int start_x_U = pos_x - rect_width_4_;
636 const int start_y_U = pos_y - rect_height_2_;
637 const int end_x_U = start_x_U + rect_width_2_;
638 const int end_y_U = start_y_U + rect_height_2_;
640 const int start_x_D = pos_x - rect_width_4_;
641 const int start_y_D = pos_y + 1;
642 const int end_x_D = start_x_D + rect_width_2_;
643 const int end_y_D = start_y_D + rect_height_2_;
645 unsigned count_L_z = 0;
646 unsigned count_R_z = 0;
647 unsigned count_U_z = 0;
648 unsigned count_D_z = 0;
650 auto cb_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
651 sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
652 sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
653 sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
654 sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
656 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
658 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
667 auto cb_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
668 sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
669 sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
670 sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
671 sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
673 mean_L_z /= float (count_L_z);
674 mean_R_z /= float (count_R_z);
675 mean_U_z /= float (count_U_z);
676 mean_D_z /= float (count_D_z);
679 PointInT pointL = (*input_)[point_index_L_y*width + point_index_L_x];
680 PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
681 PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
682 PointInT pointD = (*input_)[point_index_D_y*width + point_index_D_x];
684 const float mean_x_z = mean_R_z - mean_L_z;
685 const float mean_y_z = mean_D_z - mean_U_z;
687 const float mean_x_x = pointR.x - pointL.x;
688 const float mean_x_y = pointR.y - pointL.y;
689 const float mean_y_x = pointD.x - pointU.x;
690 const float mean_y_y = pointD.y - pointU.y;
692 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
693 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
694 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
696 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
698 if (normal_length == 0.0f)
700 normal.getNormalVector3fMap ().setConstant (bad_point);
701 normal.curvature = bad_point;
707 const float scale = 1.0f / std::sqrt (normal_length);
709 normal.normal_x = normal_x * scale;
710 normal.normal_y = normal_y * scale;
711 normal.normal_z = normal_z * scale;
712 normal.curvature = bad_point;
717 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
719 PCL_THROW_EXCEPTION (
PCLException,
"BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
722 normal.getNormalVector3fMap ().setConstant (bad_point);
723 normal.curvature = bad_point;
728template <
typename Po
intInT,
typename Po
intOutT>
void
734 float bad_point = std::numeric_limits<float>::quiet_NaN ();
737 auto depthChangeMap =
new unsigned char[input_->size ()];
738 std::fill_n(depthChangeMap, input_->size(), 255);
741 for (
unsigned int ri = 0; ri < input_->height-1; ++ri)
743 for (
unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
745 index = ri * input_->width + ci;
747 const float depth = input_->points [index].z;
748 const float depthR = input_->points [index + 1].z;
749 const float depthD = input_->points [index + input_->width].z;
752 const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
754 if (std::fabs (depth - depthR) > depthDependendDepthChange
755 || !std::isfinite (depth) || !std::isfinite (depthR))
757 depthChangeMap[index] = 0;
758 depthChangeMap[index+1] = 0;
760 if (std::fabs (depth - depthD) > depthDependendDepthChange
761 || !std::isfinite (depth) || !std::isfinite (depthD))
763 depthChangeMap[index] = 0;
764 depthChangeMap[index + input_->width] = 0;
771 delete[] distance_map_;
772 distance_map_ =
new float[input_->size ()];
773 float *distanceMap = distance_map_;
774 for (std::size_t index = 0; index < input_->size (); ++index)
776 if (depthChangeMap[index] == 0)
777 distanceMap[index] = 0.0f;
779 distanceMap[index] =
static_cast<float> (input_->width + input_->height);
783 float* previous_row = distanceMap;
784 float* current_row = previous_row + input_->width;
785 for (std::size_t ri = 1; ri < input_->height; ++ri)
787 for (std::size_t ci = 1; ci < input_->width; ++ci)
789 const float upLeft = previous_row [ci - 1] + 1.4f;
790 const float up = previous_row [ci] + 1.0f;
791 const float upRight = previous_row [ci + 1] + 1.4f;
792 const float left = current_row [ci - 1] + 1.0f;
793 const float center = current_row [ci];
795 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
797 if (minValue < center)
798 current_row [ci] = minValue;
800 previous_row = current_row;
801 current_row += input_->width;
804 float* next_row = distanceMap + input_->width * (input_->height - 1);
805 current_row = next_row - input_->width;
807 for (
int ri = input_->height-2; ri >= 0; --ri)
809 for (
int ci = input_->width-2; ci >= 0; --ci)
811 const float lowerLeft = next_row [ci - 1] + 1.4f;
812 const float lower = next_row [ci] + 1.0f;
813 const float lowerRight = next_row [ci + 1] + 1.4f;
814 const float right = current_row [ci + 1] + 1.0f;
815 const float center = current_row [ci];
817 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
819 if (minValue < center)
820 current_row [ci] = minValue;
822 next_row = current_row;
823 current_row -= input_->width;
826 if (indices_->size () < input_->size ())
827 computeFeaturePart (distanceMap, bad_point, output);
829 computeFeatureFull (distanceMap, bad_point, output);
831 delete[] depthChangeMap;
835template <
typename Po
intInT,
typename Po
intOutT>
void
837 const float &bad_point,
842 if (border_policy_ == BORDER_POLICY_IGNORE)
848 unsigned border = int(normal_smoothing_size_);
849 PointOutT* vec1 = &output [0];
850 PointOutT* vec2 = vec1 + input_->
width * (input_->height - border);
852 std::size_t count = border * input_->width;
853 for (std::size_t idx = 0; idx < count; ++idx)
855 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
856 vec1 [idx].curvature = bad_point;
857 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
858 vec2 [idx].curvature = bad_point;
862 vec1 = &output [border * input_->width];
863 vec2 = vec1 + input_->
width - border;
864 for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
866 for (std::size_t ci = 0; ci < border; ++ci)
868 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
869 vec1 [ci].curvature = bad_point;
870 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
871 vec2 [ci].curvature = bad_point;
875 if (use_depth_dependent_smoothing_)
877 index = border + input_->width * border;
878 unsigned skip = (border << 1);
879 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
881 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
883 index = ri * input_->width + ci;
885 const float depth = (*input_)[index].z;
886 if (!std::isfinite (depth))
888 output[index].getNormalVector3fMap ().setConstant (bad_point);
889 output[index].curvature = bad_point;
893 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
895 if (smoothing > 2.0f)
897 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
902 output[index].getNormalVector3fMap ().setConstant (bad_point);
903 output[index].curvature = bad_point;
910 float smoothing_constant = normal_smoothing_size_;
912 index = border + input_->
width * border;
913 unsigned skip = (border << 1);
914 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
916 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
918 index = ri * input_->width + ci;
920 if (!std::isfinite ((*input_)[index].z))
922 output [index].getNormalVector3fMap ().setConstant (bad_point);
923 output [index].curvature = bad_point;
927 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
929 if (smoothing > 2.0f)
931 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
936 output [index].getNormalVector3fMap ().setConstant (bad_point);
937 output [index].curvature = bad_point;
943 else if (border_policy_ == BORDER_POLICY_MIRROR)
947 if (use_depth_dependent_smoothing_)
952 for (
unsigned ri = 0; ri < input_->height; ++ri)
955 for (
unsigned ci = 0; ci < input_->width; ++ci)
957 index = ri * input_->width + ci;
959 const float depth = (*input_)[index].z;
960 if (!std::isfinite (depth))
962 output[index].getNormalVector3fMap ().setConstant (bad_point);
963 output[index].curvature = bad_point;
967 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
969 if (smoothing > 2.0f)
971 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
972 computePointNormalMirror (ci, ri, index, output [index]);
976 output[index].getNormalVector3fMap ().setConstant (bad_point);
977 output[index].curvature = bad_point;
984 float smoothing_constant = normal_smoothing_size_;
989 for (
unsigned ri = 0; ri < input_->height; ++ri)
992 for (
unsigned ci = 0; ci < input_->width; ++ci)
994 index = ri * input_->
width + ci;
996 if (!std::isfinite ((*input_)[index].z))
998 output [index].getNormalVector3fMap ().setConstant (bad_point);
999 output [index].curvature = bad_point;
1003 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
1005 if (smoothing > 2.0f)
1007 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1008 computePointNormalMirror (ci, ri, index, output [index]);
1012 output [index].getNormalVector3fMap ().setConstant (bad_point);
1013 output [index].curvature = bad_point;
1022template <
typename Po
intInT,
typename Po
intOutT>
void
1024 const float &bad_point,
1027 if (border_policy_ == BORDER_POLICY_IGNORE)
1030 unsigned border = int(normal_smoothing_size_);
1031 unsigned bottom = input_->height > border ? input_->height - border : 0;
1032 unsigned right = input_->width > border ? input_->width - border : 0;
1033 if (use_depth_dependent_smoothing_)
1036 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1038 unsigned pt_index = (*indices_)[idx];
1039 unsigned u = pt_index % input_->width;
1040 unsigned v = pt_index / input_->width;
1041 if (v < border || v > bottom)
1043 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1044 output[idx].curvature = bad_point;
1048 if (u < border || u > right)
1050 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1051 output[idx].curvature = bad_point;
1055 const float depth = (*input_)[pt_index].z;
1056 if (!std::isfinite (depth))
1058 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1059 output[idx].curvature = bad_point;
1063 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
1064 if (smoothing > 2.0f)
1066 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1071 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1072 output[idx].curvature = bad_point;
1078 float smoothing_constant = normal_smoothing_size_;
1080 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1082 unsigned pt_index = (*indices_)[idx];
1083 unsigned u = pt_index % input_->
width;
1084 unsigned v = pt_index / input_->width;
1085 if (v < border || v > bottom)
1087 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1088 output[idx].curvature = bad_point;
1092 if (u < border || u > right)
1094 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1095 output[idx].curvature = bad_point;
1099 if (!std::isfinite ((*input_)[pt_index].z))
1101 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1102 output [idx].curvature = bad_point;
1106 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1108 if (smoothing > 2.0f)
1110 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1115 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1116 output [idx].curvature = bad_point;
1121 else if (border_policy_ == BORDER_POLICY_MIRROR)
1125 if (use_depth_dependent_smoothing_)
1127 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1129 unsigned pt_index = (*indices_)[idx];
1130 unsigned u = pt_index % input_->width;
1131 unsigned v = pt_index / input_->width;
1133 const float depth = (*input_)[pt_index].z;
1134 if (!std::isfinite (depth))
1136 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1137 output[idx].curvature = bad_point;
1141 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
1143 if (smoothing > 2.0f)
1145 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1146 computePointNormalMirror (u, v, pt_index, output [idx]);
1150 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1151 output[idx].curvature = bad_point;
1157 float smoothing_constant = normal_smoothing_size_;
1158 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1160 unsigned pt_index = (*indices_)[idx];
1161 unsigned u = pt_index % input_->
width;
1162 unsigned v = pt_index / input_->width;
1164 if (!std::isfinite ((*input_)[pt_index].z))
1166 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1167 output [idx].curvature = bad_point;
1171 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1173 if (smoothing > 2.0f)
1175 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1176 computePointNormalMirror (u, v, pt_index, output [idx]);
1180 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1181 output [idx].curvature = bad_point;
1189template <
typename Po
intInT,
typename Po
intOutT>
bool
1192 if (!input_->isOrganized ())
1194 PCL_ERROR (
"[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1197 return (Feature<PointInT, PointOutT>::initCompute ());
1200#define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Determines an integral image representation for a given organized data array.
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Surface normal estimation on organized data using integral images.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
~IntegralImageNormalEstimation() override
Destructor.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
void computePointNormalMirror(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position with mirroring for border handling.
A base class for all pcl exceptions which inherits from std::runtime_error.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::uint32_t width
The point cloud width (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
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...
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
bool computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...