Point Cloud Library (PCL)  1.7.0
range_image_border_extractor.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, 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 #ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_
39 #define PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_
40 
41 #include <pcl/point_types.h>
42 #include <pcl/features/feature.h>
43 
44 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
45 #pragma GCC diagnostic ignored "-Weffc++"
46 #endif
47 namespace pcl
48 {
49  // FORWARD DECLARATIONS:
50  class RangeImage;
51  template <typename PointType>
52  class PointCloud;
53 
54  /** \brief @b Extract obstacle borders from range images, meaning positions where there is a transition from foreground
55  * to background.
56  * \author Bastian Steder
57  * \ingroup features
58  */
59  class PCL_EXPORTS RangeImageBorderExtractor : public Feature<PointWithRange,BorderDescription>
60  {
61  public:
62  typedef boost::shared_ptr<RangeImageBorderExtractor> Ptr;
63  typedef boost::shared_ptr<const RangeImageBorderExtractor> ConstPtr;
64  // =====TYPEDEFS=====
66 
67  // =====PUBLIC STRUCTS=====
68  //! Stores some information extracted from the neighborhood of a point
69  struct LocalSurface
70  {
72  normal (), neighborhood_mean (), eigen_values (), normal_no_jumps (),
73  neighborhood_mean_no_jumps (), eigen_values_no_jumps (), max_neighbor_distance_squared () {}
74 
75  Eigen::Vector3f normal;
76  Eigen::Vector3f neighborhood_mean;
77  Eigen::Vector3f eigen_values;
78  Eigen::Vector3f normal_no_jumps;
79  Eigen::Vector3f neighborhood_mean_no_jumps;
80  Eigen::Vector3f eigen_values_no_jumps;
82  };
83 
84  //! Stores the indices of the shadow border corresponding to obstacle borders
86  {
87  ShadowBorderIndices () : left (-1), right (-1), top (-1), bottom (-1) {}
88  int left, right, top, bottom;
89  };
90 
91  //! Parameters used in this class
92  struct Parameters
93  {
94  Parameters () : max_no_of_threads(1), pixel_radius_borders (3), pixel_radius_plane_extraction (2), pixel_radius_border_direction (2),
95  minimum_border_probability (0.8f), pixel_radius_principal_curvature (2) {}
102  };
103 
104  // =====STATIC METHODS=====
105  /** \brief Take the information from BorderTraits to calculate the local direction of the border
106  * \param border_traits contains the information needed to calculate the border angle
107  */
108  static inline float
109  getObstacleBorderAngle (const BorderTraits& border_traits);
110 
111  // =====CONSTRUCTOR & DESTRUCTOR=====
112  /** Constructor */
113  RangeImageBorderExtractor (const RangeImage* range_image=NULL);
114  /** Destructor */
115  virtual ~RangeImageBorderExtractor ();
116 
117  // =====METHODS=====
118  /** \brief Provide a pointer to the range image
119  * \param range_image a pointer to the range_image
120  */
121  void
122  setRangeImage (const RangeImage* range_image);
123 
124  /** \brief Erase all data calculated for the current range image */
125  void
126  clearData ();
127 
128  /** \brief Get the 2D directions in the range image from the border directions - probably mainly useful for
129  * visualization
130  */
131  float*
132  getAnglesImageForBorderDirections ();
133 
134  /** \brief Get the 2D directions in the range image from the surface change directions - probably mainly useful for
135  * visualization
136  */
137  float*
138  getAnglesImageForSurfaceChangeDirections ();
139 
140  /** Overwrite the compute function of the base class */
141  void
142  compute (PointCloudOut& output);
143 
144  // =====GETTER=====
145  Parameters&
146  getParameters () { return (parameters_); }
147 
148  bool
149  hasRangeImage () const { return range_image_ != NULL; }
150 
151  const RangeImage&
152  getRangeImage () const { return *range_image_; }
153 
154  float*
155  getBorderScoresLeft () { extractBorderScoreImages (); return border_scores_left_; }
156 
157  float*
158  getBorderScoresRight () { extractBorderScoreImages (); return border_scores_right_; }
159 
160  float*
161  getBorderScoresTop () { extractBorderScoreImages (); return border_scores_top_; }
162 
163  float*
164  getBorderScoresBottom () { extractBorderScoreImages (); return border_scores_bottom_; }
165 
166  LocalSurface**
167  getSurfaceStructure () { extractLocalSurfaceStructure (); return surface_structure_; }
168 
169  PointCloudOut&
170  getBorderDescriptions () { classifyBorders (); return *border_descriptions_; }
171 
172  ShadowBorderIndices**
173  getShadowBorderInformations () { findAndEvaluateShadowBorders (); return shadow_border_informations_; }
174 
175  Eigen::Vector3f**
176  getBorderDirections () { calculateBorderDirections (); return border_directions_; }
177 
178  float*
179  getSurfaceChangeScores () { calculateSurfaceChanges (); return surface_change_scores_; }
180 
181  Eigen::Vector3f*
182  getSurfaceChangeDirections () { calculateSurfaceChanges (); return surface_change_directions_; }
183 
184 
185  protected:
186  // =====PROTECTED MEMBER VARIABLES=====
190  float* border_scores_left_, * border_scores_right_, * border_scores_top_, * border_scores_bottom_;
194  Eigen::Vector3f** border_directions_;
195 
197  Eigen::Vector3f* surface_change_directions_;
198 
199 
200  // =====PROTECTED METHODS=====
201  /** \brief Calculate a border score based on how distant the neighbor is, compared to the closest neighbors
202  * /param local_surface
203  * /param x
204  * /param y
205  * /param offset_x
206  * /param offset_y
207  * /param pixel_radius (defaults to 1)
208  * /return the resulting border score
209  */
210  inline float
211  getNeighborDistanceChangeScore (const LocalSurface& local_surface, int x, int y,
212  int offset_x, int offset_y, int pixel_radius=1) const;
213 
214  /** \brief Calculate a border score based on how much the neighbor is away from the local surface plane
215  * \param local_surface
216  * \param x
217  * \param y
218  * \param offset_x
219  * \param offset_y
220  * \return the resulting border score
221  */
222  inline float
223  getNormalBasedBorderScore (const LocalSurface& local_surface, int x, int y,
224  int offset_x, int offset_y) const;
225 
226  /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value
227  * \param x
228  * \param y
229  * \param offset_x
230  * \param offset_y
231  * \param border_scores
232  * \param border_scores_other_direction
233  * \param shadow_border_idx
234  * \return
235  */
236  inline bool
237  changeScoreAccordingToShadowBorderValue (int x, int y, int offset_x, int offset_y, float* border_scores,
238  float* border_scores_other_direction, int& shadow_border_idx) const;
239 
240  /** \brief Returns a new score for the given pixel that is >= the original value, based on the neighbors values
241  * \param x the x-coordinate of the input pixel
242  * \param y the y-coordinate of the input pixel
243  * \param border_scores the input border scores
244  * \return the resulting updated border score
245  */
246  inline float
247  updatedScoreAccordingToNeighborValues (int x, int y, const float* border_scores) const;
248 
249  /** \brief For all pixels, returns a new score that is >= the original value, based on the neighbors values
250  * \param border_scores the input border scores
251  * \return a pointer to the resulting array of updated scores
252  */
253  float*
254  updatedScoresAccordingToNeighborValues (const float* border_scores) const;
255 
256  /** \brief Replace all border score values with updates according to \a updatedScoreAccordingToNeighborValues */
257  void
258  updateScoresAccordingToNeighborValues ();
259 
260  /** \brief Check if a potential border point has a corresponding shadow border
261  * \param x the x-coordinate of the input point
262  * \param y the y-coordinate of the input point
263  * \param offset_x
264  * \param offset_y
265  * \param border_scores_left
266  * \param border_scores_right
267  * \param shadow_border_idx
268  * \return a boolean value indicating whether or not the point has a corresponding shadow border
269  */
270  inline bool
271  checkPotentialBorder (int x, int y, int offset_x, int offset_y, float* border_scores_left,
272  float* border_scores_right, int& shadow_border_idx) const;
273 
274  /** \brief Check if a potential border point is a maximum regarding the border score
275  * \param x the x-coordinate of the input point
276  * \param y the y-coordinate of the input point
277  * \param offset_x
278  * \param offset_y
279  * \param border_scores
280  * \param shadow_border_idx
281  * \result a boolean value indicating whether or not the point is a maximum
282  */
283  inline bool
284  checkIfMaximum (int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const;
285 
286  /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value */
287  void
288  findAndEvaluateShadowBorders ();
289 
290  /** \brief Extract local plane information in every point (see getSurfaceStructure ()) */
291  void
292  extractLocalSurfaceStructure ();
293 
294  /** \brief Get images representing the probability that the corresponding pixels are borders in that direction
295  * (see getBorderScores... ())
296  */
297  void
298  extractBorderScoreImages ();
299 
300  /** \brief Classify the pixels in the range image according to the different classes defined below in
301  * enum BorderClass. minImpactAngle (in radians) defines how flat the angle at which a surface was seen can be.
302  */
303  void
304  classifyBorders ();
305 
306  /** \brief Calculate the 3D direction of the border just using the border traits at this position (facing away from
307  * the obstacle)
308  * \param x the x-coordinate of the input position
309  * \param y the y-coordinate of the input position
310  */
311  inline void
312  calculateBorderDirection (int x, int y);
313 
314  /** \brief Call \a calculateBorderDirection for every point and average the result over
315  * parameters_.pixel_radius_border_direction
316  */
317  void
318  calculateBorderDirections ();
319 
320  /** \brief Calculate a 3d direction from a border point by projecting the direction in the range image - returns
321  * false if direction could not be calculated
322  * \param border_description
323  * \param direction
324  * \param local_surface
325  * \return a boolean value indicating whether or not a direction could be calculated
326  */
327  inline bool
328  get3dDirection (const BorderDescription& border_description, Eigen::Vector3f& direction,
329  const LocalSurface* local_surface=NULL);
330 
331  /** \brief Calculate the main principal curvature (the largest eigenvalue and corresponding eigenvector for the
332  * normals in the area) in the given point
333  * \param x the x-coordinate of the input point
334  * \param y the y-coordinate of the input point
335  * \param radius the pixel radius that is used to find neighboring points
336  * \param magnitude the resulting magnitude
337  * \param main_direction the resulting direction
338  */
339  inline bool
340  calculateMainPrincipalCurvature (int x, int y, int radius, float& magnitude,
341  Eigen::Vector3f& main_direction) const;
342 
343  /** \brief Uses either the border or principal curvature to define a score how much the surface changes in a point
344  (1 for a border) and what the main direction of that change is */
345  void
346  calculateSurfaceChanges ();
347 
348  /** \brief Apply a blur to the surface change images */
349  void
350  blurSurfaceChanges ();
351 
352  /** \brief Implementation of abstract derived function */
353  virtual void
354  computeFeature (PointCloudOut &output);
355  };
356 } // namespace end
357 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
358 #pragma GCC diagnostic warning "-Weffc++"
359 #endif
360 
361 #include <pcl/features/impl/range_image_border_extractor.hpp> // Definitions of templated and inline functions
362 
363 #endif //#ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_