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