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 
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
void useSensorOriginAsViewPoint()
sets whether the sensor origin or a user given viewpoint should be used.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
std::string feature_name_
The feature name.
Definition: feature.h:222
Feature represents the base feature class.
Definition: feature.h:105
float * getDistanceMap()
Returns a pointer to the distance map which was computed internally.
void setBorderPolicy(const BorderPolicy border_policy)
Sets the policy for handling borders.
Surface normal estimation on organized data using integral images.
Feature< PointInT, PointOutT >::PointCloudIn PointCloudIn
void computeFeature(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.
virtual ~IntegralImageNormalEstimation()
Destructor.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
KdTreePtr tree_
A pointer to the spatial search object.
Definition: feature.h:233
Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:242
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. ...
BorderPolicy
Different types of border handling.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
boost::shared_ptr< IntegralImageNormalEstimation< PointInT, PointOutT > > Ptr
boost::shared_ptr< const IntegralImageNormalEstimation< PointInT, PointOutT > > ConstPtr
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:146
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
NormalEstimationMethod
Different normal estimation methods.