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>;
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.
void computeFeature(PointCloudOut &output)
Computes the normal for the complete cloud.
Determines an integral image representation for a given organized data array.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
uint32_t width
The point cloud width (if organized as an image-structure).
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
struct pcl::_PointXYZHSV EIGEN_ALIGN16
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. ...
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Surface normal estimation on organized data using integral images.
void 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...
virtual ~IntegralImageNormalEstimation()
Destructor.
A base class for all pcl exceptions which inherits from std::runtime_error.
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.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).