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
145 std::size_t data_size = (input_->size () << 2);
146 diff_x_ =
new float[data_size];
147 diff_y_ =
new float[data_size];
149 memset (diff_x_, 0,
sizeof(
float) * data_size);
150 memset (diff_y_, 0,
sizeof(
float) * data_size);
155 const PointInT* point_up = &(input_->points [1]);
156 const PointInT* point_dn = point_up + (input_->width << 1);
157 const PointInT* point_lf = &(input_->points [input_->width]);
158 const PointInT* point_rg = point_lf + 2;
159 float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
160 float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
161 unsigned diff_skip = 8;
163 for (std::size_t ri = 1; ri < input_->height - 1; ++ri
164 , point_up += input_->width
165 , point_dn += input_->width
166 , point_lf += input_->width
167 , point_rg += input_->width
168 , diff_x_ptr += diff_skip
169 , diff_y_ptr += diff_skip)
171 for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
173 diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
174 diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
175 diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
177 diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
178 diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
179 diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
184 integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
185 integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
186 init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ =
false;
187 init_average_3d_gradient_ =
true;
191 template <
typename Po
intInT,
typename Po
intOutT>
void
195 int element_stride =
sizeof (PointInT) /
sizeof (
float);
197 int row_stride = element_stride * input_->width;
199 const float *data_ =
reinterpret_cast<const float*
> (&(*input_)[0]);
202 integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
203 init_depth_change_ =
true;
204 init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ =
false;
208 template <
typename Po
intInT,
typename Po
intOutT>
void
210 const int pos_x,
const int pos_y,
const unsigned point_index, PointOutT &normal)
212 float bad_point = std::numeric_limits<float>::quiet_NaN ();
214 if (normal_estimation_method_ == COVARIANCE_MATRIX)
216 if (!init_covariance_matrix_)
217 initCovarianceMatrixMethod ();
219 unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
224 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
228 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
229 Eigen::Vector3f center;
231 center = integral_image_XYZ_.
getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
232 so_elements = integral_image_XYZ_.
getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
234 covariance_matrix.coeffRef (0) =
static_cast<float> (so_elements [0]);
235 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) =
static_cast<float> (so_elements [1]);
236 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) =
static_cast<float> (so_elements [2]);
237 covariance_matrix.coeffRef (4) =
static_cast<float> (so_elements [3]);
238 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) =
static_cast<float> (so_elements [4]);
239 covariance_matrix.coeffRef (8) =
static_cast<float> (so_elements [5]);
240 covariance_matrix -= (center * center.transpose ()) /
static_cast<float> (count);
242 Eigen::Vector3f eigen_vector;
243 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
245 normal.getNormalVector3fMap () = eigen_vector;
248 if (eigen_value > 0.0)
249 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
251 normal.curvature = 0;
255 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
257 if (!init_average_3d_gradient_)
258 initAverage3DGradientMethod ();
260 unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
261 unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
262 if (count_x == 0 || count_y == 0)
264 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
267 Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
268 Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
270 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
271 double normal_length = normal_vector.squaredNorm ();
272 if (normal_length == 0.0f)
274 normal.getNormalVector3fMap ().setConstant (bad_point);
275 normal.curvature = bad_point;
279 normal_vector /= sqrt (normal_length);
281 float nx =
static_cast<float> (normal_vector [0]);
282 float ny =
static_cast<float> (normal_vector [1]);
283 float nz =
static_cast<float> (normal_vector [2]);
287 normal.normal_x = nx;
288 normal.normal_y = ny;
289 normal.normal_z = nz;
290 normal.curvature = bad_point;
293 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
295 if (!init_depth_change_)
296 initAverageDepthChangeMethod ();
299 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
300 unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
301 unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
302 unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
304 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
306 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
310 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);
311 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);
312 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);
313 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);
315 PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
316 PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
317 PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
318 PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
320 const float mean_x_z = mean_R_z - mean_L_z;
321 const float mean_y_z = mean_D_z - mean_U_z;
323 const float mean_x_x = pointR.x - pointL.x;
324 const float mean_x_y = pointR.y - pointL.y;
325 const float mean_y_x = pointD.x - pointU.x;
326 const float mean_y_y = pointD.y - pointU.y;
328 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
329 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
330 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
332 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
334 if (normal_length == 0.0f)
336 normal.getNormalVector3fMap ().setConstant (bad_point);
337 normal.curvature = bad_point;
343 const float scale = 1.0f / std::sqrt (normal_length);
345 normal.normal_x = normal_x * scale;
346 normal.normal_y = normal_y * scale;
347 normal.normal_z = normal_z * scale;
348 normal.curvature = bad_point;
352 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
354 if (!init_simple_3d_gradient_)
355 initSimple3DGradientMethod ();
358 Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
359 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
361 Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
362 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
363 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
364 double normal_length = normal_vector.squaredNorm ();
365 if (normal_length == 0.0f)
367 normal.getNormalVector3fMap ().setConstant (bad_point);
368 normal.curvature = bad_point;
372 normal_vector /= sqrt (normal_length);
374 float nx =
static_cast<float> (normal_vector [0]);
375 float ny =
static_cast<float> (normal_vector [1]);
376 float nz =
static_cast<float> (normal_vector [2]);
380 normal.normal_x = nx;
381 normal.normal_y = ny;
382 normal.normal_z = nz;
383 normal.curvature = bad_point;
387 normal.getNormalVector3fMap ().setConstant (bad_point);
388 normal.curvature = bad_point;
393 template <
typename T>
395 sumArea (
int start_x,
int start_y,
int end_x,
int end_y,
const int width,
const int height,
396 const std::function<T(
unsigned,
unsigned,
unsigned,
unsigned)> &f,
403 result += f (0, 0, end_x, end_y);
404 result += f (0, 0, -start_x, -start_y);
405 result += f (0, 0, -start_x, end_y);
406 result += f (0, 0, end_x, -start_y);
408 else if (end_y >= height)
410 result += f (0, start_y, end_x, height-1);
411 result += f (0, start_y, -start_x, height-1);
412 result += f (0, height-(end_y-(height-1)), end_x, height-1);
413 result += f (0, height-(end_y-(height-1)), -start_x, height-1);
417 result += f (0, start_y, end_x, end_y);
418 result += f (0, start_y, -start_x, end_y);
421 else if (start_y < 0)
425 result += f (start_x, 0, width-1, end_y);
426 result += f (start_x, 0, width-1, -start_y);
427 result += f (width-(end_x-(width-1)), 0, width-1, end_y);
428 result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
432 result += f (start_x, 0, end_x, end_y);
433 result += f (start_x, 0, end_x, -start_y);
436 else if (end_x >= width)
440 result += f (start_x, start_y, width-1, height-1);
441 result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
442 result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
443 result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
447 result += f (start_x, start_y, width-1, end_y);
448 result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
451 else if (end_y >= height)
453 result += f (start_x, start_y, end_x, height-1);
454 result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
458 result += f (start_x, start_y, end_x, end_y);
463 template <
typename Po
intInT,
typename Po
intOutT>
void
465 const int pos_x,
const int pos_y,
const unsigned point_index, PointOutT &normal)
467 float bad_point = std::numeric_limits<float>::quiet_NaN ();
469 const int width = input_->width;
470 const int height = input_->height;
473 if (normal_estimation_method_ == COVARIANCE_MATRIX)
475 if (!init_covariance_matrix_)
476 initCovarianceMatrixMethod ();
478 const int start_x = pos_x - rect_width_2_;
479 const int start_y = pos_y - rect_height_2_;
480 const int end_x = start_x + rect_width_;
481 const int end_y = start_y + rect_height_;
484 auto cb_xyz_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
485 sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
490 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
494 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
495 Eigen::Vector3f center;
513 auto cb_xyz_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.
getFirstOrderSumSE (p1, p2, p3, p4); };
514 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
515 auto cb_xyz_sosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
516 sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
518 center[0] = float (tmp_center[0]);
519 center[1] = float (tmp_center[1]);
520 center[2] = float (tmp_center[2]);
522 covariance_matrix.coeffRef (0) =
static_cast<float> (so_elements [0]);
523 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) =
static_cast<float> (so_elements [1]);
524 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) =
static_cast<float> (so_elements [2]);
525 covariance_matrix.coeffRef (4) =
static_cast<float> (so_elements [3]);
526 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) =
static_cast<float> (so_elements [4]);
527 covariance_matrix.coeffRef (8) =
static_cast<float> (so_elements [5]);
528 covariance_matrix -= (center * center.transpose ()) /
static_cast<float> (count);
530 Eigen::Vector3f eigen_vector;
531 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
533 normal.getNormalVector3fMap () = eigen_vector;
536 if (eigen_value > 0.0)
537 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
539 normal.curvature = 0;
544 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
546 if (!init_average_3d_gradient_)
547 initAverage3DGradientMethod ();
549 const int start_x = pos_x - rect_width_2_;
550 const int start_y = pos_y - rect_height_2_;
551 const int end_x = start_x + rect_width_;
552 const int end_y = start_y + rect_height_;
554 unsigned count_x = 0;
555 unsigned count_y = 0;
557 auto cb_dx_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
558 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
559 auto cb_dy_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
560 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
563 if (count_x == 0 || count_y == 0)
565 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
568 Eigen::Vector3d gradient_x (0, 0, 0);
569 Eigen::Vector3d gradient_y (0, 0, 0);
571 auto cb_dx_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
572 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
573 auto cb_dy_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
574 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
577 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
578 double normal_length = normal_vector.squaredNorm ();
579 if (normal_length == 0.0f)
581 normal.getNormalVector3fMap ().setConstant (bad_point);
582 normal.curvature = bad_point;
586 normal_vector /= sqrt (normal_length);
588 float nx =
static_cast<float> (normal_vector [0]);
589 float ny =
static_cast<float> (normal_vector [1]);
590 float nz =
static_cast<float> (normal_vector [2]);
594 normal.normal_x = nx;
595 normal.normal_y = ny;
596 normal.normal_z = nz;
597 normal.curvature = bad_point;
601 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
603 if (!init_depth_change_)
604 initAverageDepthChangeMethod ();
606 int point_index_L_x = pos_x - rect_width_4_ - 1;
607 int point_index_L_y = pos_y;
608 int point_index_R_x = pos_x + rect_width_4_ + 1;
609 int point_index_R_y = pos_y;
610 int point_index_U_x = pos_x - 1;
611 int point_index_U_y = pos_y - rect_height_4_;
612 int point_index_D_x = pos_x + 1;
613 int point_index_D_y = pos_y + rect_height_4_;
615 if (point_index_L_x < 0)
616 point_index_L_x = -point_index_L_x;
617 if (point_index_U_x < 0)
618 point_index_U_x = -point_index_U_x;
619 if (point_index_U_y < 0)
620 point_index_U_y = -point_index_U_y;
622 if (point_index_R_x >= width)
623 point_index_R_x = width-(point_index_R_x-(width-1));
624 if (point_index_D_x >= width)
625 point_index_D_x = width-(point_index_D_x-(width-1));
626 if (point_index_D_y >= height)
627 point_index_D_y = height-(point_index_D_y-(height-1));
629 const int start_x_L = pos_x - rect_width_2_;
630 const int start_y_L = pos_y - rect_height_4_;
631 const int end_x_L = start_x_L + rect_width_2_;
632 const int end_y_L = start_y_L + rect_height_2_;
634 const int start_x_R = pos_x + 1;
635 const int start_y_R = pos_y - rect_height_4_;
636 const int end_x_R = start_x_R + rect_width_2_;
637 const int end_y_R = start_y_R + rect_height_2_;
639 const int start_x_U = pos_x - rect_width_4_;
640 const int start_y_U = pos_y - rect_height_2_;
641 const int end_x_U = start_x_U + rect_width_2_;
642 const int end_y_U = start_y_U + rect_height_2_;
644 const int start_x_D = pos_x - rect_width_4_;
645 const int start_y_D = pos_y + 1;
646 const int end_x_D = start_x_D + rect_width_2_;
647 const int end_y_D = start_y_D + rect_height_2_;
649 unsigned count_L_z = 0;
650 unsigned count_R_z = 0;
651 unsigned count_U_z = 0;
652 unsigned count_D_z = 0;
654 auto cb_fecse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
655 sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
656 sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
657 sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
658 sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
660 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
662 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
671 auto cb_fosse = [
this] (
unsigned p1,
unsigned p2,
unsigned p3,
unsigned p4) {
return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
672 sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
673 sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
674 sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
675 sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
677 mean_L_z /= float (count_L_z);
678 mean_R_z /= float (count_R_z);
679 mean_U_z /= float (count_U_z);
680 mean_D_z /= float (count_D_z);
683 PointInT pointL = (*input_)[point_index_L_y*width + point_index_L_x];
684 PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
685 PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
686 PointInT pointD = (*input_)[point_index_D_y*width + point_index_D_x];
688 const float mean_x_z = mean_R_z - mean_L_z;
689 const float mean_y_z = mean_D_z - mean_U_z;
691 const float mean_x_x = pointR.x - pointL.x;
692 const float mean_x_y = pointR.y - pointL.y;
693 const float mean_y_x = pointD.x - pointU.x;
694 const float mean_y_y = pointD.y - pointU.y;
696 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
697 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
698 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
700 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
702 if (normal_length == 0.0f)
704 normal.getNormalVector3fMap ().setConstant (bad_point);
705 normal.curvature = bad_point;
711 const float scale = 1.0f / std::sqrt (normal_length);
713 normal.normal_x = normal_x * scale;
714 normal.normal_y = normal_y * scale;
715 normal.normal_z = normal_z * scale;
716 normal.curvature = bad_point;
721 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
723 PCL_THROW_EXCEPTION (
PCLException,
"BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
726 normal.getNormalVector3fMap ().setConstant (bad_point);
727 normal.curvature = bad_point;
732 template <
typename Po
intInT,
typename Po
intOutT>
void
738 float bad_point = std::numeric_limits<float>::quiet_NaN ();
741 unsigned char * depthChangeMap =
new unsigned char[input_->size ()];
742 memset (depthChangeMap, 255, input_->size ());
745 for (
unsigned int ri = 0; ri < input_->height-1; ++ri)
747 for (
unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
749 index = ri * input_->width + ci;
751 const float depth = input_->points [index].z;
752 const float depthR = input_->points [index + 1].z;
753 const float depthD = input_->points [index + input_->width].z;
756 const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
758 if (std::fabs (depth - depthR) > depthDependendDepthChange
759 || !std::isfinite (depth) || !std::isfinite (depthR))
761 depthChangeMap[index] = 0;
762 depthChangeMap[index+1] = 0;
764 if (std::fabs (depth - depthD) > depthDependendDepthChange
765 || !std::isfinite (depth) || !std::isfinite (depthD))
767 depthChangeMap[index] = 0;
768 depthChangeMap[index + input_->width] = 0;
775 delete[] distance_map_;
776 distance_map_ =
new float[input_->size ()];
777 float *distanceMap = distance_map_;
778 for (std::size_t index = 0; index < input_->size (); ++index)
780 if (depthChangeMap[index] == 0)
781 distanceMap[index] = 0.0f;
783 distanceMap[index] =
static_cast<float> (input_->width + input_->height);
787 float* previous_row = distanceMap;
788 float* current_row = previous_row + input_->width;
789 for (std::size_t ri = 1; ri < input_->height; ++ri)
791 for (std::size_t ci = 1; ci < input_->width; ++ci)
793 const float upLeft = previous_row [ci - 1] + 1.4f;
794 const float up = previous_row [ci] + 1.0f;
795 const float upRight = previous_row [ci + 1] + 1.4f;
796 const float left = current_row [ci - 1] + 1.0f;
797 const float center = current_row [ci];
799 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
801 if (minValue < center)
802 current_row [ci] = minValue;
804 previous_row = current_row;
805 current_row += input_->width;
808 float* next_row = distanceMap + input_->width * (input_->height - 1);
809 current_row = next_row - input_->width;
811 for (
int ri = input_->height-2; ri >= 0; --ri)
813 for (
int ci = input_->width-2; ci >= 0; --ci)
815 const float lowerLeft = next_row [ci - 1] + 1.4f;
816 const float lower = next_row [ci] + 1.0f;
817 const float lowerRight = next_row [ci + 1] + 1.4f;
818 const float right = current_row [ci + 1] + 1.0f;
819 const float center = current_row [ci];
821 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
823 if (minValue < center)
824 current_row [ci] = minValue;
826 next_row = current_row;
827 current_row -= input_->width;
830 if (indices_->size () < input_->size ())
831 computeFeaturePart (distanceMap, bad_point, output);
833 computeFeatureFull (distanceMap, bad_point, output);
835 delete[] depthChangeMap;
839 template <
typename Po
intInT,
typename Po
intOutT>
void
841 const float &bad_point,
846 if (border_policy_ == BORDER_POLICY_IGNORE)
852 unsigned border = int(normal_smoothing_size_);
853 PointOutT* vec1 = &output [0];
854 PointOutT* vec2 = vec1 + input_->
width * (input_->height - border);
856 std::size_t count = border * input_->width;
857 for (std::size_t idx = 0; idx < count; ++idx)
859 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
860 vec1 [idx].curvature = bad_point;
861 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
862 vec2 [idx].curvature = bad_point;
866 vec1 = &output [border * input_->width];
867 vec2 = vec1 + input_->
width - border;
868 for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
870 for (std::size_t ci = 0; ci < border; ++ci)
872 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
873 vec1 [ci].curvature = bad_point;
874 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
875 vec2 [ci].curvature = bad_point;
879 if (use_depth_dependent_smoothing_)
881 index = border + input_->width * border;
882 unsigned skip = (border << 1);
883 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
885 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
887 index = ri * input_->width + ci;
889 const float depth = (*input_)[index].z;
890 if (!std::isfinite (depth))
892 output[index].getNormalVector3fMap ().setConstant (bad_point);
893 output[index].curvature = bad_point;
897 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
899 if (smoothing > 2.0f)
901 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
906 output[index].getNormalVector3fMap ().setConstant (bad_point);
907 output[index].curvature = bad_point;
914 float smoothing_constant = normal_smoothing_size_;
916 index = border + input_->
width * border;
917 unsigned skip = (border << 1);
918 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
920 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
922 index = ri * input_->width + ci;
924 if (!std::isfinite ((*input_)[index].z))
926 output [index].getNormalVector3fMap ().setConstant (bad_point);
927 output [index].curvature = bad_point;
931 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
933 if (smoothing > 2.0f)
935 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
940 output [index].getNormalVector3fMap ().setConstant (bad_point);
941 output [index].curvature = bad_point;
947 else if (border_policy_ == BORDER_POLICY_MIRROR)
951 if (use_depth_dependent_smoothing_)
956 for (
unsigned ri = 0; ri < input_->height; ++ri)
959 for (
unsigned ci = 0; ci < input_->width; ++ci)
961 index = ri * input_->width + ci;
963 const float depth = (*input_)[index].z;
964 if (!std::isfinite (depth))
966 output[index].getNormalVector3fMap ().setConstant (bad_point);
967 output[index].curvature = bad_point;
971 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
973 if (smoothing > 2.0f)
975 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
976 computePointNormalMirror (ci, ri, index, output [index]);
980 output[index].getNormalVector3fMap ().setConstant (bad_point);
981 output[index].curvature = bad_point;
988 float smoothing_constant = normal_smoothing_size_;
993 for (
unsigned ri = 0; ri < input_->height; ++ri)
996 for (
unsigned ci = 0; ci < input_->width; ++ci)
998 index = ri * input_->
width + ci;
1000 if (!std::isfinite ((*input_)[index].z))
1002 output [index].getNormalVector3fMap ().setConstant (bad_point);
1003 output [index].curvature = bad_point;
1007 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
1009 if (smoothing > 2.0f)
1011 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1012 computePointNormalMirror (ci, ri, index, output [index]);
1016 output [index].getNormalVector3fMap ().setConstant (bad_point);
1017 output [index].curvature = bad_point;
1026 template <
typename Po
intInT,
typename Po
intOutT>
void
1028 const float &bad_point,
1031 if (border_policy_ == BORDER_POLICY_IGNORE)
1034 unsigned border = int(normal_smoothing_size_);
1035 unsigned bottom = input_->height > border ? input_->height - border : 0;
1036 unsigned right = input_->width > border ? input_->width - border : 0;
1037 if (use_depth_dependent_smoothing_)
1040 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1042 unsigned pt_index = (*indices_)[idx];
1043 unsigned u = pt_index % input_->width;
1044 unsigned v = pt_index / input_->width;
1045 if (v < border || v > bottom)
1047 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1048 output[idx].curvature = bad_point;
1052 if (u < border || u > right)
1054 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1055 output[idx].curvature = bad_point;
1059 const float depth = (*input_)[pt_index].z;
1060 if (!std::isfinite (depth))
1062 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1063 output[idx].curvature = bad_point;
1067 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
1068 if (smoothing > 2.0f)
1070 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1075 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1076 output[idx].curvature = bad_point;
1082 float smoothing_constant = normal_smoothing_size_;
1084 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1086 unsigned pt_index = (*indices_)[idx];
1087 unsigned u = pt_index % input_->
width;
1088 unsigned v = pt_index / input_->width;
1089 if (v < border || v > bottom)
1091 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1092 output[idx].curvature = bad_point;
1096 if (u < border || u > right)
1098 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1099 output[idx].curvature = bad_point;
1103 if (!std::isfinite ((*input_)[pt_index].z))
1105 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1106 output [idx].curvature = bad_point;
1110 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1112 if (smoothing > 2.0f)
1114 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1119 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1120 output [idx].curvature = bad_point;
1125 else if (border_policy_ == BORDER_POLICY_MIRROR)
1129 if (use_depth_dependent_smoothing_)
1131 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1133 unsigned pt_index = (*indices_)[idx];
1134 unsigned u = pt_index % input_->width;
1135 unsigned v = pt_index / input_->width;
1137 const float depth = (*input_)[pt_index].z;
1138 if (!std::isfinite (depth))
1140 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1141 output[idx].curvature = bad_point;
1145 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ +
static_cast<float>(depth)/10.0f);
1147 if (smoothing > 2.0f)
1149 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1150 computePointNormalMirror (u, v, pt_index, output [idx]);
1154 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1155 output[idx].curvature = bad_point;
1161 float smoothing_constant = normal_smoothing_size_;
1162 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1164 unsigned pt_index = (*indices_)[idx];
1165 unsigned u = pt_index % input_->
width;
1166 unsigned v = pt_index / input_->width;
1168 if (!std::isfinite ((*input_)[pt_index].z))
1170 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1171 output [idx].curvature = bad_point;
1175 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1177 if (smoothing > 2.0f)
1179 setRectSize (
static_cast<int> (smoothing),
static_cast<int> (smoothing));
1180 computePointNormalMirror (u, v, pt_index, output [idx]);
1184 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1185 output [idx].curvature = bad_point;
1193 template <
typename Po
intInT,
typename Po
intOutT>
bool
1196 if (!input_->isOrganized ())
1198 PCL_ERROR (
"[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1201 return (Feature<PointInT, PointOutT>::initCompute ());
1204 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;