Point Cloud Library (PCL)  1.12.1-dev
integral_image_normal.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 #pragma once
39 #include <pcl/features/integral_image_normal.h>
40 
41 #include <algorithm>
42 
43 //////////////////////////////////////////////////////////////////////////////////////////
44 template <typename PointInT, typename PointOutT>
46 {
47  delete[] diff_x_;
48  delete[] diff_y_;
49  delete[] depth_data_;
50  delete[] distance_map_;
51 }
52 
53 //////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointInT, typename PointOutT> void
56 {
57  if (border_policy_ != BORDER_POLICY_IGNORE &&
58  border_policy_ != BORDER_POLICY_MIRROR)
59  PCL_THROW_EXCEPTION (InitFailedException,
60  "[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
61 
62  if (normal_estimation_method_ != COVARIANCE_MATRIX &&
63  normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
64  normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
65  normal_estimation_method_ != SIMPLE_3D_GRADIENT)
66  PCL_THROW_EXCEPTION (InitFailedException,
67  "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
68 
69  // compute derivatives
70  delete[] diff_x_;
71  delete[] diff_y_;
72  delete[] depth_data_;
73  delete[] distance_map_;
74  diff_x_ = nullptr;
75  diff_y_ = nullptr;
76  depth_data_ = nullptr;
77  distance_map_ = nullptr;
78 
79  if (normal_estimation_method_ == COVARIANCE_MATRIX)
80  initCovarianceMatrixMethod ();
81  else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
82  initAverage3DGradientMethod ();
83  else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
84  initAverageDepthChangeMethod ();
85  else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
86  initSimple3DGradientMethod ();
87 }
88 
89 
90 //////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointInT, typename PointOutT> void
93 {
94  rect_width_ = width;
95  rect_width_2_ = width/2;
96  rect_width_4_ = width/4;
97  rect_height_ = height;
98  rect_height_2_ = height/2;
99  rect_height_4_ = height/4;
100 }
101 
102 //////////////////////////////////////////////////////////////////////////////////////////
103 template <typename PointInT, typename PointOutT> void
105 {
106  // number of DataType entries per element (equal or bigger than dimensions)
107  int element_stride = sizeof (PointInT) / sizeof (float);
108  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
109  int row_stride = element_stride * input_->width;
110 
111  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
112 
113  integral_image_XYZ_.setSecondOrderComputation (false);
114  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
115 
116  init_simple_3d_gradient_ = true;
117  init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
118 }
119 
120 //////////////////////////////////////////////////////////////////////////////////////////
121 template <typename PointInT, typename PointOutT> void
123 {
124  // number of DataType entries per element (equal or bigger than dimensions)
125  int element_stride = sizeof (PointInT) / sizeof (float);
126  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
127  int row_stride = element_stride * input_->width;
128 
129  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
130 
131  integral_image_XYZ_.setSecondOrderComputation (true);
132  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
133 
134  init_covariance_matrix_ = true;
135  init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false;
136 }
137 
138 //////////////////////////////////////////////////////////////////////////////////////////
139 template <typename PointInT, typename PointOutT> void
141 {
142  delete[] diff_x_;
143  delete[] diff_y_;
144  std::size_t data_size = (input_->size () << 2);
145  diff_x_ = new float[data_size]{};
146  diff_y_ = new float[data_size]{};
147 
148  // x u x
149  // l x r
150  // x d x
151  const PointInT* point_up = &(input_->points [1]);
152  const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]);
153  const PointInT* point_lf = &(input_->points [input_->width]);
154  const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]);
155  float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
156  float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
157  unsigned diff_skip = 8; // skip last element in row and the first in the next row
158 
159  for (std::size_t ri = 1; ri < input_->height - 1; ++ri
160  , point_up += input_->width
161  , point_dn += input_->width
162  , point_lf += input_->width
163  , point_rg += input_->width
164  , diff_x_ptr += diff_skip
165  , diff_y_ptr += diff_skip)
166  {
167  for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
168  {
169  diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
170  diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
171  diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
172 
173  diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
174  diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
175  diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
176  }
177  }
178 
179  // Compute integral images
180  integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
181  integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
182  init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false;
183  init_average_3d_gradient_ = true;
184 }
185 
186 //////////////////////////////////////////////////////////////////////////////////////////
187 template <typename PointInT, typename PointOutT> void
189 {
190  // number of DataType entries per element (equal or bigger than dimensions)
191  int element_stride = sizeof (PointInT) / sizeof (float);
192  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
193  int row_stride = element_stride * input_->width;
194 
195  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
196 
197  // integral image over the z - value
198  integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
199  init_depth_change_ = true;
200  init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
201 }
202 
203 //////////////////////////////////////////////////////////////////////////////////////////
204 template <typename PointInT, typename PointOutT> void
206  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
207 {
208  float bad_point = std::numeric_limits<float>::quiet_NaN ();
209 
210  if (normal_estimation_method_ == COVARIANCE_MATRIX)
211  {
212  if (!init_covariance_matrix_)
213  initCovarianceMatrixMethod ();
214 
215  unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
216 
217  // no valid points within the rectangular region?
218  if (count == 0)
219  {
220  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
221  return;
222  }
223 
224  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
225  Eigen::Vector3f center;
226  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
227  center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
228  so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
229 
230  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
231  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
232  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
233  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
234  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
235  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
236  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
237  float eigen_value;
238  Eigen::Vector3f eigen_vector;
239  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
240  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
241  normal.getNormalVector3fMap () = eigen_vector;
242 
243  // Compute the curvature surface change
244  if (eigen_value > 0.0)
245  normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
246  else
247  normal.curvature = 0;
248 
249  return;
250  }
251  if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
252  {
253  if (!init_average_3d_gradient_)
254  initAverage3DGradientMethod ();
255 
256  unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
257  unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
258  if (count_x == 0 || count_y == 0)
259  {
260  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
261  return;
262  }
263  Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
264  Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
265 
266  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
267  double normal_length = normal_vector.squaredNorm ();
268  if (normal_length == 0.0f)
269  {
270  normal.getNormalVector3fMap ().setConstant (bad_point);
271  normal.curvature = bad_point;
272  return;
273  }
274 
275  normal_vector /= sqrt (normal_length);
276 
277  float nx = static_cast<float> (normal_vector [0]);
278  float ny = static_cast<float> (normal_vector [1]);
279  float nz = static_cast<float> (normal_vector [2]);
280 
281  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
282 
283  normal.normal_x = nx;
284  normal.normal_y = ny;
285  normal.normal_z = nz;
286  normal.curvature = bad_point;
287  return;
288  }
289  if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
290  {
291  if (!init_depth_change_)
292  initAverageDepthChangeMethod ();
293 
294  // width and height are at least 3 x 3
295  unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
296  unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
297  unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
298  unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
299 
300  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
301  {
302  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
303  return;
304  }
305 
306  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);
307  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);
308  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);
309  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);
310 
311  PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
312  PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
313  PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
314  PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
315 
316  const float mean_x_z = mean_R_z - mean_L_z;
317  const float mean_y_z = mean_D_z - mean_U_z;
318 
319  const float mean_x_x = pointR.x - pointL.x;
320  const float mean_x_y = pointR.y - pointL.y;
321  const float mean_y_x = pointD.x - pointU.x;
322  const float mean_y_y = pointD.y - pointU.y;
323 
324  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
325  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
326  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
327 
328  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
329 
330  if (normal_length == 0.0f)
331  {
332  normal.getNormalVector3fMap ().setConstant (bad_point);
333  normal.curvature = bad_point;
334  return;
335  }
336 
337  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
338 
339  const float scale = 1.0f / std::sqrt (normal_length);
340 
341  normal.normal_x = normal_x * scale;
342  normal.normal_y = normal_y * scale;
343  normal.normal_z = normal_z * scale;
344  normal.curvature = bad_point;
345 
346  return;
347  }
348  if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
349  {
350  if (!init_simple_3d_gradient_)
351  initSimple3DGradientMethod ();
352 
353  // this method does not work if lots of NaNs are in the neighborhood of the point
354  Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
355  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
356 
357  Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
358  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
359  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
360  double normal_length = normal_vector.squaredNorm ();
361  if (normal_length == 0.0f)
362  {
363  normal.getNormalVector3fMap ().setConstant (bad_point);
364  normal.curvature = bad_point;
365  return;
366  }
367 
368  normal_vector /= sqrt (normal_length);
369 
370  float nx = static_cast<float> (normal_vector [0]);
371  float ny = static_cast<float> (normal_vector [1]);
372  float nz = static_cast<float> (normal_vector [2]);
373 
374  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
375 
376  normal.normal_x = nx;
377  normal.normal_y = ny;
378  normal.normal_z = nz;
379  normal.curvature = bad_point;
380  return;
381  }
382 
383  normal.getNormalVector3fMap ().setConstant (bad_point);
384  normal.curvature = bad_point;
385  return;
386 }
387 
388 //////////////////////////////////////////////////////////////////////////////////////////
389 template <typename T>
390 void
391 sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
392  const std::function<T(unsigned, unsigned, unsigned, unsigned)> &f,
393  T & result)
394 {
395  if (start_x < 0)
396  {
397  if (start_y < 0)
398  {
399  result += f (0, 0, end_x, end_y);
400  result += f (0, 0, -start_x, -start_y);
401  result += f (0, 0, -start_x, end_y);
402  result += f (0, 0, end_x, -start_y);
403  }
404  else if (end_y >= height)
405  {
406  result += f (0, start_y, end_x, height-1);
407  result += f (0, start_y, -start_x, height-1);
408  result += f (0, height-(end_y-(height-1)), end_x, height-1);
409  result += f (0, height-(end_y-(height-1)), -start_x, height-1);
410  }
411  else
412  {
413  result += f (0, start_y, end_x, end_y);
414  result += f (0, start_y, -start_x, end_y);
415  }
416  }
417  else if (start_y < 0)
418  {
419  if (end_x >= width)
420  {
421  result += f (start_x, 0, width-1, end_y);
422  result += f (start_x, 0, width-1, -start_y);
423  result += f (width-(end_x-(width-1)), 0, width-1, end_y);
424  result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
425  }
426  else
427  {
428  result += f (start_x, 0, end_x, end_y);
429  result += f (start_x, 0, end_x, -start_y);
430  }
431  }
432  else if (end_x >= width)
433  {
434  if (end_y >= height)
435  {
436  result += f (start_x, start_y, width-1, height-1);
437  result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
438  result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
439  result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
440  }
441  else
442  {
443  result += f (start_x, start_y, width-1, end_y);
444  result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
445  }
446  }
447  else if (end_y >= height)
448  {
449  result += f (start_x, start_y, end_x, height-1);
450  result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
451  }
452  else
453  {
454  result += f (start_x, start_y, end_x, end_y);
455  }
456 }
457 
458 //////////////////////////////////////////////////////////////////////////////////////////
459 template <typename PointInT, typename PointOutT> void
461  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
462 {
463  float bad_point = std::numeric_limits<float>::quiet_NaN ();
464 
465  const int width = input_->width;
466  const int height = input_->height;
467 
468  // ==============================================================
469  if (normal_estimation_method_ == COVARIANCE_MATRIX)
470  {
471  if (!init_covariance_matrix_)
472  initCovarianceMatrixMethod ();
473 
474  const int start_x = pos_x - rect_width_2_;
475  const int start_y = pos_y - rect_height_2_;
476  const int end_x = start_x + rect_width_;
477  const int end_y = start_y + rect_height_;
478 
479  unsigned count = 0;
480  auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
481  sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
482 
483  // no valid points within the rectangular region?
484  if (count == 0)
485  {
486  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
487  return;
488  }
489 
490  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
491  Eigen::Vector3f center;
492  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
493  typename IntegralImage2D<float, 3>::ElementType tmp_center;
494  typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
495 
496  center[0] = 0;
497  center[1] = 0;
498  center[2] = 0;
499  tmp_center[0] = 0;
500  tmp_center[1] = 0;
501  tmp_center[2] = 0;
502  so_elements[0] = 0;
503  so_elements[1] = 0;
504  so_elements[2] = 0;
505  so_elements[3] = 0;
506  so_elements[4] = 0;
507  so_elements[5] = 0;
508 
509  auto cb_xyz_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFirstOrderSumSE (p1, p2, p3, p4); };
510  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
511  auto cb_xyz_sosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
512  sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
513 
514  center[0] = float (tmp_center[0]);
515  center[1] = float (tmp_center[1]);
516  center[2] = float (tmp_center[2]);
517 
518  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
519  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
520  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
521  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
522  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
523  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
524  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
525  float eigen_value;
526  Eigen::Vector3f eigen_vector;
527  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
528  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
529  normal.getNormalVector3fMap () = eigen_vector;
530 
531  // Compute the curvature surface change
532  if (eigen_value > 0.0)
533  normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
534  else
535  normal.curvature = 0;
536 
537  return;
538  }
539  // =======================================================
540  if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
541  {
542  if (!init_average_3d_gradient_)
543  initAverage3DGradientMethod ();
544 
545  const int start_x = pos_x - rect_width_2_;
546  const int start_y = pos_y - rect_height_2_;
547  const int end_x = start_x + rect_width_;
548  const int end_y = start_y + rect_height_;
549 
550  unsigned count_x = 0;
551  unsigned count_y = 0;
552 
553  auto cb_dx_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
554  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
555  auto cb_dy_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
556  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
557 
558 
559  if (count_x == 0 || count_y == 0)
560  {
561  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
562  return;
563  }
564  Eigen::Vector3d gradient_x (0, 0, 0);
565  Eigen::Vector3d gradient_y (0, 0, 0);
566 
567  auto cb_dx_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
568  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
569  auto cb_dy_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
570  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
571 
572 
573  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
574  double normal_length = normal_vector.squaredNorm ();
575  if (normal_length == 0.0f)
576  {
577  normal.getNormalVector3fMap ().setConstant (bad_point);
578  normal.curvature = bad_point;
579  return;
580  }
581 
582  normal_vector /= sqrt (normal_length);
583 
584  float nx = static_cast<float> (normal_vector [0]);
585  float ny = static_cast<float> (normal_vector [1]);
586  float nz = static_cast<float> (normal_vector [2]);
587 
588  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
589 
590  normal.normal_x = nx;
591  normal.normal_y = ny;
592  normal.normal_z = nz;
593  normal.curvature = bad_point;
594  return;
595  }
596  // ======================================================
597  if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
598  {
599  if (!init_depth_change_)
600  initAverageDepthChangeMethod ();
601 
602  int point_index_L_x = pos_x - rect_width_4_ - 1;
603  int point_index_L_y = pos_y;
604  int point_index_R_x = pos_x + rect_width_4_ + 1;
605  int point_index_R_y = pos_y;
606  int point_index_U_x = pos_x - 1;
607  int point_index_U_y = pos_y - rect_height_4_;
608  int point_index_D_x = pos_x + 1;
609  int point_index_D_y = pos_y + rect_height_4_;
610 
611  if (point_index_L_x < 0)
612  point_index_L_x = -point_index_L_x;
613  if (point_index_U_x < 0)
614  point_index_U_x = -point_index_U_x;
615  if (point_index_U_y < 0)
616  point_index_U_y = -point_index_U_y;
617 
618  if (point_index_R_x >= width)
619  point_index_R_x = width-(point_index_R_x-(width-1));
620  if (point_index_D_x >= width)
621  point_index_D_x = width-(point_index_D_x-(width-1));
622  if (point_index_D_y >= height)
623  point_index_D_y = height-(point_index_D_y-(height-1));
624 
625  const int start_x_L = pos_x - rect_width_2_;
626  const int start_y_L = pos_y - rect_height_4_;
627  const int end_x_L = start_x_L + rect_width_2_;
628  const int end_y_L = start_y_L + rect_height_2_;
629 
630  const int start_x_R = pos_x + 1;
631  const int start_y_R = pos_y - rect_height_4_;
632  const int end_x_R = start_x_R + rect_width_2_;
633  const int end_y_R = start_y_R + rect_height_2_;
634 
635  const int start_x_U = pos_x - rect_width_4_;
636  const int start_y_U = pos_y - rect_height_2_;
637  const int end_x_U = start_x_U + rect_width_2_;
638  const int end_y_U = start_y_U + rect_height_2_;
639 
640  const int start_x_D = pos_x - rect_width_4_;
641  const int start_y_D = pos_y + 1;
642  const int end_x_D = start_x_D + rect_width_2_;
643  const int end_y_D = start_y_D + rect_height_2_;
644 
645  unsigned count_L_z = 0;
646  unsigned count_R_z = 0;
647  unsigned count_U_z = 0;
648  unsigned count_D_z = 0;
649 
650  auto cb_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
651  sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
652  sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
653  sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
654  sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
655 
656  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
657  {
658  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
659  return;
660  }
661 
662  float mean_L_z = 0;
663  float mean_R_z = 0;
664  float mean_U_z = 0;
665  float mean_D_z = 0;
666 
667  auto cb_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
668  sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
669  sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
670  sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
671  sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
672 
673  mean_L_z /= float (count_L_z);
674  mean_R_z /= float (count_R_z);
675  mean_U_z /= float (count_U_z);
676  mean_D_z /= float (count_D_z);
677 
678 
679  PointInT pointL = (*input_)[point_index_L_y*width + point_index_L_x];
680  PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
681  PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
682  PointInT pointD = (*input_)[point_index_D_y*width + point_index_D_x];
683 
684  const float mean_x_z = mean_R_z - mean_L_z;
685  const float mean_y_z = mean_D_z - mean_U_z;
686 
687  const float mean_x_x = pointR.x - pointL.x;
688  const float mean_x_y = pointR.y - pointL.y;
689  const float mean_y_x = pointD.x - pointU.x;
690  const float mean_y_y = pointD.y - pointU.y;
691 
692  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
693  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
694  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
695 
696  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
697 
698  if (normal_length == 0.0f)
699  {
700  normal.getNormalVector3fMap ().setConstant (bad_point);
701  normal.curvature = bad_point;
702  return;
703  }
704 
705  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
706 
707  const float scale = 1.0f / std::sqrt (normal_length);
708 
709  normal.normal_x = normal_x * scale;
710  normal.normal_y = normal_y * scale;
711  normal.normal_z = normal_z * scale;
712  normal.curvature = bad_point;
713 
714  return;
715  }
716  // ========================================================
717  if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
718  {
719  PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
720  }
721 
722  normal.getNormalVector3fMap ().setConstant (bad_point);
723  normal.curvature = bad_point;
724  return;
725 }
726 
727 //////////////////////////////////////////////////////////////////////////////////////////
728 template <typename PointInT, typename PointOutT> void
730 {
731  output.sensor_origin_ = input_->sensor_origin_;
732  output.sensor_orientation_ = input_->sensor_orientation_;
733 
734  float bad_point = std::numeric_limits<float>::quiet_NaN ();
735 
736  // compute depth-change map
737  auto depthChangeMap = new unsigned char[input_->size ()];
738  std::fill_n(depthChangeMap, input_->size(), 255);
739 
740  unsigned index = 0;
741  for (unsigned int ri = 0; ri < input_->height-1; ++ri)
742  {
743  for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
744  {
745  index = ri * input_->width + ci;
746 
747  const float depth = input_->points [index].z;
748  const float depthR = input_->points [index + 1].z;
749  const float depthD = input_->points [index + input_->width].z;
750 
751  //const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs(depth)+1.0f))/(500.0f*0.001f);
752  const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
753 
754  if (std::fabs (depth - depthR) > depthDependendDepthChange
755  || !std::isfinite (depth) || !std::isfinite (depthR))
756  {
757  depthChangeMap[index] = 0;
758  depthChangeMap[index+1] = 0;
759  }
760  if (std::fabs (depth - depthD) > depthDependendDepthChange
761  || !std::isfinite (depth) || !std::isfinite (depthD))
762  {
763  depthChangeMap[index] = 0;
764  depthChangeMap[index + input_->width] = 0;
765  }
766  }
767  }
768 
769  // compute distance map
770  //float *distanceMap = new float[input_->size ()];
771  delete[] distance_map_;
772  distance_map_ = new float[input_->size ()];
773  float *distanceMap = distance_map_;
774  for (std::size_t index = 0; index < input_->size (); ++index)
775  {
776  if (depthChangeMap[index] == 0)
777  distanceMap[index] = 0.0f;
778  else
779  distanceMap[index] = static_cast<float> (input_->width + input_->height);
780  }
781 
782  // first pass
783  float* previous_row = distanceMap;
784  float* current_row = previous_row + input_->width;
785  for (std::size_t ri = 1; ri < input_->height; ++ri)
786  {
787  for (std::size_t ci = 1; ci < input_->width; ++ci)
788  {
789  const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
790  const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
791  const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
792  const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f;
793  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
794 
795  const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
796 
797  if (minValue < center)
798  current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
799  }
800  previous_row = current_row;
801  current_row += input_->width;
802  }
803 
804  float* next_row = distanceMap + input_->width * (input_->height - 1);
805  current_row = next_row - input_->width;
806  // second pass
807  for (int ri = input_->height-2; ri >= 0; --ri)
808  {
809  for (int ci = input_->width-2; ci >= 0; --ci)
810  {
811  const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
812  const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
813  const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
814  const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
815  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
816 
817  const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
818 
819  if (minValue < center)
820  current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
821  }
822  next_row = current_row;
823  current_row -= input_->width;
824  }
825 
826  if (indices_->size () < input_->size ())
827  computeFeaturePart (distanceMap, bad_point, output);
828  else
829  computeFeatureFull (distanceMap, bad_point, output);
830 
831  delete[] depthChangeMap;
832 }
833 
834 //////////////////////////////////////////////////////////////////////////////////////////
835 template <typename PointInT, typename PointOutT> void
837  const float &bad_point,
838  PointCloudOut &output)
839 {
840  unsigned index = 0;
841 
842  if (border_policy_ == BORDER_POLICY_IGNORE)
843  {
844  // Set all normals that we do not touch to NaN
845  // top and bottom borders
846  // That sets the output density to false!
847  output.is_dense = false;
848  unsigned border = int(normal_smoothing_size_);
849  PointOutT* vec1 = &output [0];
850  PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
851 
852  std::size_t count = border * input_->width;
853  for (std::size_t idx = 0; idx < count; ++idx)
854  {
855  vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
856  vec1 [idx].curvature = bad_point;
857  vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
858  vec2 [idx].curvature = bad_point;
859  }
860 
861  // left and right borders actually columns
862  vec1 = &output [border * input_->width];
863  vec2 = vec1 + input_->width - border;
864  for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
865  {
866  for (std::size_t ci = 0; ci < border; ++ci)
867  {
868  vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
869  vec1 [ci].curvature = bad_point;
870  vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
871  vec2 [ci].curvature = bad_point;
872  }
873  }
874 
875  if (use_depth_dependent_smoothing_)
876  {
877  index = border + input_->width * border;
878  unsigned skip = (border << 1);
879  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
880  {
881  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
882  {
883  index = ri * input_->width + ci;
884 
885  const float depth = (*input_)[index].z;
886  if (!std::isfinite (depth))
887  {
888  output[index].getNormalVector3fMap ().setConstant (bad_point);
889  output[index].curvature = bad_point;
890  continue;
891  }
892 
893  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
894 
895  if (smoothing > 2.0f)
896  {
897  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
898  computePointNormal (ci, ri, index, output [index]);
899  }
900  else
901  {
902  output[index].getNormalVector3fMap ().setConstant (bad_point);
903  output[index].curvature = bad_point;
904  }
905  }
906  }
907  }
908  else
909  {
910  float smoothing_constant = normal_smoothing_size_;
911 
912  index = border + input_->width * border;
913  unsigned skip = (border << 1);
914  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
915  {
916  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
917  {
918  index = ri * input_->width + ci;
919 
920  if (!std::isfinite ((*input_)[index].z))
921  {
922  output [index].getNormalVector3fMap ().setConstant (bad_point);
923  output [index].curvature = bad_point;
924  continue;
925  }
926 
927  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
928 
929  if (smoothing > 2.0f)
930  {
931  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
932  computePointNormal (ci, ri, index, output [index]);
933  }
934  else
935  {
936  output [index].getNormalVector3fMap ().setConstant (bad_point);
937  output [index].curvature = bad_point;
938  }
939  }
940  }
941  }
942  }
943  else if (border_policy_ == BORDER_POLICY_MIRROR)
944  {
945  output.is_dense = false;
946 
947  if (use_depth_dependent_smoothing_)
948  {
949  //index = 0;
950  //unsigned skip = 0;
951  //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
952  for (unsigned ri = 0; ri < input_->height; ++ri)
953  {
954  //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
955  for (unsigned ci = 0; ci < input_->width; ++ci)
956  {
957  index = ri * input_->width + ci;
958 
959  const float depth = (*input_)[index].z;
960  if (!std::isfinite (depth))
961  {
962  output[index].getNormalVector3fMap ().setConstant (bad_point);
963  output[index].curvature = bad_point;
964  continue;
965  }
966 
967  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
968 
969  if (smoothing > 2.0f)
970  {
971  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
972  computePointNormalMirror (ci, ri, index, output [index]);
973  }
974  else
975  {
976  output[index].getNormalVector3fMap ().setConstant (bad_point);
977  output[index].curvature = bad_point;
978  }
979  }
980  }
981  }
982  else
983  {
984  float smoothing_constant = normal_smoothing_size_;
985 
986  //index = border + input_->width * border;
987  //unsigned skip = (border << 1);
988  //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
989  for (unsigned ri = 0; ri < input_->height; ++ri)
990  {
991  //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
992  for (unsigned ci = 0; ci < input_->width; ++ci)
993  {
994  index = ri * input_->width + ci;
995 
996  if (!std::isfinite ((*input_)[index].z))
997  {
998  output [index].getNormalVector3fMap ().setConstant (bad_point);
999  output [index].curvature = bad_point;
1000  continue;
1001  }
1002 
1003  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
1004 
1005  if (smoothing > 2.0f)
1006  {
1007  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1008  computePointNormalMirror (ci, ri, index, output [index]);
1009  }
1010  else
1011  {
1012  output [index].getNormalVector3fMap ().setConstant (bad_point);
1013  output [index].curvature = bad_point;
1014  }
1015  }
1016  }
1017  }
1018  }
1019 }
1020 
1021 ///////////////////////////////////////////////////////////////////////////////////////////
1022 template <typename PointInT, typename PointOutT> void
1024  const float &bad_point,
1025  PointCloudOut &output)
1026 {
1027  if (border_policy_ == BORDER_POLICY_IGNORE)
1028  {
1029  output.is_dense = false;
1030  unsigned border = int(normal_smoothing_size_);
1031  unsigned bottom = input_->height > border ? input_->height - border : 0;
1032  unsigned right = input_->width > border ? input_->width - border : 0;
1033  if (use_depth_dependent_smoothing_)
1034  {
1035  // Iterating over the entire index vector
1036  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1037  {
1038  unsigned pt_index = (*indices_)[idx];
1039  unsigned u = pt_index % input_->width;
1040  unsigned v = pt_index / input_->width;
1041  if (v < border || v > bottom)
1042  {
1043  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1044  output[idx].curvature = bad_point;
1045  continue;
1046  }
1047 
1048  if (u < border || u > right)
1049  {
1050  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1051  output[idx].curvature = bad_point;
1052  continue;
1053  }
1054 
1055  const float depth = (*input_)[pt_index].z;
1056  if (!std::isfinite (depth))
1057  {
1058  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1059  output[idx].curvature = bad_point;
1060  continue;
1061  }
1062 
1063  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1064  if (smoothing > 2.0f)
1065  {
1066  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1067  computePointNormal (u, v, pt_index, output [idx]);
1068  }
1069  else
1070  {
1071  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1072  output[idx].curvature = bad_point;
1073  }
1074  }
1075  }
1076  else
1077  {
1078  float smoothing_constant = normal_smoothing_size_;
1079  // Iterating over the entire index vector
1080  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1081  {
1082  unsigned pt_index = (*indices_)[idx];
1083  unsigned u = pt_index % input_->width;
1084  unsigned v = pt_index / input_->width;
1085  if (v < border || v > bottom)
1086  {
1087  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1088  output[idx].curvature = bad_point;
1089  continue;
1090  }
1091 
1092  if (u < border || u > right)
1093  {
1094  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1095  output[idx].curvature = bad_point;
1096  continue;
1097  }
1098 
1099  if (!std::isfinite ((*input_)[pt_index].z))
1100  {
1101  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1102  output [idx].curvature = bad_point;
1103  continue;
1104  }
1105 
1106  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1107 
1108  if (smoothing > 2.0f)
1109  {
1110  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1111  computePointNormal (u, v, pt_index, output [idx]);
1112  }
1113  else
1114  {
1115  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1116  output [idx].curvature = bad_point;
1117  }
1118  }
1119  }
1120  }// border_policy_ == BORDER_POLICY_IGNORE
1121  else if (border_policy_ == BORDER_POLICY_MIRROR)
1122  {
1123  output.is_dense = false;
1124 
1125  if (use_depth_dependent_smoothing_)
1126  {
1127  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1128  {
1129  unsigned pt_index = (*indices_)[idx];
1130  unsigned u = pt_index % input_->width;
1131  unsigned v = pt_index / input_->width;
1132 
1133  const float depth = (*input_)[pt_index].z;
1134  if (!std::isfinite (depth))
1135  {
1136  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1137  output[idx].curvature = bad_point;
1138  continue;
1139  }
1140 
1141  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1142 
1143  if (smoothing > 2.0f)
1144  {
1145  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1146  computePointNormalMirror (u, v, pt_index, output [idx]);
1147  }
1148  else
1149  {
1150  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1151  output[idx].curvature = bad_point;
1152  }
1153  }
1154  }
1155  else
1156  {
1157  float smoothing_constant = normal_smoothing_size_;
1158  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1159  {
1160  unsigned pt_index = (*indices_)[idx];
1161  unsigned u = pt_index % input_->width;
1162  unsigned v = pt_index / input_->width;
1163 
1164  if (!std::isfinite ((*input_)[pt_index].z))
1165  {
1166  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1167  output [idx].curvature = bad_point;
1168  continue;
1169  }
1170 
1171  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1172 
1173  if (smoothing > 2.0f)
1174  {
1175  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1176  computePointNormalMirror (u, v, pt_index, output [idx]);
1177  }
1178  else
1179  {
1180  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1181  output [idx].curvature = bad_point;
1182  }
1183  }
1184  }
1185  } // border_policy_ == BORDER_POLICY_MIRROR
1186 }
1187 
1188 //////////////////////////////////////////////////////////////////////////////////////////
1189 template <typename PointInT, typename PointOutT> bool
1191 {
1192  if (!input_->isOrganized ())
1193  {
1194  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1195  return (false);
1196  }
1197  return (Feature<PointInT, PointOutT>::initCompute ());
1198 }
1199 
1200 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
1201 
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
Determines an integral image representation for a given organized data array.
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Surface normal estimation on organized data using integral images.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
~IntegralImageNormalEstimation() override
Destructor.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
void computePointNormalMirror(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position with mirroring for border handling.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
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...
Definition: eigen.hpp:296
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.
Definition: normal_3d.h:122
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...
Definition: normal_3d.h:61