Point Cloud Library (PCL)  1.13.0-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 = shared_ptr<RangeImageBorderExtractor>;
59  using ConstPtr = 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
122 
123  /** \brief Get the 2D directions in the range image from the border directions - probably mainly useful for
124  * visualization
125  */
126  float*
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*
134 
135  /** Overwrite the compute function of the base class */
136  void
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 
167  ShadowBorderIndices**
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_;
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
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
285 
286  /** \brief Extract local plane information in every point (see getSurfaceStructure ()) */
287  void
289 
290  /** \brief Get images representing the probability that the corresponding pixels are borders in that direction
291  * (see getBorderScores... ())
292  */
293  void
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
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
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
343 
344  /** \brief Apply a blur to the surface change images */
345  void
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
Feature represents the base feature class.
Definition: feature.h:107
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
Extract obstacle borders from range images, meaning positions where there is a transition from foregr...
float * updatedScoresAccordingToNeighborValues(const float *border_scores) const
For all pixels, returns a new score that is >= the original value, based on the neighbors values.
float getNormalBasedBorderScore(const LocalSurface &local_surface, int x, int y, int offset_x, int offset_y) const
Calculate a border score based on how much the neighbor is away from the local surface plane.
void extractBorderScoreImages()
Get images representing the probability that the corresponding pixels are borders in that direction (...
void updateScoresAccordingToNeighborValues()
Replace all border score values with updates according to updatedScoreAccordingToNeighborValues.
float * getAnglesImageForSurfaceChangeDirections()
Get the 2D directions in the range image from the surface change directions - probably mainly useful ...
float * getAnglesImageForBorderDirections()
Get the 2D directions in the range image from the border directions - probably mainly useful for visu...
void compute(PointCloudOut &output)
Overwrite the compute function of the base class.
void setRangeImage(const RangeImage *range_image)
Provide a pointer to the range image.
void extractLocalSurfaceStructure()
Extract local plane information in every point (see getSurfaceStructure ())
ShadowBorderIndices ** getShadowBorderInformations()
ShadowBorderIndices ** shadow_border_informations_
RangeImageBorderExtractor(const RangeImage *range_image=nullptr)
Constructor.
void computeFeature(PointCloudOut &output) override
Implementation of abstract derived function.
void calculateSurfaceChanges()
Uses either the border or principal curvature to define a score how much the surface changes in a poi...
shared_ptr< const RangeImageBorderExtractor > ConstPtr
~RangeImageBorderExtractor() override
Destructor.
void blurSurfaceChanges()
Apply a blur to the surface change images.
void calculateBorderDirections()
Call calculateBorderDirection for every point and average the result over parameters_....
void classifyBorders()
Classify the pixels in the range image according to the different classes defined below in enum Borde...
void findAndEvaluateShadowBorders()
Find the best corresponding shadow border and lower score according to the shadow borders value.
void clearData()
Erase all data calculated for the current range image.
shared_ptr< RangeImageBorderExtractor > Ptr
RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where...
Definition: range_image.h:55
Defines all the PCL implemented PointT point type structures.
std::bitset< 32 > BorderTraits
Data type to store extended information about a transition from foreground to backgroundSpecification...
Definition: point_types.h:307
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A structure to store if a point in a range image lies on a border between an obstacle and the backgro...
Stores some information extracted from the neighborhood of a point.
Stores the indices of the shadow border corresponding to obstacle borders.