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 / 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 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 / std::sqrt (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 (indices_->size () < input_->size ())
820 computeFeaturePart (distanceMap, bad_point, output);
822 computeFeatureFull (distanceMap, bad_point, output);
824 delete[] depthChangeMap;
828 template <
typename Po
intInT,
typename Po
intOutT>
void 830 const float &bad_point,
835 if (border_policy_ == BORDER_POLICY_IGNORE)
841 unsigned border = int(normal_smoothing_size_);
842 PointOutT* vec1 = &output [0];
843 PointOutT* vec2 = vec1 + input_->
width * (input_->height - border);
845 size_t count = border * input_->width;
846 for (
size_t idx = 0; idx < count; ++idx)
848 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
849 vec1 [idx].curvature = bad_point;
850 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
851 vec2 [idx].curvature = bad_point;
855 vec1 = &output [border * input_->width];
856 vec2 = vec1 + input_->
width - border;
857 for (
size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
859 for (
size_t ci = 0; ci < border; ++ci)
861 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
862 vec1 [ci].curvature = bad_point;
863 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
864 vec2 [ci].curvature = bad_point;
868 if (use_depth_dependent_smoothing_)
870 index = border + input_->width * border;
871 unsigned skip = (border << 1);
872 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
874 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
876 index = ri * input_->width + ci;
878 const float depth = input_->points[index].z;
879 if (!pcl_isfinite (depth))
881 output[index].getNormalVector3fMap ().setConstant (bad_point);
882 output[index].curvature = bad_point;
886 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
888 if (smoothing > 2.0f)
890 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
895 output[index].getNormalVector3fMap ().setConstant (bad_point);
896 output[index].curvature = bad_point;
903 float smoothing_constant = normal_smoothing_size_;
905 index = border + input_->
width * border;
906 unsigned skip = (border << 1);
907 for (
unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
909 for (
unsigned ci = border; ci < input_->width - border; ++ci, ++index)
911 index = ri * input_->width + ci;
913 if (!pcl_isfinite (input_->points[index].z))
915 output [index].getNormalVector3fMap ().setConstant (bad_point);
916 output [index].curvature = bad_point;
920 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
922 if (smoothing > 2.0f)
924 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
929 output [index].getNormalVector3fMap ().setConstant (bad_point);
930 output [index].curvature = bad_point;
936 else if (border_policy_ == BORDER_POLICY_MIRROR)
940 if (use_depth_dependent_smoothing_)
945 for (
unsigned ri = 0; ri < input_->height; ++ri)
948 for (
unsigned ci = 0; ci < input_->width; ++ci)
950 index = ri * input_->width + ci;
952 const float depth = input_->points[index].z;
953 if (!pcl_isfinite (depth))
955 output[index].getNormalVector3fMap ().setConstant (bad_point);
956 output[index].curvature = bad_point;
960 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
962 if (smoothing > 2.0f)
964 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
965 computePointNormalMirror (ci, ri, index, output [index]);
969 output[index].getNormalVector3fMap ().setConstant (bad_point);
970 output[index].curvature = bad_point;
977 float smoothing_constant = normal_smoothing_size_;
982 for (
unsigned ri = 0; ri < input_->height; ++ri)
985 for (
unsigned ci = 0; ci < input_->width; ++ci)
987 index = ri * input_->
width + ci;
989 if (!pcl_isfinite (input_->points[index].z))
991 output [index].getNormalVector3fMap ().setConstant (bad_point);
992 output [index].curvature = bad_point;
996 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
998 if (smoothing > 2.0f)
1000 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1001 computePointNormalMirror (ci, ri, index, output [index]);
1005 output [index].getNormalVector3fMap ().setConstant (bad_point);
1006 output [index].curvature = bad_point;
1015 template <
typename Po
intInT,
typename Po
intOutT>
void 1017 const float &bad_point,
1020 if (border_policy_ == BORDER_POLICY_IGNORE)
1023 unsigned border = int(normal_smoothing_size_);
1024 unsigned bottom = input_->height > border ? input_->height - border : 0;
1025 unsigned right = input_->width > border ? input_->width - border : 0;
1026 if (use_depth_dependent_smoothing_)
1029 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1031 unsigned pt_index = (*indices_)[idx];
1032 unsigned u = pt_index % input_->width;
1033 unsigned v = pt_index / input_->width;
1034 if (v < border || v > bottom)
1036 output.
points[idx].getNormalVector3fMap ().setConstant (bad_point);
1037 output.
points[idx].curvature = bad_point;
1041 if (u < border || v > right)
1043 output.
points[idx].getNormalVector3fMap ().setConstant (bad_point);
1044 output.
points[idx].curvature = bad_point;
1048 const float depth = input_->points[pt_index].z;
1049 if (!pcl_isfinite (depth))
1051 output.
points[idx].getNormalVector3fMap ().setConstant (bad_point);
1052 output.
points[idx].curvature = bad_point;
1056 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1057 if (smoothing > 2.0f)
1059 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1064 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1065 output[idx].curvature = bad_point;
1071 float smoothing_constant = normal_smoothing_size_;
1073 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1075 unsigned pt_index = (*indices_)[idx];
1076 unsigned u = pt_index % input_->
width;
1077 unsigned v = pt_index / input_->width;
1078 if (v < border || v > bottom)
1080 output.
points[idx].getNormalVector3fMap ().setConstant (bad_point);
1081 output.
points[idx].curvature = bad_point;
1085 if (u < border || v > right)
1087 output.
points[idx].getNormalVector3fMap ().setConstant (bad_point);
1088 output.
points[idx].curvature = bad_point;
1092 if (!pcl_isfinite (input_->points[pt_index].z))
1094 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1095 output [idx].curvature = bad_point;
1099 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1101 if (smoothing > 2.0f)
1103 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1108 output [pt_index].getNormalVector3fMap ().setConstant (bad_point);
1109 output [pt_index].curvature = bad_point;
1114 else if (border_policy_ == BORDER_POLICY_MIRROR)
1118 if (use_depth_dependent_smoothing_)
1120 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1122 unsigned pt_index = (*indices_)[idx];
1123 unsigned u = pt_index % input_->width;
1124 unsigned v = pt_index / input_->width;
1126 const float depth = input_->points[pt_index].z;
1127 if (!pcl_isfinite (depth))
1129 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1130 output[idx].curvature = bad_point;
1134 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1136 if (smoothing > 2.0f)
1138 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1139 computePointNormalMirror (u, v, pt_index, output [idx]);
1143 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1144 output[idx].curvature = bad_point;
1150 float smoothing_constant = normal_smoothing_size_;
1151 for (
size_t idx = 0; idx < indices_->size (); ++idx)
1153 unsigned pt_index = (*indices_)[idx];
1154 unsigned u = pt_index % input_->
width;
1155 unsigned v = pt_index / input_->width;
1157 if (!pcl_isfinite (input_->points[pt_index].z))
1159 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1160 output [idx].curvature = bad_point;
1164 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1166 if (smoothing > 2.0f)
1168 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1169 computePointNormalMirror (u, v, pt_index, output [idx]);
1173 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1174 output [idx].curvature = bad_point;
1182 template <
typename Po
intInT,
typename Po
intOutT>
bool 1185 if (!input_->isOrganized ())
1187 PCL_ERROR (
"[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1190 return (Feature<PointInT, PointOutT>::initCompute ());
1193 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>; std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
virtual ~IntegralImageNormalEstimation()
Destructor.
struct pcl::PointXYZIEdge EIGEN_ALIGN16
A base class for all pcl exceptions which inherits from std::runtime_error.
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. ...
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
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...
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
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.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
uint32_t width
The point cloud width (if organized as an image-structure).
void computeFeature(PointCloudOut &output)
Computes the normal for the complete cloud or only indices_ if provided.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
Surface normal estimation on organized data using integral images.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
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.
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...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.