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>
48 if (diff_x_ != NULL)
delete[] diff_x_;
49 if (diff_y_ != NULL)
delete[] diff_y_;
50 if (depth_data_ != NULL)
delete[] depth_data_;
51 if (distance_map_ != NULL)
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.");
71 if (diff_x_ != NULL)
delete[] diff_x_;
72 if (diff_y_ != NULL)
delete[] diff_y_;
73 if (depth_data_ != NULL)
delete[] depth_data_;
74 if (distance_map_ != NULL)
delete[] distance_map_;
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_->points[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_->points[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 size_t data_size = (input_->points.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 (
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 (
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_->points[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;
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);
242 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
243 normal.getNormalVector3fMap () = eigen_vector;
246 if (eigen_value > 0.0)
247 normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
249 normal.curvature = 0;
253 else 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 else 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_->points[point_index - rect_width_4_ - 1];
314 PointInT pointR = input_->points[point_index + rect_width_4_ + 1];
315 PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1];
316 PointInT pointD = input_->points[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 / sqrtf (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 else 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 boost::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 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&
IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_XYZ_, _1, _2, _3, _4), count);
487 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
492 Eigen::Vector3f center;
510 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&
IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), tmp_center);
511 sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&
IntegralImage2D<float, 3>::getSecondOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), so_elements);
513 center[0] = float (tmp_center[0]);
514 center[1] = float (tmp_center[1]);
515 center[2] = float (tmp_center[2]);
517 covariance_matrix.coeffRef (0) =
static_cast<float> (so_elements [0]);
518 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) =
static_cast<float> (so_elements [1]);
519 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) =
static_cast<float> (so_elements [2]);
520 covariance_matrix.coeffRef (4) =
static_cast<float> (so_elements [3]);
521 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) =
static_cast<float> (so_elements [4]);
522 covariance_matrix.coeffRef (8) =
static_cast<float> (so_elements [5]);
523 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
525 Eigen::Vector3f eigen_vector;
526 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
527 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
528 normal.getNormalVector3fMap () = eigen_vector;
531 if (eigen_value > 0.0)
532 normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
534 normal.curvature = 0;
539 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
541 if (!init_average_3d_gradient_)
542 initAverage3DGradientMethod ();
544 const int start_x = pos_x - rect_width_2_;
545 const int start_y = pos_y - rect_height_2_;
546 const int end_x = start_x + rect_width_;
547 const int end_y = start_y + rect_height_;
549 unsigned count_x = 0;
550 unsigned count_y = 0;
552 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&
IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DX_, _1, _2, _3, _4), count_x);
553 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&
IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DY_, _1, _2, _3, _4), count_y);
556 if (count_x == 0 || count_y == 0)
558 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
561 Eigen::Vector3d gradient_x (0, 0, 0);
562 Eigen::Vector3d gradient_y (0, 0, 0);
564 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&
IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DX_, _1, _2, _3, _4), gradient_x);
565 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&
IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DY_, _1, _2, _3, _4), gradient_y);
568 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
569 double normal_length = normal_vector.squaredNorm ();
570 if (normal_length == 0.0f)
572 normal.getNormalVector3fMap ().setConstant (bad_point);
573 normal.curvature = bad_point;
577 normal_vector /= sqrt (normal_length);
579 float nx =
static_cast<float> (normal_vector [0]);
580 float ny =
static_cast<float> (normal_vector [1]);
581 float nz =
static_cast<float> (normal_vector [2]);
585 normal.normal_x = nx;
586 normal.normal_y = ny;
587 normal.normal_z = nz;
588 normal.curvature = bad_point;
592 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
594 if (!init_depth_change_)
595 initAverageDepthChangeMethod ();
597 int point_index_L_x = pos_x - rect_width_4_ - 1;
598 int point_index_L_y = pos_y;
599 int point_index_R_x = pos_x + rect_width_4_ + 1;
600 int point_index_R_y = pos_y;
601 int point_index_U_x = pos_x - 1;
602 int point_index_U_y = pos_y - rect_height_4_;
603 int point_index_D_x = pos_x + 1;
604 int point_index_D_y = pos_y + rect_height_4_;
606 if (point_index_L_x < 0)
607 point_index_L_x = -point_index_L_x;
608 if (point_index_U_x < 0)
609 point_index_U_x = -point_index_U_x;
610 if (point_index_U_y < 0)
611 point_index_U_y = -point_index_U_y;
613 if (point_index_R_x >= width)
614 point_index_R_x = width-(point_index_R_x-(width-1));
615 if (point_index_D_x >= width)
616 point_index_D_x = width-(point_index_D_x-(width-1));
617 if (point_index_D_y >= height)
618 point_index_D_y = height-(point_index_D_y-(height-1));
620 const int start_x_L = pos_x - rect_width_2_;
621 const int start_y_L = pos_y - rect_height_4_;
622 const int end_x_L = start_x_L + rect_width_2_;
623 const int end_y_L = start_y_L + rect_height_2_;
625 const int start_x_R = pos_x + 1;
626 const int start_y_R = pos_y - rect_height_4_;
627 const int end_x_R = start_x_R + rect_width_2_;
628 const int end_y_R = start_y_R + rect_height_2_;
630 const int start_x_U = pos_x - rect_width_4_;
631 const int start_y_U = pos_y - rect_height_2_;
632 const int end_x_U = start_x_U + rect_width_2_;
633 const int end_y_U = start_y_U + rect_height_2_;
635 const int start_x_D = pos_x - rect_width_4_;
636 const int start_y_D = pos_y + 1;
637 const int end_x_D = start_x_D + rect_width_2_;
638 const int end_y_D = start_y_D + rect_height_2_;
640 unsigned count_L_z = 0;
641 unsigned count_R_z = 0;
642 unsigned count_U_z = 0;
643 unsigned count_D_z = 0;
645 sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_L_z);
646 sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_R_z);
647 sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_U_z);
648 sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_D_z);
650 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
652 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
661 sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_L_z);
662 sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_R_z);
663 sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_U_z);
664 sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&
IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_D_z);
666 mean_L_z /= float (count_L_z);
667 mean_R_z /= float (count_R_z);
668 mean_U_z /= float (count_U_z);
669 mean_D_z /= float (count_D_z);
672 PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x];
673 PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x];
674 PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x];
675 PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x];
677 const float mean_x_z = mean_R_z - mean_L_z;
678 const float mean_y_z = mean_D_z - mean_U_z;
680 const float mean_x_x = pointR.x - pointL.x;
681 const float mean_x_y = pointR.y - pointL.y;
682 const float mean_y_x = pointD.x - pointU.x;
683 const float mean_y_y = pointD.y - pointU.y;
685 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
686 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
687 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
689 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
691 if (normal_length == 0.0f)
693 normal.getNormalVector3fMap ().setConstant (bad_point);
694 normal.curvature = bad_point;
700 const float scale = 1.0f / sqrtf (normal_length);
702 normal.normal_x = normal_x * scale;
703 normal.normal_y = normal_y * scale;
704 normal.normal_z = normal_z * scale;
705 normal.curvature = bad_point;
710 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
712 PCL_THROW_EXCEPTION (
PCLException,
"BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
715 normal.getNormalVector3fMap ().setConstant (bad_point);
716 normal.curvature = bad_point;
721 template <
typename Po
intInT,
typename Po
intOutT>
void
727 float bad_point = std::numeric_limits<float>::quiet_NaN ();
730 unsigned char * depthChangeMap =
new unsigned char[input_->points.size ()];
731 memset (depthChangeMap, 255, input_->points.size ());
734 for (
unsigned int ri = 0; ri < input_->height-1; ++ri)
736 for (
unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
738 index = ri * input_->width + ci;
740 const float depth = input_->points [index].z;
741 const float depthR = input_->points [index + 1].z;
742 const float depthD = input_->points [index + input_->width].z;
745 const float depthDependendDepthChange = (max_depth_change_factor_ * (fabsf (depth) + 1.0f) * 2.0f);
747 if (fabs (depth - depthR) > depthDependendDepthChange
748 || !pcl_isfinite (depth) || !pcl_isfinite (depthR))
750 depthChangeMap[index] = 0;
751 depthChangeMap[index+1] = 0;
753 if (fabs (depth - depthD) > depthDependendDepthChange
754 || !pcl_isfinite (depth) || !pcl_isfinite (depthD))
756 depthChangeMap[index] = 0;
757 depthChangeMap[index + input_->width] = 0;
764 if (distance_map_ != NULL)
delete[] distance_map_;
765 distance_map_ =
new float[input_->points.size ()];
766 float *distanceMap = distance_map_;
767 for (
size_t index = 0; index < input_->points.size (); ++index)
769 if (depthChangeMap[index] == 0)
770 distanceMap[index] = 0.0f;
772 distanceMap[index] =
static_cast<float> (input_->width + input_->height);
776 float* previous_row = distanceMap;
777 float* current_row = previous_row + input_->width;
778 for (
size_t ri = 1; ri < input_->height; ++ri)
780 for (
size_t ci = 1; ci < input_->width; ++ci)
782 const float upLeft = previous_row [ci - 1] + 1.4f;
783 const float up = previous_row [ci] + 1.0f;
784 const float upRight = previous_row [ci + 1] + 1.4f;
785 const float left = current_row [ci - 1] + 1.0f;
786 const float center = current_row [ci];
788 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
790 if (minValue < center)
791 current_row [ci] = minValue;
793 previous_row = current_row;
794 current_row += input_->width;
797 float* next_row = distanceMap + input_->width * (input_->height - 1);
798 current_row = next_row - input_->width;
800 for (
int ri = input_->height-2; ri >= 0; --ri)
802 for (
int ci = input_->width-2; ci >= 0; --ci)
804 const float lowerLeft = next_row [ci - 1] + 1.4f;
805 const float lower = next_row [ci] + 1.0f;
806 const float lowerRight = next_row [ci + 1] + 1.4f;
807 const float right = current_row [ci + 1] + 1.0f;
808 const float center = current_row [ci];
810 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
812 if (minValue < center)
813 current_row [ci] = minValue;
815 next_row = current_row;
816 current_row -= input_->width;
819 if (border_policy_ == BORDER_POLICY_IGNORE)
825 unsigned border = int(normal_smoothing_size_);
826 PointOutT* vec1 = &output [0];
827 PointOutT* vec2 = vec1 + input_->
width * (input_->height - border);
829 size_t count = border * input_->width;
830 for (
size_t idx = 0; idx < count; ++idx)
832 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
833 vec1 [idx].curvature = bad_point;
834 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
835 vec2 [idx].curvature = bad_point;
839 vec1 = &output [border * input_->width];
840 vec2 = vec1 + input_->
width - border;
841 for (
size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
843 for (
size_t ci = 0; ci < border; ++ci)
845 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
846 vec1 [ci].curvature = bad_point;
847 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
848 vec2 [ci].curvature = bad_point;
852 if (use_depth_dependent_smoothing_)
854 index = border + input_->width * border;
855 unsigned skip = (border << 1);
856 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
858 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
860 index = ri * input_->width + ci;
862 const float depth = input_->points[index].z;
863 if (!pcl_isfinite (depth))
865 output[index].getNormalVector3fMap ().setConstant (bad_point);
866 output[index].curvature = bad_point;
870 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
872 if (smoothing > 2.0f)
874 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
879 output[index].getNormalVector3fMap ().setConstant (bad_point);
880 output[index].curvature = bad_point;
887 float smoothing_constant = normal_smoothing_size_;
889 index = border + input_->
width * border;
890 unsigned skip = (border << 1);
891 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
893 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
895 index = ri * input_->width + ci;
897 if (!pcl_isfinite (input_->points[index].z))
899 output [index].getNormalVector3fMap ().setConstant (bad_point);
900 output [index].curvature = bad_point;
904 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
906 if (smoothing > 2.0f)
908 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
913 output [index].getNormalVector3fMap ().setConstant (bad_point);
914 output [index].curvature = bad_point;
920 else if (border_policy_ == BORDER_POLICY_MIRROR)
924 if (use_depth_dependent_smoothing_)
929 for (
unsigned ri = 0; ri < input_->height; ++ri)
932 for (
unsigned ci = 0; ci < input_->width; ++ci)
934 index = ri * input_->width + ci;
936 const float depth = input_->points[index].z;
937 if (!pcl_isfinite (depth))
939 output[index].getNormalVector3fMap ().setConstant (bad_point);
940 output[index].curvature = bad_point;
944 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
946 if (smoothing > 2.0f)
948 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
949 computePointNormalMirror (ci, ri, index, output [index]);
953 output[index].getNormalVector3fMap ().setConstant (bad_point);
954 output[index].curvature = bad_point;
961 float smoothing_constant = normal_smoothing_size_;
966 for (
unsigned ri = 0; ri < input_->height; ++ri)
969 for (
unsigned ci = 0; ci < input_->width; ++ci)
971 index = ri * input_->
width + ci;
973 if (!pcl_isfinite (input_->points[index].z))
975 output [index].getNormalVector3fMap ().setConstant (bad_point);
976 output [index].curvature = bad_point;
980 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
982 if (smoothing > 2.0f)
984 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
985 computePointNormalMirror (ci, ri, index, output [index]);
989 output [index].getNormalVector3fMap ().setConstant (bad_point);
990 output [index].curvature = bad_point;
997 delete[] depthChangeMap;
1002 template <
typename Po
intInT,
typename Po
intOutT>
bool
1005 if (!input_->isOrganized ())
1007 PCL_ERROR (
"[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1010 return (Feature<PointInT, PointOutT>::initCompute ());
1013 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;