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