Point Cloud Library (PCL)  1.7.1
integral_image_normal.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-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 
39 #ifndef PCL_INTEGRALIMAGE_BASED_NORMAL_ESTIMATOR_H_
40 #define PCL_INTEGRALIMAGE_BASED_NORMAL_ESTIMATOR_H_
41 
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
44 #include <pcl/features/feature.h>
45 #include <pcl/features/integral_image2D.h>
46 
47 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
48 #pragma GCC diagnostic ignored "-Weffc++"
49 #endif
50 namespace pcl
51 {
52  /** \brief Surface normal estimation on organized data using integral images.
53  * \author Stefan Holzer
54  */
55  template <typename PointInT, typename PointOutT>
56  class IntegralImageNormalEstimation: public Feature<PointInT, PointOutT>
57  {
62 
63  public:
64  typedef boost::shared_ptr<IntegralImageNormalEstimation<PointInT, PointOutT> > Ptr;
65  typedef boost::shared_ptr<const IntegralImageNormalEstimation<PointInT, PointOutT> > ConstPtr;
66 
67  /** \brief Different types of border handling. */
69  {
72  };
73 
74  /** \brief Different normal estimation methods.
75  * <ul>
76  * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point
77  * from the covariance matrix of its local neighborhood.</li>
78  * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of
79  * horizontal and vertical 3D gradients and computes the normals using the cross-product between these
80  * two gradients.
81  * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals
82  * from the average depth changes.
83  * </ul>
84  */
86  {
91  };
92 
95 
96  /** \brief Constructor */
98  : normal_estimation_method_(AVERAGE_3D_GRADIENT)
99  , border_policy_ (BORDER_POLICY_IGNORE)
100  , rect_width_ (0), rect_width_2_ (0), rect_width_4_ (0)
101  , rect_height_ (0), rect_height_2_ (0), rect_height_4_ (0)
102  , distance_threshold_ (0)
103  , integral_image_DX_ (false)
104  , integral_image_DY_ (false)
105  , integral_image_depth_ (false)
106  , integral_image_XYZ_ (true)
107  , diff_x_ (NULL)
108  , diff_y_ (NULL)
109  , depth_data_ (NULL)
110  , distance_map_ (NULL)
111  , use_depth_dependent_smoothing_ (false)
112  , max_depth_change_factor_ (20.0f*0.001f)
113  , normal_smoothing_size_ (10.0f)
114  , init_covariance_matrix_ (false)
115  , init_average_3d_gradient_ (false)
116  , init_simple_3d_gradient_ (false)
117  , init_depth_change_ (false)
118  , vpx_ (0.0f)
119  , vpy_ (0.0f)
120  , vpz_ (0.0f)
121  , use_sensor_origin_ (true)
122  {
123  feature_name_ = "IntegralImagesNormalEstimation";
124  tree_.reset ();
125  k_ = 1;
126  }
127 
128  /** \brief Destructor **/
130 
131  /** \brief Set the regions size which is considered for normal estimation.
132  * \param[in] width the width of the search rectangle
133  * \param[in] height the height of the search rectangle
134  */
135  void
136  setRectSize (const int width, const int height);
137 
138  /** \brief Sets the policy for handling borders.
139  * \param[in] border_policy the border policy.
140  */
141  void
142  setBorderPolicy (const BorderPolicy border_policy)
143  {
144  border_policy_ = border_policy;
145  }
146 
147  /** \brief Computes the normal at the specified position.
148  * \param[in] pos_x x position (pixel)
149  * \param[in] pos_y y position (pixel)
150  * \param[in] point_index the position index of the point
151  * \param[out] normal the output estimated normal
152  */
153  void
154  computePointNormal (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal);
155 
156  /** \brief Computes the normal at the specified position with mirroring for border handling.
157  * \param[in] pos_x x position (pixel)
158  * \param[in] pos_y y position (pixel)
159  * \param[in] point_index the position index of the point
160  * \param[out] normal the output estimated normal
161  */
162  void
163  computePointNormalMirror (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal);
164 
165  /** \brief The depth change threshold for computing object borders
166  * \param[in] max_depth_change_factor the depth change threshold for computing object borders based on
167  * depth changes
168  */
169  void
170  setMaxDepthChangeFactor (float max_depth_change_factor)
171  {
172  max_depth_change_factor_ = max_depth_change_factor;
173  }
174 
175  /** \brief Set the normal smoothing size
176  * \param[in] normal_smoothing_size factor which influences the size of the area used to smooth normals
177  * (depth dependent if useDepthDependentSmoothing is true)
178  */
179  void
180  setNormalSmoothingSize (float normal_smoothing_size)
181  {
182  if (normal_smoothing_size <= 0)
183  {
184  PCL_ERROR ("[pcl::%s::setNormalSmoothingSize] Invalid normal smoothing size given! (%f). Allowed ranges are: 0 < N. Defaulting to %f.\n",
185  feature_name_.c_str (), normal_smoothing_size, normal_smoothing_size_);
186  return;
187  }
188  normal_smoothing_size_ = normal_smoothing_size;
189  }
190 
191  /** \brief Set the normal estimation method. The current implemented algorithms are:
192  * <ul>
193  * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point
194  * from the covariance matrix of its local neighborhood.</li>
195  * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of
196  * horizontal and vertical 3D gradients and computes the normals using the cross-product between these
197  * two gradients.
198  * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals
199  * from the average depth changes.
200  * </ul>
201  * \param[in] normal_estimation_method the method used for normal estimation
202  */
203  void
205  {
206  normal_estimation_method_ = normal_estimation_method;
207  }
208 
209  /** \brief Set whether to use depth depending smoothing or not
210  * \param[in] use_depth_dependent_smoothing decides whether the smoothing is depth dependent
211  */
212  void
213  setDepthDependentSmoothing (bool use_depth_dependent_smoothing)
214  {
215  use_depth_dependent_smoothing_ = use_depth_dependent_smoothing;
216  }
217 
218  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
219  * \param[in] cloud the const boost shared pointer to a PointCloud message
220  */
221  virtual inline void
222  setInputCloud (const typename PointCloudIn::ConstPtr &cloud)
223  {
224  input_ = cloud;
225  if (!cloud->isOrganized ())
226  {
227  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).\n");
228  return;
229  }
230 
231  init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
232 
233  if (use_sensor_origin_)
234  {
235  vpx_ = input_->sensor_origin_.coeff (0);
236  vpy_ = input_->sensor_origin_.coeff (1);
237  vpz_ = input_->sensor_origin_.coeff (2);
238  }
239 
240  // Initialize the correct data structure based on the normal estimation method chosen
241  initData ();
242  }
243 
244  /** \brief Returns a pointer to the distance map which was computed internally
245  */
246  inline float*
248  {
249  return (distance_map_);
250  }
251 
252  /** \brief Set the viewpoint.
253  * \param vpx the X coordinate of the viewpoint
254  * \param vpy the Y coordinate of the viewpoint
255  * \param vpz the Z coordinate of the viewpoint
256  */
257  inline void
258  setViewPoint (float vpx, float vpy, float vpz)
259  {
260  vpx_ = vpx;
261  vpy_ = vpy;
262  vpz_ = vpz;
263  use_sensor_origin_ = false;
264  }
265 
266  /** \brief Get the viewpoint.
267  * \param [out] vpx x-coordinate of the view point
268  * \param [out] vpy y-coordinate of the view point
269  * \param [out] vpz z-coordinate of the view point
270  * \note this method returns the currently used viewpoint for normal flipping.
271  * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates.
272  * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0)
273  */
274  inline void
275  getViewPoint (float &vpx, float &vpy, float &vpz)
276  {
277  vpx = vpx_;
278  vpy = vpy_;
279  vpz = vpz_;
280  }
281 
282  /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the
283  * normal estimation method uses the sensor origin of the input cloud.
284  * to use a user defined view point, use the method setViewPoint
285  */
286  inline void
288  {
289  use_sensor_origin_ = true;
290  if (input_)
291  {
292  vpx_ = input_->sensor_origin_.coeff (0);
293  vpy_ = input_->sensor_origin_.coeff (1);
294  vpz_ = input_->sensor_origin_.coeff (2);
295  }
296  else
297  {
298  vpx_ = 0;
299  vpy_ = 0;
300  vpz_ = 0;
301  }
302  }
303 
304  protected:
305 
306  /** \brief Computes the normal for the complete cloud.
307  * \param[out] output the resultant normals
308  */
309  void
310  computeFeature (PointCloudOut &output);
311 
312  /** \brief Initialize the data structures, based on the normal estimation method chosen. */
313  void
314  initData ();
315 
316  private:
317 
318  /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
319  * \param point a given point
320  * \param vp_x the X coordinate of the viewpoint
321  * \param vp_y the X coordinate of the viewpoint
322  * \param vp_z the X coordinate of the viewpoint
323  * \param nx the resultant X component of the plane normal
324  * \param ny the resultant Y component of the plane normal
325  * \param nz the resultant Z component of the plane normal
326  * \ingroup features
327  */
328  inline void
329  flipNormalTowardsViewpoint (const PointInT &point,
330  float vp_x, float vp_y, float vp_z,
331  float &nx, float &ny, float &nz)
332  {
333  // See if we need to flip any plane normals
334  vp_x -= point.x;
335  vp_y -= point.y;
336  vp_z -= point.z;
337 
338  // Dot product between the (viewpoint - point) and the plane normal
339  float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
340 
341  // Flip the plane normal
342  if (cos_theta < 0)
343  {
344  nx *= -1;
345  ny *= -1;
346  nz *= -1;
347  }
348  }
349 
350  /** \brief The normal estimation method to use. Currently, 3 implementations are provided:
351  *
352  * - COVARIANCE_MATRIX
353  * - AVERAGE_3D_GRADIENT
354  * - AVERAGE_DEPTH_CHANGE
355  */
356  NormalEstimationMethod normal_estimation_method_;
357 
358  /** \brief The policy for handling borders. */
359  BorderPolicy border_policy_;
360 
361  /** The width of the neighborhood region used for computing the normal. */
362  int rect_width_;
363  int rect_width_2_;
364  int rect_width_4_;
365  /** The height of the neighborhood region used for computing the normal. */
366  int rect_height_;
367  int rect_height_2_;
368  int rect_height_4_;
369 
370  /** the threshold used to detect depth discontinuities */
371  float distance_threshold_;
372 
373  /** integral image in x-direction */
374  IntegralImage2D<float, 3> integral_image_DX_;
375  /** integral image in y-direction */
376  IntegralImage2D<float, 3> integral_image_DY_;
377  /** integral image */
378  IntegralImage2D<float, 1> integral_image_depth_;
379  /** integral image xyz */
380  IntegralImage2D<float, 3> integral_image_XYZ_;
381 
382  /** derivatives in x-direction */
383  float *diff_x_;
384  /** derivatives in y-direction */
385  float *diff_y_;
386 
387  /** depth data */
388  float *depth_data_;
389 
390  /** distance map */
391  float *distance_map_;
392 
393  /** \brief Smooth data based on depth (true/false). */
394  bool use_depth_dependent_smoothing_;
395 
396  /** \brief Threshold for detecting depth discontinuities */
397  float max_depth_change_factor_;
398 
399  /** \brief */
400  float normal_smoothing_size_;
401 
402  /** \brief True when a dataset has been received and the covariance_matrix data has been initialized. */
403  bool init_covariance_matrix_;
404 
405  /** \brief True when a dataset has been received and the average 3d gradient data has been initialized. */
406  bool init_average_3d_gradient_;
407 
408  /** \brief True when a dataset has been received and the simple 3d gradient data has been initialized. */
409  bool init_simple_3d_gradient_;
410 
411  /** \brief True when a dataset has been received and the depth change data has been initialized. */
412  bool init_depth_change_;
413 
414  /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit
415  * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */
416  float vpx_, vpy_, vpz_;
417 
418  /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/
419  bool use_sensor_origin_;
420 
421  /** \brief This method should get called before starting the actual computation. */
422  bool
423  initCompute ();
424 
425  /** \brief Internal initialization method for COVARIANCE_MATRIX estimation. */
426  void
427  initCovarianceMatrixMethod ();
428 
429  /** \brief Internal initialization method for AVERAGE_3D_GRADIENT estimation. */
430  void
431  initAverage3DGradientMethod ();
432 
433  /** \brief Internal initialization method for AVERAGE_DEPTH_CHANGE estimation. */
434  void
435  initAverageDepthChangeMethod ();
436 
437  /** \brief Internal initialization method for SIMPLE_3D_GRADIENT estimation. */
438  void
439  initSimple3DGradientMethod ();
440 
441  public:
442  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
443  };
444 }
445 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
446 #pragma GCC diagnostic warning "-Weffc++"
447 #endif
448 
449 #ifdef PCL_NO_PRECOMPILE
450 #include <pcl/features/impl/integral_image_normal.hpp>
451 #endif
452 
453 #endif
454