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