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>;
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Determines an integral image representation for a given organized data array.
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Surface normal estimation on organized data using integral images.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
~IntegralImageNormalEstimation()
Destructor.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
void computePointNormalMirror(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position with mirroring for border handling.
A base class for all pcl exceptions which inherits from std::runtime_error.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::uint32_t width
The point cloud width (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
bool computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...