39 #ifndef PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
40 #define PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
42 #include <pcl/features/integral_image_normal.h>
45 template <
typename Po
intInT,
typename Po
intOutT>
51 delete[] distance_map_;
55 template <
typename Po
intInT,
typename Po
intOutT>
void
58 if (border_policy_ != BORDER_POLICY_IGNORE &&
59 border_policy_ != BORDER_POLICY_MIRROR)
61 "[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
63 if (normal_estimation_method_ != COVARIANCE_MATRIX &&
64 normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
65 normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
66 normal_estimation_method_ != SIMPLE_3D_GRADIENT)
68 "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
74 delete[] distance_map_;
77 depth_data_ =
nullptr;
78 distance_map_ =
nullptr;
80 if (normal_estimation_method_ == COVARIANCE_MATRIX)
81 initCovarianceMatrixMethod ();
82 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
83 initAverage3DGradientMethod ();
84 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
85 initAverageDepthChangeMethod ();
86 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
87 initSimple3DGradientMethod ();
92 template <
typename Po
intInT,
typename Po
intOutT>
void
96 rect_width_2_ = width/2;
97 rect_width_4_ = width/4;
98 rect_height_ = height;
99 rect_height_2_ = height/2;
100 rect_height_4_ = height/4;
104 template <
typename Po
intInT,
typename Po
intOutT>
void
108 int element_stride =
sizeof (PointInT) /
sizeof (
float);
110 int row_stride = element_stride * input_->width;
112 const float *data_ =
reinterpret_cast<const float*
> (&(*input_)[0]);
114 integral_image_XYZ_.setSecondOrderComputation (
false);
115 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
117 init_simple_3d_gradient_ =
true;
118 init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ =
false;
122 template <
typename Po
intInT,
typename Po
intOutT>
void
126 int element_stride =
sizeof (PointInT) /
sizeof (
float);
128 int row_stride = element_stride * input_->width;
130 const float *data_ =
reinterpret_cast<const float*
> (&(*input_)[0]);
132 integral_image_XYZ_.setSecondOrderComputation (
true);
133 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
135 init_covariance_matrix_ =
true;
136 init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ =
false;
140 template <
typename Po
intInT,
typename Po
intOutT>
void
143 std::size_t data_size = (input_->size () << 2);
144 diff_x_ =
new float[data_size];
145 diff_y_ =
new float[data_size];
147 memset (diff_x_, 0,
sizeof(
float) * data_size);
148 memset (diff_y_, 0,
sizeof(
float) * data_size);
153 const PointInT* point_up = &(input_->points [1]);
154 const PointInT* point_dn = point_up + (input_->width << 1);
155 const PointInT* point_lf = &(input_->points [input_->width]);
156 const PointInT* point_rg = point_lf + 2;
157 float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
158 float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
159 unsigned diff_skip = 8;
161 for (std::size_t ri = 1; ri < input_->height - 1; ++ri
162 , point_up += input_->width
163 , point_dn += input_->width
164 , point_lf += input_->width
165 , point_rg += input_->width
166 , diff_x_ptr += diff_skip
167 , diff_y_ptr += diff_skip)
169 for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
171 diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
172 diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
173 diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
175 diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
176 diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
177 diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
182 integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
183 integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
184 init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ =
false;
185 init_average_3d_gradient_ =
true;
189 template <
typename Po
intInT,
typename Po
intOutT>
void
193 int element_stride =
sizeof (PointInT) /
sizeof (
float);
195 int row_stride = element_stride * input_->width;
197 const float *data_ =
reinterpret_cast<const float*
> (&(*input_)[0]);
200 integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
201 init_depth_change_ =
true;
202 init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ =
false;
206 template <
typename Po
intInT,
typename Po
intOutT>
void
208 const int pos_x,
const int pos_y,
const unsigned point_index, PointOutT &normal)
210 float bad_point = std::numeric_limits<float>::quiet_NaN ();
212 if (normal_estimation_method_ == COVARIANCE_MATRIX)
214 if (!init_covariance_matrix_)
215 initCovarianceMatrixMethod ();
217 unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
222 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
226 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
227 Eigen::Vector3f center;
229 center = integral_image_XYZ_.
getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
230 so_elements = integral_image_XYZ_.
getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
232 covariance_matrix.coeffRef (0) =
static_cast<float> (so_elements [0]);
233 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) =
static_cast<float> (so_elements [1]);
234 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) =
static_cast<float> (so_elements [2]);
235 covariance_matrix.coeffRef (4) =
static_cast<float> (so_elements [3]);
236 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) =
static_cast<float> (so_elements [4]);
237 covariance_matrix.coeffRef (8) =
static_cast<float> (so_elements [5]);
238 covariance_matrix -= (center * center.transpose ()) /
static_cast<float> (count);
240 Eigen::Vector3f eigen_vector;
241 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
243 normal.getNormalVector3fMap () = eigen_vector;
246 if (eigen_value > 0.0)
247 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
249 normal.curvature = 0;
253 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
255 if (!init_average_3d_gradient_)
256 initAverage3DGradientMethod ();
258 unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
259 unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
260 if (count_x == 0 || count_y == 0)
262 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
265 Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
266 Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
268 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
269 double normal_length = normal_vector.squaredNorm ();
270 if (normal_length == 0.0f)
272 normal.getNormalVector3fMap ().setConstant (bad_point);
273 normal.curvature = bad_point;
277 normal_vector /= sqrt (normal_length);
279 float nx =
static_cast<float> (normal_vector [0]);
280 float ny =
static_cast<float> (normal_vector [1]);
281 float nz =
static_cast<float> (normal_vector [2]);
285 normal.normal_x = nx;
286 normal.normal_y = ny;
287 normal.normal_z = nz;
288 normal.curvature = bad_point;
291 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
293 if (!init_depth_change_)
294 initAverageDepthChangeMethod ();
297 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
298 unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
299 unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
300 unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
302 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
304 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
308 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);
309 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);
310 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);
311 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);
313 PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
314 PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
315 PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
316 PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
318 const float mean_x_z = mean_R_z - mean_L_z;
319 const float mean_y_z = mean_D_z - mean_U_z;
321 const float mean_x_x = pointR.x - pointL.x;
322 const float mean_x_y = pointR.y - pointL.y;
323 const float mean_y_x = pointD.x - pointU.x;
324 const float mean_y_y = pointD.y - pointU.y;
326 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
327 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
328 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
330 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
332 if (normal_length == 0.0f)
334 normal.getNormalVector3fMap ().setConstant (bad_point);
335 normal.curvature = bad_point;
341 const float scale = 1.0f / std::sqrt (normal_length);
343 normal.normal_x = normal_x * scale;
344 normal.normal_y = normal_y * scale;
345 normal.normal_z = normal_z * scale;
346 normal.curvature = bad_point;
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);
362 double normal_length = normal_vector.squaredNorm ();
363 if (normal_length == 0.0f)
365 normal.getNormalVector3fMap ().setConstant (bad_point);
366 normal.curvature = 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;
391 template <
typename T>
393 sumArea (
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);
461 template <
typename Po
intInT,
typename Po
intOutT>
void
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;
730 template <
typename Po
intInT,
typename Po
intOutT>
void
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;
837 template <
typename Po
intInT,
typename Po
intOutT>
void
839 const float &bad_point,
844 if (border_policy_ == BORDER_POLICY_IGNORE)
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)
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;
1024 template <
typename Po
intInT,
typename Po
intOutT>
void
1026 const float &bad_point,
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;
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)
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;
1191 template <
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");
1199 return (Feature<PointInT, PointOutT>::initCompute ());
1202 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;