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;
731 output.sensor_origin_ = input_->sensor_origin_;
732 output.sensor_orientation_ = input_->sensor_orientation_;
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;
837 const float &bad_point,
842 if (border_policy_ == BORDER_POLICY_IGNORE)
847 output.is_dense =
false;
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)
945 output.is_dense =
false;
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;
1024 const float &bad_point,
1027 if (border_policy_ == BORDER_POLICY_IGNORE)
1029 output.is_dense =
false;
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)
1123 output.is_dense =
false;
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;