463 const int pos_x,
const int pos_y,
const unsigned point_index, PointOutT &normal)
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 ();
476 const int start_x = pos_x - rect_width_2_;
477 const int start_y = pos_y - rect_height_2_;
478 const int end_x = start_x + rect_width_;
479 const int end_y = start_y + rect_height_;
482 auto cb_xyz_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
483 sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
488 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
492 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
493 Eigen::Vector3f center;
511 auto cb_xyz_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.
getFirstOrderSumSE (p1, p2, p3, p4); };
512 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
513 auto cb_xyz_sosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
514 sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
516 center[0] = float (tmp_center[0]);
517 center[1] = float (tmp_center[1]);
518 center[2] = float (tmp_center[2]);
520 covariance_matrix.coeffRef (0) =
static_cast<float> (so_elements [0]);
521 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) =
static_cast<float> (so_elements [1]);
522 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) =
static_cast<float> (so_elements [2]);
523 covariance_matrix.coeffRef (4) =
static_cast<float> (so_elements [3]);
524 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) =
static_cast<float> (so_elements [4]);
525 covariance_matrix.coeffRef (8) =
static_cast<float> (so_elements [5]);
526 covariance_matrix -= (center * center.transpose ()) /
static_cast<float> (count);
528 Eigen::Vector3f eigen_vector;
529 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
531 normal.getNormalVector3fMap () = eigen_vector;
534 if (eigen_value > 0.0)
535 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
537 normal.curvature = 0;
542 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
544 if (!init_average_3d_gradient_)
545 initAverage3DGradientMethod ();
547 const int start_x = pos_x - rect_width_2_;
548 const int start_y = pos_y - rect_height_2_;
549 const int end_x = start_x + rect_width_;
550 const int end_y = start_y + rect_height_;
552 unsigned count_x = 0;
553 unsigned count_y = 0;
555 auto cb_dx_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
556 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
557 auto cb_dy_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
558 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
561 if (count_x == 0 || count_y == 0)
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); };
570 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
571 auto cb_dy_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
572 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
575 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
576 double normal_length = normal_vector.squaredNorm ();
577 if (normal_length == 0.0f)
579 normal.getNormalVector3fMap ().setConstant (bad_point);
580 normal.curvature = bad_point;
584 normal_vector /= sqrt (normal_length);
586 float nx =
static_cast<float> (normal_vector [0]);
587 float ny =
static_cast<float> (normal_vector [1]);
588 float nz =
static_cast<float> (normal_vector [2]);
592 normal.normal_x = nx;
593 normal.normal_y = ny;
594 normal.normal_z = nz;
595 normal.curvature = bad_point;
599 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
601 if (!init_depth_change_)
602 initAverageDepthChangeMethod ();
604 int point_index_L_x = pos_x - rect_width_4_ - 1;
605 int point_index_L_y = pos_y;
606 int point_index_R_x = pos_x + rect_width_4_ + 1;
607 int point_index_R_y = pos_y;
608 int point_index_U_x = pos_x - 1;
609 int point_index_U_y = pos_y - rect_height_4_;
610 int point_index_D_x = pos_x + 1;
611 int point_index_D_y = pos_y + rect_height_4_;
613 if (point_index_L_x < 0)
614 point_index_L_x = -point_index_L_x;
615 if (point_index_U_x < 0)
616 point_index_U_x = -point_index_U_x;
617 if (point_index_U_y < 0)
618 point_index_U_y = -point_index_U_y;
620 if (point_index_R_x >= width)
621 point_index_R_x = width-(point_index_R_x-(width-1));
622 if (point_index_D_x >= width)
623 point_index_D_x = width-(point_index_D_x-(width-1));
624 if (point_index_D_y >= height)
625 point_index_D_y = height-(point_index_D_y-(height-1));
627 const int start_x_L = pos_x - rect_width_2_;
628 const int start_y_L = pos_y - rect_height_4_;
629 const int end_x_L = start_x_L + rect_width_2_;
630 const int end_y_L = start_y_L + rect_height_2_;
632 const int start_x_R = pos_x + 1;
633 const int start_y_R = pos_y - rect_height_4_;
634 const int end_x_R = start_x_R + rect_width_2_;
635 const int end_y_R = start_y_R + rect_height_2_;
637 const int start_x_U = pos_x - rect_width_4_;
638 const int start_y_U = pos_y - rect_height_2_;
639 const int end_x_U = start_x_U + rect_width_2_;
640 const int end_y_U = start_y_U + rect_height_2_;
642 const int start_x_D = pos_x - rect_width_4_;
643 const int start_y_D = pos_y + 1;
644 const int end_x_D = start_x_D + rect_width_2_;
645 const int end_y_D = start_y_D + rect_height_2_;
647 unsigned count_L_z = 0;
648 unsigned count_R_z = 0;
649 unsigned count_U_z = 0;
650 unsigned count_D_z = 0;
652 auto cb_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
653 sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
654 sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
655 sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
656 sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
658 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
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); };
670 sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
671 sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
672 sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
673 sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
675 mean_L_z /= float (count_L_z);
676 mean_R_z /= float (count_R_z);
677 mean_U_z /= float (count_U_z);
678 mean_D_z /= float (count_D_z);
681 PointInT pointL = (*input_)[point_index_L_y*width + point_index_L_x];
682 PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
683 PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
684 PointInT pointD = (*input_)[point_index_D_y*width + point_index_D_x];
686 const float mean_x_z = mean_R_z - mean_L_z;
687 const float mean_y_z = mean_D_z - mean_U_z;
689 const float mean_x_x = pointR.x - pointL.x;
690 const float mean_x_y = pointR.y - pointL.y;
691 const float mean_y_x = pointD.x - pointU.x;
692 const float mean_y_y = pointD.y - pointU.y;
694 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
695 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
696 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
698 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
700 if (normal_length == 0.0f)
702 normal.getNormalVector3fMap ().setConstant (bad_point);
703 normal.curvature = bad_point;
709 const float scale = 1.0f / std::sqrt (normal_length);
711 normal.normal_x = normal_x * scale;
712 normal.normal_y = normal_y * scale;
713 normal.normal_z = normal_z * scale;
714 normal.curvature = bad_point;
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);
725 normal.curvature = bad_point;
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 ()];
740 memset (depthChangeMap, 255, 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;
754 const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
756 if (std::fabs (depth - depthR) > depthDependendDepthChange
757 || !std::isfinite (depth) || !std::isfinite (depthR))
759 depthChangeMap[index] = 0;
760 depthChangeMap[index+1] = 0;
762 if (std::fabs (depth - depthD) > depthDependendDepthChange
763 || !std::isfinite (depth) || !std::isfinite (depthD))
765 depthChangeMap[index] = 0;
766 depthChangeMap[index + input_->width] = 0;
773 delete[] distance_map_;
774 distance_map_ =
new float[input_->size ()];
775 float *distanceMap = distance_map_;
776 for (std::size_t index = 0; index < input_->size (); ++index)
778 if (depthChangeMap[index] == 0)
779 distanceMap[index] = 0.0f;
781 distanceMap[index] =
static_cast<float> (input_->width + input_->height);
785 float* previous_row = distanceMap;
786 float* current_row = previous_row + input_->width;
787 for (std::size_t ri = 1; ri < input_->height; ++ri)
789 for (std::size_t ci = 1; ci < input_->width; ++ci)
791 const float upLeft = previous_row [ci - 1] + 1.4f;
792 const float up = previous_row [ci] + 1.0f;
793 const float upRight = previous_row [ci + 1] + 1.4f;
794 const float left = current_row [ci - 1] + 1.0f;
795 const float center = current_row [ci];
797 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
799 if (minValue < center)
800 current_row [ci] = minValue;
802 previous_row = current_row;
803 current_row += input_->width;
806 float* next_row = distanceMap + input_->width * (input_->height - 1);
807 current_row = next_row - input_->width;
809 for (
int ri = input_->height-2; ri >= 0; --ri)
811 for (
int ci = input_->width-2; ci >= 0; --ci)
813 const float lowerLeft = next_row [ci - 1] + 1.4f;
814 const float lower = next_row [ci] + 1.0f;
815 const float lowerRight = next_row [ci + 1] + 1.4f;
816 const float right = current_row [ci + 1] + 1.0f;
817 const float center = current_row [ci];
819 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
821 if (minValue < center)
822 current_row [ci] = minValue;
824 next_row = current_row;
825 current_row -= input_->width;
828 if (indices_->size () < input_->size ())
829 computeFeaturePart (distanceMap, bad_point, output);
831 computeFeatureFull (distanceMap, bad_point, output);
833 delete[] depthChangeMap;
839 const float &bad_point,
844 if (border_policy_ == BORDER_POLICY_IGNORE)
849 output.is_dense =
false;
850 unsigned border = int(normal_smoothing_size_);
851 PointOutT* vec1 = &output [0];
852 PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
854 std::size_t count = border * input_->width;
855 for (std::size_t idx = 0; idx < count; ++idx)
857 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
858 vec1 [idx].curvature = bad_point;
859 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
860 vec2 [idx].curvature = bad_point;
864 vec1 = &output [border * input_->width];
865 vec2 = vec1 + input_->width - border;
866 for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
868 for (std::size_t ci = 0; ci < border; ++ci)
870 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
871 vec1 [ci].curvature = bad_point;
872 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
873 vec2 [ci].curvature = bad_point;
877 if (use_depth_dependent_smoothing_)
879 index = border + input_->width * border;
880 unsigned skip = (border << 1);
881 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
883 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
885 index = ri * input_->width + ci;
887 const float depth = (*input_)[index].z;
888 if (!std::isfinite (depth))
890 output[index].getNormalVector3fMap ().setConstant (bad_point);
891 output[index].curvature = bad_point;
895 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
897 if (smoothing > 2.0f)
899 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
904 output[index].getNormalVector3fMap ().setConstant (bad_point);
905 output[index].curvature = bad_point;
912 float smoothing_constant = normal_smoothing_size_;
914 index = border + input_->width * border;
915 unsigned skip = (border << 1);
916 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
918 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
920 index = ri * input_->width + ci;
922 if (!std::isfinite ((*input_)[index].z))
924 output [index].getNormalVector3fMap ().setConstant (bad_point);
925 output [index].curvature = bad_point;
929 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
931 if (smoothing > 2.0f)
933 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
938 output [index].getNormalVector3fMap ().setConstant (bad_point);
939 output [index].curvature = bad_point;
945 else if (border_policy_ == BORDER_POLICY_MIRROR)
947 output.is_dense =
false;
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))
964 output[index].getNormalVector3fMap ().setConstant (bad_point);
965 output[index].curvature = bad_point;
969 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
971 if (smoothing > 2.0f)
973 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
974 computePointNormalMirror (ci, ri, index, output [index]);
978 output[index].getNormalVector3fMap ().setConstant (bad_point);
979 output[index].curvature = bad_point;
986 float smoothing_constant = normal_smoothing_size_;
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))
1000 output [index].getNormalVector3fMap ().setConstant (bad_point);
1001 output [index].curvature = bad_point;
1005 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
1007 if (smoothing > 2.0f)
1009 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1010 computePointNormalMirror (ci, ri, index, output [index]);
1014 output [index].getNormalVector3fMap ().setConstant (bad_point);
1015 output [index].curvature = bad_point;
1026 const float &bad_point,
1029 if (border_policy_ == BORDER_POLICY_IGNORE)
1031 output.is_dense =
false;
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;
1043 if (v < border || v > bottom)
1045 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1046 output[idx].curvature = bad_point;
1050 if (u < border || u > right)
1052 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1053 output[idx].curvature = bad_point;
1057 const float depth = (*input_)[pt_index].z;
1058 if (!std::isfinite (depth))
1060 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1061 output[idx].curvature = bad_point;
1065 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
1066 if (smoothing > 2.0f)
1068 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1073 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1074 output[idx].curvature = bad_point;
1080 float smoothing_constant = normal_smoothing_size_;
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;
1087 if (v < border || v > bottom)
1089 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1090 output[idx].curvature = bad_point;
1094 if (u < border || u > right)
1096 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1097 output[idx].curvature = bad_point;
1101 if (!std::isfinite ((*input_)[pt_index].z))
1103 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1104 output [idx].curvature = bad_point;
1108 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1110 if (smoothing > 2.0f)
1112 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1117 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1118 output [idx].curvature = bad_point;
1123 else if (border_policy_ == BORDER_POLICY_MIRROR)
1125 output.is_dense =
false;
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))
1138 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1139 output[idx].curvature = bad_point;
1143 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
1145 if (smoothing > 2.0f)
1147 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1148 computePointNormalMirror (u, v, pt_index, output [idx]);
1152 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1153 output[idx].curvature = bad_point;
1159 float smoothing_constant = normal_smoothing_size_;
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))
1168 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1169 output [idx].curvature = bad_point;
1173 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1175 if (smoothing > 2.0f)
1177 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1178 computePointNormalMirror (u, v, pt_index, output [idx]);
1182 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1183 output [idx].curvature = bad_point;