343 normal.normal_x = normal_x * scale;
344 normal.normal_y = normal_y * scale;
345 normal.normal_z = normal_z * scale;
350 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
352 if (!init_simple_3d_gradient_)
353 initSimple3DGradientMethod ();
356 Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (
pos_x + rect_width_2_,
pos_y - rect_height_2_, 1, rect_height_) -
357 integral_image_XYZ_.getFirstOrderSum (
pos_x - rect_width_2_,
pos_y - rect_height_2_, 1, rect_height_);
359 Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (
pos_x - rect_width_2_,
pos_y + rect_height_2_, rect_width_, 1) -
360 integral_image_XYZ_.getFirstOrderSum (
pos_x - rect_width_2_,
pos_y - rect_height_2_, rect_width_, 1);
361 Eigen::Vector3d
normal_vector = gradient_y.cross (gradient_x);
365 normal.getNormalVector3fMap ().setConstant (
bad_point);
370 normal_vector /= sqrt (normal_length);
372 float nx =
static_cast<float> (normal_vector [0]);
373 float ny =
static_cast<float> (normal_vector [1]);
374 float nz =
static_cast<float> (normal_vector [2]);
378 normal.normal_x = nx;
379 normal.normal_y = ny;
380 normal.normal_z = nz;
381 normal.curvature = bad_point;
385 normal.getNormalVector3fMap ().setConstant (bad_point);
386 normal.curvature = bad_point;
393sumArea (
int start_x,
int start_y,
int end_x,
int end_y,
const int width,
const int height,
394 const std::function<T(
unsigned,
unsigned,
unsigned,
unsigned)> &f,
401 result += f (0, 0, end_x, end_y);
402 result += f (0, 0, -start_x, -start_y);
403 result += f (0, 0, -start_x, end_y);
404 result += f (0, 0, end_x, -start_y);
406 else if (end_y >= height)
408 result += f (0, start_y, end_x, height-1);
409 result += f (0, start_y, -start_x, height-1);
410 result += f (0, height-(end_y-(height-1)), end_x, height-1);
411 result += f (0, height-(end_y-(height-1)), -start_x, height-1);
415 result += f (0, start_y, end_x, end_y);
416 result += f (0, start_y, -start_x, end_y);
419 else if (start_y < 0)
423 result += f (start_x, 0, width-1, end_y);
424 result += f (start_x, 0, width-1, -start_y);
425 result += f (width-(end_x-(width-1)), 0, width-1, end_y);
426 result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
430 result += f (start_x, 0, end_x, end_y);
431 result += f (start_x, 0, end_x, -start_y);
434 else if (end_x >= width)
438 result += f (start_x, start_y, width-1, height-1);
439 result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
440 result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
441 result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
445 result += f (start_x, start_y, width-1, end_y);
446 result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
449 else if (end_y >= height)
451 result += f (start_x, start_y, end_x, height-1);
452 result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
456 result += f (start_x, start_y, end_x, end_y);
461template <
typename Po
intInT,
typename Po
intOutT>
void
465 float bad_point = std::numeric_limits<float>::quiet_NaN ();
467 const int width = input_->width;
468 const int height = input_->height;
471 if (normal_estimation_method_ == COVARIANCE_MATRIX)
473 if (!init_covariance_matrix_)
474 initCovarianceMatrixMethod ();
482 auto cb_xyz_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
488 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature =
bad_point;
511 auto cb_xyz_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getFirstOrderSumSE (p1, p2, p3, p4); };
513 auto cb_xyz_sosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
537 normal.curvature = 0;
542 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
544 if (!init_average_3d_gradient_)
545 initAverage3DGradientMethod ();
555 auto cb_dx_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
557 auto cb_dy_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
563 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature =
bad_point;
566 Eigen::Vector3d gradient_x (0, 0, 0);
567 Eigen::Vector3d gradient_y (0, 0, 0);
569 auto cb_dx_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
571 auto cb_dy_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
575 Eigen::Vector3d
normal_vector = gradient_y.cross (gradient_x);
579 normal.getNormalVector3fMap ().setConstant (
bad_point);
592 normal.normal_x =
nx;
593 normal.normal_y =
ny;
594 normal.normal_z =
nz;
599 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
601 if (!init_depth_change_)
602 initAverageDepthChangeMethod ();
652 auto cb_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
660 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature =
bad_point;
669 auto cb_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
698 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
702 normal.getNormalVector3fMap ().setConstant (
bad_point);
711 normal.normal_x = normal_x * scale;
712 normal.normal_y = normal_y * scale;
713 normal.normal_z = normal_z * scale;
719 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
721 PCL_THROW_EXCEPTION (
PCLException,
"BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
724 normal.getNormalVector3fMap ().setConstant (
bad_point);
730template <
typename Po
intInT,
typename Po
intOutT>
void
733 output.sensor_origin_ = input_->sensor_origin_;
734 output.sensor_orientation_ = input_->sensor_orientation_;
736 float bad_point = std::numeric_limits<float>::quiet_NaN ();
739 unsigned char *
depthChangeMap =
new unsigned char[input_->size ()];
743 for (
unsigned int ri = 0;
ri < input_->height-1; ++
ri)
745 for (
unsigned int ci = 0;
ci < input_->width-1; ++
ci, ++index)
747 index =
ri * input_->width +
ci;
749 const float depth = input_->points [index].z;
750 const float depthR = input_->points [index + 1].z;
751 const float depthD = input_->points [index + input_->width].z;
757 || !std::isfinite (depth) || !std::isfinite (
depthR))
763 || !std::isfinite (depth) || !std::isfinite (
depthD))
773 delete[] distance_map_;
774 distance_map_ =
new float[input_->size ()];
776 for (std::size_t index = 0; index < input_->
size (); ++index)
781 distanceMap[index] =
static_cast<float> (input_->width + input_->height);
787 for (std::size_t
ri = 1;
ri < input_->height; ++
ri)
789 for (std::size_t
ci = 1;
ci < input_->width; ++
ci)
809 for (
int ri = input_->height-2;
ri >= 0; --
ri)
811 for (
int ci = input_->width-2;
ci >= 0; --
ci)
828 if (indices_->size () < input_->size ())
837template <
typename Po
intInT,
typename Po
intOutT>
void
844 if (border_policy_ == BORDER_POLICY_IGNORE)
850 unsigned border =
int(normal_smoothing_size_);
854 std::size_t count =
border * input_->width;
855 for (std::size_t idx = 0; idx < count; ++idx)
877 if (use_depth_dependent_smoothing_)
885 index =
ri * input_->width +
ci;
887 const float depth = (*input_)[index].z;
888 if (!std::isfinite (depth))
895 float smoothing = (std::min)(
distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
920 index =
ri * input_->width +
ci;
922 if (!std::isfinite ((*input_)[index].z))
945 else if (border_policy_ == BORDER_POLICY_MIRROR)
949 if (use_depth_dependent_smoothing_)
954 for (
unsigned ri = 0;
ri < input_->height; ++
ri)
957 for (
unsigned ci = 0;
ci < input_->width; ++
ci)
959 index =
ri * input_->width +
ci;
961 const float depth = (*input_)[index].z;
962 if (!std::isfinite (depth))
969 float smoothing = (std::min)(
distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
974 computePointNormalMirror (
ci,
ri, index,
output [index]);
991 for (
unsigned ri = 0;
ri < input_->height; ++
ri)
994 for (
unsigned ci = 0;
ci < input_->width; ++
ci)
996 index =
ri * input_->width +
ci;
998 if (!std::isfinite ((*input_)[index].z))
1010 computePointNormalMirror (
ci,
ri, index,
output [index]);
1024template <
typename Po
intInT,
typename Po
intOutT>
void
1029 if (border_policy_ == BORDER_POLICY_IGNORE)
1032 unsigned border =
int(normal_smoothing_size_);
1033 unsigned bottom = input_->height >
border ? input_->height -
border : 0;
1034 unsigned right = input_->width >
border ? input_->width -
border : 0;
1035 if (use_depth_dependent_smoothing_)
1038 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1040 unsigned pt_index = (*indices_)[idx];
1041 unsigned u =
pt_index % input_->width;
1042 unsigned v =
pt_index / input_->width;
1057 const float depth = (*input_)[
pt_index].z;
1058 if (!std::isfinite (depth))
1082 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1084 unsigned pt_index = (*indices_)[idx];
1085 unsigned u =
pt_index % input_->width;
1086 unsigned v =
pt_index / input_->width;
1101 if (!std::isfinite ((*input_)[
pt_index].z))
1123 else if (border_policy_ == BORDER_POLICY_MIRROR)
1127 if (use_depth_dependent_smoothing_)
1129 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1131 unsigned pt_index = (*indices_)[idx];
1132 unsigned u =
pt_index % input_->width;
1133 unsigned v =
pt_index / input_->width;
1135 const float depth = (*input_)[
pt_index].z;
1136 if (!std::isfinite (depth))
1160 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1162 unsigned pt_index = (*indices_)[idx];
1163 unsigned u =
pt_index % input_->width;
1164 unsigned v =
pt_index / input_->width;
1166 if (!std::isfinite ((*input_)[
pt_index].z))
1191template <
typename Po
intInT,
typename Po
intOutT>
bool
1194 if (!input_->isOrganized ())
1196 PCL_ERROR (
"[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1202#define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;