Point Cloud Library (PCL)  1.14.1-dev
range_image.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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/memory.h>
41 #include <pcl/point_cloud.h>
42 #include <pcl/pcl_macros.h>
43 #include <pcl/point_types.h>
44 #include <pcl/common/angles.h> // for deg2rad
45 namespace pcl { struct PCLPointCloud2; }
46 
47 namespace pcl
48 {
49  /** \brief RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where
50  * a 3D scene was captured from a specific view point.
51  * \author Bastian Steder
52  * \ingroup range_image
53  */
54  class RangeImage : public pcl::PointCloud<PointWithRange>
55  {
56  public:
57  // =====TYPEDEFS=====
59  using VectorOfEigenVector3f = std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >;
60  using Ptr = shared_ptr<RangeImage>;
61  using ConstPtr = shared_ptr<const RangeImage>;
62 
64  {
66  LASER_FRAME = 1
67  };
68 
69 
70  // =====CONSTRUCTOR & DESTRUCTOR=====
71  /** Constructor */
73  /** Destructor */
75 
76  // =====STATIC VARIABLES=====
77  /** The maximum number of openmp threads that can be used in this class */
78  static int max_no_of_threads;
79 
80  // =====STATIC METHODS=====
81  /** \brief Get the size of a certain area when seen from the given pose
82  * \param viewer_pose an affine matrix defining the pose of the viewer
83  * \param center the center of the area
84  * \param radius the radius of the area
85  * \return the size of the area as viewed according to \a viewer_pose
86  */
87  static inline float
88  getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center,
89  float radius);
90 
91  /** \brief Get Eigen::Vector3f from PointWithRange
92  * \param point the input point
93  * \return an Eigen::Vector3f representation of the input point
94  */
95  static inline Eigen::Vector3f
96  getEigenVector3f (const PointWithRange& point);
97 
98  /** \brief Get the transformation that transforms the given coordinate frame into CAMERA_FRAME
99  * \param coordinate_frame the input coordinate frame
100  * \param transformation the resulting transformation that warps \a coordinate_frame into CAMERA_FRAME
101  */
102  PCL_EXPORTS static void
104  Eigen::Affine3f& transformation);
105 
106  /** \brief Get the average viewpoint of a point cloud where each point carries viewpoint information as
107  * vp_x, vp_y, vp_z
108  * \param point_cloud the input point cloud
109  * \return the average viewpoint (as an Eigen::Vector3f)
110  */
111  template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f
112  getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
113 
114  /** \brief Check if the provided data includes far ranges and add them to far_ranges
115  * \param point_cloud_data a PCLPointCloud2 message containing the input cloud
116  * \param far_ranges the resulting cloud containing those points with far ranges
117  */
118  PCL_EXPORTS static void
120 
121  // =====METHODS=====
122  /** \brief Get a boost shared pointer of a copy of this */
123  inline Ptr
124  makeShared () { return Ptr (new RangeImage (*this)); }
125 
126  /** \brief Reset all values to an empty range image */
127  PCL_EXPORTS void
128  reset ();
129 
130  /** \brief Create the depth image from a point cloud
131  * \param point_cloud the input point cloud
132  * \param angular_resolution the angular difference (in radians) between the individual pixels in the image
133  * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
134  * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
135  * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
136  * Eigen::Affine3f::Identity () )
137  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
138  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
139  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
140  * will always take the minimum per cell.
141  * \param min_range the minimum visible range (defaults to 0)
142  * \param border_size the border size (defaults to 0). Set to `std::numeric_limits<int>::min()` to turn cropping off.
143  */
144  template <typename PointCloudType> void
145  createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution=pcl::deg2rad (0.5f),
146  float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f),
147  const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
148  CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
149  float min_range=0.0f, int border_size=0);
150 
151  /** \brief Create the depth image from a point cloud
152  * \param point_cloud the input point cloud
153  * \param angular_resolution_x the angular difference (in radians) between the
154  * individual pixels in the image in the x-direction
155  * \param angular_resolution_y the angular difference (in radians) between the
156  * individual pixels in the image in the y-direction
157  * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
158  * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
159  * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
160  * Eigen::Affine3f::Identity () )
161  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
162  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
163  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
164  * will always take the minimum per cell.
165  * \param min_range the minimum visible range (defaults to 0)
166  * \param border_size the border size (defaults to 0). Set to `std::numeric_limits<int>::min()` to turn cropping off.
167  */
168  template <typename PointCloudType> void
169  createFromPointCloud (const PointCloudType& point_cloud,
170  float angular_resolution_x=pcl::deg2rad (0.5f), float angular_resolution_y=pcl::deg2rad (0.5f),
171  float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f),
172  const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
173  CoordinateFrame coordinate_frame=CAMERA_FRAME,
174  float noise_level=0.0f, float min_range=0.0f, int border_size=0);
175 
176  /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for
177  * faster calculation.
178  * \param point_cloud the input point cloud
179  * \param angular_resolution the angle (in radians) between each sample in the depth image
180  * \param point_cloud_center the center of bounding sphere
181  * \param point_cloud_radius the radius of the bounding sphere
182  * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
183  * Eigen::Affine3f::Identity () )
184  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
185  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
186  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
187  * will always take the minimum per cell.
188  * \param min_range the minimum visible range (defaults to 0)
189  * \param border_size the border size (defaults to 0). Set to `std::numeric_limits<int>::min()` to turn cropping off.
190  */
191  template <typename PointCloudType> void
192  createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution,
193  const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
194  const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
195  CoordinateFrame coordinate_frame=CAMERA_FRAME,
196  float noise_level=0.0f, float min_range=0.0f, int border_size=0);
197 
198  /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for
199  * faster calculation.
200  * \param point_cloud the input point cloud
201  * \param angular_resolution_x the angular difference (in radians) between the
202  * individual pixels in the image in the x-direction
203  * \param angular_resolution_y the angular difference (in radians) between the
204  * individual pixels in the image in the y-direction
205  * \param point_cloud_center the center of bounding sphere
206  * \param point_cloud_radius the radius of the bounding sphere
207  * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
208  * Eigen::Affine3f::Identity () )
209  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
210  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
211  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
212  * will always take the minimum per cell.
213  * \param min_range the minimum visible range (defaults to 0)
214  * \param border_size the border size (defaults to 0). Set to `std::numeric_limits<int>::min()` to turn cropping off.
215  */
216  template <typename PointCloudType> void
217  createFromPointCloudWithKnownSize (const PointCloudType& point_cloud,
218  float angular_resolution_x, float angular_resolution_y,
219  const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
220  const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
221  CoordinateFrame coordinate_frame=CAMERA_FRAME,
222  float noise_level=0.0f, float min_range=0.0f, int border_size=0);
223 
224  /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
225  * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
226  * \param point_cloud the input point cloud
227  * \param angular_resolution the angle (in radians) between each sample in the depth image
228  * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
229  * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
230  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
231  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
232  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
233  * will always take the minimum per cell.
234  * \param min_range the minimum visible range (defaults to 0)
235  * \param border_size the border size (defaults to 0). Set to `std::numeric_limits<int>::min()` to turn cropping off.
236  * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
237  * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y
238  * to the bottom and z to the front) */
239  template <typename PointCloudTypeWithViewpoints> void
240  createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution,
241  float max_angle_width, float max_angle_height,
242  CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
243  float min_range=0.0f, int border_size=0);
244 
245  /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
246  * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
247  * \param point_cloud the input point cloud
248  * \param angular_resolution_x the angular difference (in radians) between the
249  * individual pixels in the image in the x-direction
250  * \param angular_resolution_y the angular difference (in radians) between the
251  * individual pixels in the image in the y-direction
252  * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
253  * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
254  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
255  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
256  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
257  * will always take the minimum per cell.
258  * \param min_range the minimum visible range (defaults to 0)
259  * \param border_size the border size (defaults to 0). Set to `std::numeric_limits<int>::min()` to turn cropping off.
260  * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
261  * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y
262  * to the bottom and z to the front) */
263  template <typename PointCloudTypeWithViewpoints> void
264  createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
265  float angular_resolution_x, float angular_resolution_y,
266  float max_angle_width, float max_angle_height,
267  CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
268  float min_range=0.0f, int border_size=0);
269 
270  /** \brief Create an empty depth image (filled with unobserved points)
271  * \param[in] angular_resolution the angle (in radians) between each sample in the depth image
272  * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () )
273  * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
274  * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg))
275  * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg))
276  */
277  void
278  createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
279  RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
280  float angle_height=pcl::deg2rad (180.0f));
281 
282  /** \brief Create an empty depth image (filled with unobserved points)
283  * \param angular_resolution_x the angular difference (in radians) between the
284  * individual pixels in the image in the x-direction
285  * \param angular_resolution_y the angular difference (in radians) between the
286  * individual pixels in the image in the y-direction
287  * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () )
288  * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
289  * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg))
290  * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg))
291  */
292  void
293  createEmpty (float angular_resolution_x, float angular_resolution_y,
294  const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
295  RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
296  float angle_height=pcl::deg2rad (180.0f));
297 
298  /** \brief Integrate the given point cloud into the current range image using a z-buffer
299  * \param point_cloud the input point cloud
300  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
301  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
302  * will always take the minimum per cell.
303  * \param min_range the minimum visible range
304  * \param top returns the minimum y pixel position in the image where a point was added
305  * \param right returns the maximum x pixel position in the image where a point was added
306  * \param bottom returns the maximum y pixel position in the image where a point was added
307  * \param left returns the minimum x pixel position in the image where a point was added
308  */
309  template <typename PointCloudType> void
310  doZBuffer (const PointCloudType& point_cloud, float noise_level,
311  float min_range, int& top, int& right, int& bottom, int& left);
312 
313  /** \brief Integrates the given far range measurements into the range image */
314  template <typename PointCloudType> void
315  integrateFarRanges (const PointCloudType& far_ranges);
316 
317  /** \brief Cut the range image to the minimal size so that it still contains all actual range readings.
318  * \param border_size allows increase from the minimal size by the specified number of pixels (defaults to 0)
319  * \param top if positive, this value overrides the position of the top edge (defaults to -1)
320  * \param right if positive, this value overrides the position of the right edge (defaults to -1)
321  * \param bottom if positive, this value overrides the position of the bottom edge (defaults to -1)
322  * \param left if positive, this value overrides the position of the left edge (defaults to -1)
323  */
324  PCL_EXPORTS void
325  cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
326 
327  /** \brief Get all the range values in one float array of size width*height
328  * \return a pointer to a new float array containing the range values
329  * \note This method allocates a new float array; the caller is responsible for freeing this memory.
330  */
331  PCL_EXPORTS float*
332  getRangesArray () const;
333 
334  /** Getter for the transformation from the world system into the range image system
335  * (the sensor coordinate frame) */
336  inline const Eigen::Affine3f&
338 
339  /** Setter for the transformation from the range image system
340  * (the sensor coordinate frame) into the world system */
341  inline void
342  setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system);
343 
344  /** Getter for the transformation from the range image system
345  * (the sensor coordinate frame) into the world system */
346  inline const Eigen::Affine3f&
348 
349  /** Getter for the angular resolution of the range image in x direction in radians per pixel.
350  * Provided for downwards compatibility */
351  inline float
353 
354  /** Getter for the angular resolution of the range image in x direction in radians per pixel. */
355  inline float
357 
358  /** Getter for the angular resolution of the range image in y direction in radians per pixel. */
359  inline float
361 
362  /** Getter for the angular resolution of the range image in x and y direction (in radians). */
363  inline void
364  getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const;
365 
366  /** \brief Set the angular resolution of the range image
367  * \param angular_resolution the new angular resolution in x and y direction (in radians per pixel)
368  */
369  inline void
370  setAngularResolution (float angular_resolution);
371 
372  /** \brief Set the angular resolution of the range image
373  * \param angular_resolution_x the new angular resolution in x direction (in radians per pixel)
374  * \param angular_resolution_y the new angular resolution in y direction (in radians per pixel)
375  */
376  inline void
377  setAngularResolution (float angular_resolution_x, float angular_resolution_y);
378 
379 
380  /** \brief Return the 3D point with range at the given image position
381  * \param image_x the x coordinate
382  * \param image_y the y coordinate
383  * \return the point at the specified location (returns unobserved_point if outside of the image bounds)
384  */
385  inline const PointWithRange&
386  getPoint (int image_x, int image_y) const;
387 
388  /** \brief Non-const-version of getPoint */
389  inline PointWithRange&
390  getPoint (int image_x, int image_y);
391 
392  /** Return the 3d point with range at the given image position */
393  inline const PointWithRange&
394  getPoint (float image_x, float image_y) const;
395 
396  /** Non-const-version of the above */
397  inline PointWithRange&
398  getPoint (float image_x, float image_y);
399 
400  /** \brief Return the 3D point with range at the given image position. This method performs no error checking
401  * to make sure the specified image position is inside of the image!
402  * \param image_x the x coordinate
403  * \param image_y the y coordinate
404  * \return the point at the specified location (program may fail if the location is outside of the image bounds)
405  */
406  inline const PointWithRange&
407  getPointNoCheck (int image_x, int image_y) const;
408 
409  /** Non-const-version of getPointNoCheck */
410  inline PointWithRange&
411  getPointNoCheck (int image_x, int image_y);
412 
413  /** Same as above */
414  inline void
415  getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
416 
417  /** Same as above */
418  inline void
419  getPoint (int index, Eigen::Vector3f& point) const;
420 
421  /** Same as above */
422  inline const Eigen::Map<const Eigen::Vector3f>
423  getEigenVector3f (int x, int y) const;
424 
425  /** Same as above */
426  inline const Eigen::Map<const Eigen::Vector3f>
427  getEigenVector3f (int index) const;
428 
429  /** Return the 3d point with range at the given index (whereas index=y*width+x) */
430  inline const PointWithRange&
431  getPoint (int index) const;
432 
433  /** Calculate the 3D point according to the given image point and range */
434  inline void
435  calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const;
436  /** Calculate the 3D point according to the given image point and the range value at the closest pixel */
437  inline void
438  calculate3DPoint (float image_x, float image_y, PointWithRange& point) const;
439 
440  /** Calculate the 3D point according to the given image point and range */
441  virtual inline void
442  calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
443  /** Calculate the 3D point according to the given image point and the range value at the closest pixel */
444  inline void
445  calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
446 
447  /** Recalculate all 3D point positions according to their pixel position and range */
448  PCL_EXPORTS void
450 
451  /** Get imagePoint from 3D point in world coordinates */
452  inline virtual void
453  getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
454 
455  /** Same as above */
456  inline void
457  getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
458 
459  /** Same as above */
460  inline void
461  getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
462 
463  /** Same as above */
464  inline void
465  getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
466 
467  /** Same as above */
468  inline void
469  getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
470 
471  /** Same as above */
472  inline void
473  getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
474 
475  /** Same as above */
476  inline void
477  getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
478 
479  /** point_in_image will be the point in the image at the position the given point would be. Returns
480  * the range of the given point. */
481  inline float
482  checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const;
483 
484  /** Returns the difference in range between the given point and the range of the point in the image
485  * at the position the given point would be.
486  * (Return value is point_in_image.range-given_point.range) */
487  inline float
488  getRangeDifference (const Eigen::Vector3f& point) const;
489 
490  /** Get the image point corresponding to the given angles */
491  inline void
492  getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
493 
494  /** Get the angles corresponding to the given image point */
495  inline void
496  getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
497 
498  /** Transforms an image point in float values to an image point in int values */
499  inline void
500  real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
501 
502  /** Check if a point is inside of the image */
503  inline bool
504  isInImage (int x, int y) const;
505 
506  /** Check if a point is inside of the image and has a finite range */
507  inline bool
508  isValid (int x, int y) const;
509 
510  /** Check if a point has a finite range */
511  inline bool
512  isValid (int index) const;
513 
514  /** Check if a point is inside of the image and has either a finite range or a max reading (range=INFINITY) */
515  inline bool
516  isObserved (int x, int y) const;
517 
518  /** Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! */
519  inline bool
520  isMaxRange (int x, int y) const;
521 
522  /** Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius.
523  * step_size determines how many pixels are used. 1 means all, 2 only every second, etc..
524  * Returns false if it was unable to calculate a normal.*/
525  inline bool
526  getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
527 
528  /** Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered. */
529  inline bool
530  getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
531  int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const;
532 
533  /** Same as above */
534  inline bool
535  getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point,
536  int no_of_nearest_neighbors, Eigen::Vector3f& normal,
537  Eigen::Vector3f* point_on_plane=nullptr, int step_size=1) const;
538 
539  /** Same as above, using default values */
540  inline bool
541  getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
542 
543  /** Same as above but extracts some more data and can also return the extracted
544  * information for all neighbors in radius if normal_all_neighbors is not NULL */
545  inline bool
546  getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point,
547  int no_of_closest_neighbors, int step_size,
548  float& max_closest_neighbor_distance_squared,
549  Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
550  Eigen::Vector3f* normal_all_neighbors=nullptr,
551  Eigen::Vector3f* mean_all_neighbors=nullptr,
552  Eigen::Vector3f* eigen_values_all_neighbors=nullptr) const;
553 
554  // Return the squared distance to the n-th neighbors of the point at x,y
555  inline float
556  getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const;
557 
558  /** Calculate the impact angle based on the sensor position and the two given points - will return
559  * -INFINITY if one of the points is unobserved */
560  inline float
561  getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const;
562  //! Same as above
563  inline float
564  getImpactAngle (int x1, int y1, int x2, int y2) const;
565 
566  /** Extract a local normal (with a heuristic not to include background points) and calculate the impact
567  * angle based on this */
568  inline float
569  getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const;
570  /** Uses the above function for every point in the image */
571  PCL_EXPORTS float*
573 
574  /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg)
575  * This uses getImpactAngleBasedOnLocalNormal
576  * Will return -INFINITY if no normal could be calculated */
577  inline float
578  getNormalBasedAcutenessValue (int x, int y, int radius) const;
579 
580  /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg)
581  * will return -INFINITY if one of the points is unobserved */
582  inline float
583  getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const;
584  //! Same as above
585  inline float
586  getAcutenessValue (int x1, int y1, int x2, int y2) const;
587 
588  /** Calculate getAcutenessValue for every point */
589  PCL_EXPORTS void
590  getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
591  float*& acuteness_value_image_y) const;
592 
593  /** Calculates, how much the surface changes at a point. Pi meaning a flat surface and 0.0f
594  * would be a needle point */
595  //inline float
596  // getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1,
597  // const PointWithRange& neighbor2) const;
598 
599  /** Calculates, how much the surface changes at a point. 1 meaning a 90deg angle and 0 a flat surface */
600  PCL_EXPORTS float
601  getSurfaceChange (int x, int y, int radius) const;
602 
603  /** Uses the above function for every point in the image */
604  PCL_EXPORTS float*
605  getSurfaceChangeImage (int radius) const;
606 
607  /** Calculates, how much the surface changes at a point. Returns an angle [0.0f, PI] for x and y direction.
608  * A return value of -INFINITY means that a point was unobserved. */
609  inline void
610  getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
611 
612  /** Uses the above function for every point in the image */
613  PCL_EXPORTS void
614  getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
615 
616  /** Calculates the curvature in a point using pca */
617  inline float
618  getCurvature (int x, int y, int radius, int step_size) const;
619 
620  //! Get the sensor position
621  inline const Eigen::Vector3f
622  getSensorPos () const;
623 
624  /** Sets all -INFINITY values to INFINITY */
625  PCL_EXPORTS void
627 
628  //! Getter for image_offset_x_
629  inline int
630  getImageOffsetX () const { return image_offset_x_;}
631  //! Getter for image_offset_y_
632  inline int
633  getImageOffsetY () const { return image_offset_y_;}
634 
635  //! Setter for image offsets
636  inline void
637  setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;}
638 
639 
640 
641  /** Get a sub part of the complete image as a new range image.
642  * \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image.
643  * This is always according to absolute 0,0 meaning -180°,-90°
644  * and it is already in the system of the new image, so the
645  * actual pixel used in the original image is
646  * combine_pixels* (image_offset_x-image_offset_x_)
647  * \param sub_image_image_offset_y - Same as image_offset_x for the y coordinate
648  * \param sub_image_width - width of the new image
649  * \param sub_image_height - height of the new image
650  * \param combine_pixels - shrinking factor, meaning the new angular resolution
651  * is combine_pixels times the old one
652  * \param sub_image - the output image
653  */
654  virtual void
655  getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
656  int sub_image_height, int combine_pixels, RangeImage& sub_image) const;
657 
658  //! Get a range image with half the resolution
659  virtual void
660  getHalfImage (RangeImage& half_image) const;
661 
662  //! Find the minimum and maximum range in the image
663  PCL_EXPORTS void
664  getMinMaxRanges (float& min_range, float& max_range) const;
665 
666  //! This function sets the sensor pose to 0 and transforms all point positions to this local coordinate frame
667  PCL_EXPORTS void
669 
670  /** Calculate a range patch as the z values of the coordinate frame given by pose.
671  * The patch will have size pixel_size x pixel_size and each pixel
672  * covers world_size/pixel_size meters in the world
673  * You are responsible for deleting the structure afterwards! */
674  PCL_EXPORTS float*
675  getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
676 
677  //! Same as above, but using the local coordinate frame defined by point and the viewing direction
678  PCL_EXPORTS float*
679  getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
680 
681  //! Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction
682  inline Eigen::Affine3f
683  getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
684  //! Same as above, using a reference for the retrurn value
685  inline void
686  getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point,
687  Eigen::Affine3f& transformation) const;
688  //! Same as above, but only returning the rotation
689  inline void
690  getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const;
691 
692  /** Get a local coordinate frame at the given point based on the normal. */
693  PCL_EXPORTS bool
694  getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
695  float max_dist, Eigen::Affine3f& transformation) const;
696 
697  /** Get the integral image of the range values (used for fast blur operations).
698  * You are responsible for deleting it after usage! */
699  PCL_EXPORTS void
700  getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
701 
702  /** Get a blurred version of the range image using box filters on the provided integral image*/
703  PCL_EXPORTS void // Template necessary so that this function also works in derived classes
704  getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image,
705  RangeImage& range_image) const;
706 
707  /** Get a blurred version of the range image using box filters */
708  PCL_EXPORTS virtual void // Template necessary so that this function also works in derived classes
709  getBlurredImage (int blur_radius, RangeImage& range_image) const;
710 
711  /** Get the squared euclidean distance between the two image points.
712  * Returns -INFINITY if one of the points was not observed */
713  inline float
714  getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const;
715  //! Doing the above for some steps in the given direction and averaging
716  inline float
717  getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
718 
719  //! Project all points on the local plane approximation, thereby smoothing the surface of the scan
720  PCL_EXPORTS void
721  getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
722  //void getLocalNormals (int radius) const;
723 
724  /** Calculates the average 3D position of the no_of_points points described by the start
725  * point x,y in the direction delta.
726  * Returns a max range point (range=INFINITY) if the first point is max range and an
727  * unobserved point (range=-INFINITY) if non of the points is observed. */
728  inline void
729  get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points,
730  PointWithRange& average_point) const;
731 
732  /** Calculates the overlap of two range images given the relative transformation
733  * (from the given image to *this) */
734  PCL_EXPORTS float
735  getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation,
736  int search_radius, float max_distance, int pixel_step=1) const;
737 
738  /** Get the viewing direction for the given point */
739  inline bool
740  getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
741 
742  /** Get the viewing direction for the given point */
743  inline void
744  getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
745 
746  /** Return a newly created Range image.
747  * Can be reimplemented in derived classes like RangeImagePlanar to return an image of the same type. */
748  PCL_EXPORTS virtual RangeImage*
749  getNew () const { return new RangeImage; }
750 
751  /** Copy other to *this. Necessary for use in virtual functions that need to copy derived RangeImage classes (like RangeImagePlanar) */
752  PCL_EXPORTS virtual void
753  copyTo (RangeImage& other) const;
754 
755 
756  // =====MEMBER VARIABLES=====
757  // BaseClass:
758  // roslib::Header header;
759  // std::vector<PointT> points;
760  // std::uint32_t width;
761  // std::uint32_t height;
762  // bool is_dense;
763 
764  static bool debug; /**< Just for... well... debugging purposes. :-) */
765 
766  protected:
767  // =====PROTECTED MEMBER VARIABLES=====
768  Eigen::Affine3f to_range_image_system_; /**< Inverse of to_world_system_ */
769  Eigen::Affine3f to_world_system_; /**< Inverse of to_range_image_system_ */
770  float angular_resolution_x_{0.0f}; /**< Angular resolution of the range image in x direction in radians per pixel */
771  float angular_resolution_y_{0.0f}; /**< Angular resolution of the range image in y direction in radians per pixel */
772  float angular_resolution_x_reciprocal_{0.0f}; /**< 1.0/angular_resolution_x_ - provided for better performance of
773  * multiplication compared to division */
774  float angular_resolution_y_reciprocal_{0.0f}; /**< 1.0/angular_resolution_y_ - provided for better performance of
775  * multiplication compared to division */
776  int image_offset_x_{0}, image_offset_y_{0}; /**< Position of the top left corner of the range image compared to
777  * an image of full size (360x180 degrees) */
778  PointWithRange unobserved_point; /**< This point is used to be able to return
779  * a reference to a non-existing point */
780 
781  // =====PROTECTED METHODS=====
782 
783 
784  // =====STATIC PROTECTED=====
785  PCL_EXPORTS static const int lookup_table_size;
786  PCL_EXPORTS static std::vector<float> asin_lookup_table;
787  PCL_EXPORTS static std::vector<float> atan_lookup_table;
788  PCL_EXPORTS static std::vector<float> cos_lookup_table;
789  /** Create lookup tables for trigonometric functions */
790  static void
792 
793  /** Query the asin lookup table */
794  static inline float
795  asinLookUp (float value);
796 
797  /** Query the std::atan2 lookup table */
798  static inline float
799  atan2LookUp (float y, float x);
800 
801  /** Query the cos lookup table */
802  static inline float
803  cosLookUp (float value);
804 
805 
806  public:
808  };
809 
810  /**
811  * /ingroup range_image
812  */
813  inline std::ostream&
814  operator<< (std::ostream& os, const RangeImage& r)
815  {
816  os << "header: " << std::endl;
817  os << r.header;
818  os << "points[]: " << r.size () << std::endl;
819  os << "width: " << r.width << std::endl;
820  os << "height: " << r.height << std::endl;
821  os << "sensor_origin_: "
822  << r.sensor_origin_[0] << ' '
823  << r.sensor_origin_[1] << ' '
824  << r.sensor_origin_[2] << std::endl;
825  os << "sensor_orientation_: "
826  << r.sensor_orientation_.x () << ' '
827  << r.sensor_orientation_.y () << ' '
828  << r.sensor_orientation_.z () << ' '
829  << r.sensor_orientation_.w () << std::endl;
830  os << "is_dense: " << r.is_dense << std::endl;
831  os << "angular resolution: "
832  << RAD2DEG (r.getAngularResolutionX ()) << "deg/pixel in x and "
833  << RAD2DEG (r.getAngularResolutionY ()) << "deg/pixel in y.\n" << std::endl;
834  return (os);
835  }
836 
837 } // namespace end
838 
839 
840 #include <pcl/range_image/impl/range_image.hpp> // Definitions of templated and inline functions
Define standard C methods to do angle calculations.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where...
Definition: range_image.h:55
PCL_EXPORTS void getIntegralImage(float *&integral_image, int *&valid_points_num_image) const
Get the integral image of the range values (used for fast blur operations).
PCL_EXPORTS void getSurfaceAngleChangeImages(int radius, float *&angle_change_image_x, float *&angle_change_image_y) const
Uses the above function for every point in the image.
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
PCL_EXPORTS bool getNormalBasedUprightTransformation(const Eigen::Vector3f &point, float max_dist, Eigen::Affine3f &transformation) const
Get a local coordinate frame at the given point based on the normal.
PCL_EXPORTS void getBlurredImageUsingIntegralImage(int blur_radius, float *integral_image, int *valid_points_num_image, RangeImage &range_image) const
Get a blurred version of the range image using box filters on the provided integral image.
int image_offset_y_
Position of the top left corner of the range image compared to an image of full size (360x180 degrees...
Definition: range_image.h:776
PCL_EXPORTS float getSurfaceChange(int x, int y, int radius) const
Calculates, how much the surface changes at a point.
static float atan2LookUp(float y, float x)
Query the std::atan2 lookup table.
Definition: range_image.hpp:64
void calculate3DPoint(float image_x, float image_y, float range, PointWithRange &point) const
Calculate the 3D point according to the given image point and range.
static bool debug
Just for...
Definition: range_image.h:764
float getImpactAngleBasedOnLocalNormal(int x, int y, int radius) const
Extract a local normal (with a heuristic not to include background points) and calculate the impact a...
PCL_EXPORTS void cropImage(int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1)
Cut the range image to the minimal size so that it still contains all actual range readings.
PCL_EXPORTS float * getInterpolatedSurfaceProjection(const Eigen::Affine3f &pose, int pixel_size, float world_size) const
Calculate a range patch as the z values of the coordinate frame given by pose.
float getAcutenessValue(const PointWithRange &point1, const PointWithRange &point2) const
Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) will r...
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
void getAnglesFromImagePoint(float image_x, float image_y, float &angle_x, float &angle_y) const
Get the angles corresponding to the given image point.
PCL_EXPORTS float * getSurfaceChangeImage(int radius) const
Uses the above function for every point in the image.
int getImageOffsetX() const
Getter for image_offset_x_.
Definition: range_image.h:630
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
virtual PCL_EXPORTS ~RangeImage()
Destructor.
static float cosLookUp(float value)
Query the cos lookup table.
Definition: range_image.hpp:90
virtual PCL_EXPORTS void getBlurredImage(int blur_radius, RangeImage &range_image) const
Get a blurred version of the range image using box filters.
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
Definition: range_image.h:769
virtual void getSubImage(int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width, int sub_image_height, int combine_pixels, RangeImage &sub_image) const
Get a sub part of the complete image as a new range image.
static int max_no_of_threads
The maximum number of openmp threads that can be used in this class.
Definition: range_image.h:78
PCL_EXPORTS void change3dPointsToLocalCoordinateFrame()
This function sets the sensor pose to 0 and transforms all point positions to this local coordinate f...
void get1dPointAverage(int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange &average_point) const
Calculates the average 3D position of the no_of_points points described by the start point x,...
shared_ptr< RangeImage > Ptr
Definition: range_image.h:60
float checkPoint(const Eigen::Vector3f &point, PointWithRange &point_in_image) const
point_in_image will be the point in the image at the position the given point would be.
int getImageOffsetY() const
Getter for image_offset_y_.
Definition: range_image.h:633
PCL_EXPORTS void getRangeImageWithSmoothedSurface(int radius, RangeImage &smoothed_range_image) const
Project all points on the local plane approximation, thereby smoothing the surface of the scan.
virtual PCL_EXPORTS void copyTo(RangeImage &other) const
Copy other to *this.
void createEmpty(float angular_resolution, const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad(360.0f), float angle_height=pcl::deg2rad(180.0f))
Create an empty depth image (filled with unobserved points)
static Eigen::Vector3f getAverageViewPoint(const PointCloudTypeWithViewpoints &point_cloud)
Get the average viewpoint of a point cloud where each point carries viewpoint information as vp_x,...
float getAverageEuclideanDistance(int x, int y, int offset_x, int offset_y, int max_steps) const
Doing the above for some steps in the given direction and averaging.
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
Definition: range_image.h:778
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
void doZBuffer(const PointCloudType &point_cloud, float noise_level, float min_range, int &top, int &right, int &bottom, int &left)
Integrate the given point cloud into the current range image using a z-buffer.
PCL_EXPORTS void getAcutenessValueImages(int pixel_distance, float *&acuteness_value_image_x, float *&acuteness_value_image_y) const
Calculate getAcutenessValue for every point.
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
float getNormalBasedAcutenessValue(int x, int y, int radius) const
Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) This u...
float getAngularResolutionX() const
Getter for the angular resolution of the range image in x direction in radians per pixel.
Definition: range_image.h:356
Eigen::Affine3f getTransformationToViewerCoordinateFrame(const Eigen::Vector3f &point) const
Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction.
float getImpactAngle(const PointWithRange &point1, const PointWithRange &point2) const
Calculate the impact angle based on the sensor position and the two given points - will return -INFIN...
void integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.
static float asinLookUp(float value)
Query the asin lookup table.
Definition: range_image.hpp:54
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel.
Definition: range_image.h:352
static PCL_EXPORTS std::vector< float > cos_lookup_table
Definition: range_image.h:788
static PCL_EXPORTS void extractFarRanges(const pcl::PCLPointCloud2 &point_cloud_data, PointCloud< PointWithViewpoint > &far_ranges)
Check if the provided data includes far ranges and add them to far_ranges.
bool isObserved(int x, int y) const
Check if a point is inside of the image and has either a finite range or a max reading (range=INFINIT...
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first!
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
virtual PCL_EXPORTS RangeImage * getNew() const
Return a newly created Range image.
Definition: range_image.h:749
Ptr makeShared()
Get a boost shared pointer of a copy of this.
Definition: range_image.h:124
bool isInImage(int x, int y) const
Check if a point is inside of the image.
void createFromPointCloud(const PointCloudType &point_cloud, float angular_resolution=pcl::deg2rad(0.5f), float max_angle_width=pcl::deg2rad(360.0f), float max_angle_height=pcl::deg2rad(180.0f), const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud.
Definition: range_image.hpp:98
PCL_EXPORTS void getMinMaxRanges(float &min_range, float &max_range) const
Find the minimum and maximum range in the image.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performance of multiplication compared to division
Definition: range_image.h:774
void real2DToInt2D(float x, float y, int &xInt, int &yInt) const
Transforms an image point in float values to an image point in int values.
void createFromPointCloudWithViewpoints(const PointCloudTypeWithViewpoints &point_cloud, float angular_resolution, float max_angle_width, float max_angle_height, CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud, using the average viewpoint of the points (vp_x,...
float getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
Definition: range_image.h:771
shared_ptr< const RangeImage > ConstPtr
Definition: range_image.h:61
void setImageOffsets(int offset_x, int offset_y)
Setter for image offsets.
Definition: range_image.h:637
static PCL_EXPORTS std::vector< float > atan_lookup_table
Definition: range_image.h:787
static PCL_EXPORTS std::vector< float > asin_lookup_table
Definition: range_image.h:786
bool getSurfaceInformation(int x, int y, int radius, const Eigen::Vector3f &point, int no_of_closest_neighbors, int step_size, float &max_closest_neighbor_distance_squared, Eigen::Vector3f &normal, Eigen::Vector3f &mean, Eigen::Vector3f &eigen_values, Eigen::Vector3f *normal_all_neighbors=nullptr, Eigen::Vector3f *mean_all_neighbors=nullptr, Eigen::Vector3f *eigen_values_all_neighbors=nullptr) const
Same as above but extracts some more data and can also return the extracted information for all neigh...
void setTransformationToRangeImageSystem(const Eigen::Affine3f &to_range_image_system)
Setter for the transformation from the range image system (the sensor coordinate frame) into the worl...
PCL_EXPORTS float * getRangesArray() const
Get all the range values in one float array of size width*height.
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
virtual void getHalfImage(RangeImage &half_image) const
Get a range image with half the resolution.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > VectorOfEigenVector3f
Definition: range_image.h:59
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
Definition: range_image.h:770
const Eigen::Affine3f & getTransformationToRangeImageSystem() const
Getter for the transformation from the world system into the range image system (the sensor coordinat...
Definition: range_image.h:337
static float getMaxAngleSize(const Eigen::Affine3f &viewer_pose, const Eigen::Vector3f &center, float radius)
Get the size of a certain area when seen from the given pose.
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
static PCL_EXPORTS const int lookup_table_size
Definition: range_image.h:785
PCL_EXPORTS float * getInterpolatedSurfaceProjection(const Eigen::Vector3f &point, int pixel_size, float world_size) const
Same as above, but using the local coordinate frame defined by point and the viewing direction.
bool getNormalForClosestNeighbors(int x, int y, int radius, const PointWithRange &point, int no_of_nearest_neighbors, Eigen::Vector3f &normal, int step_size=1) const
Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered.
PCL_EXPORTS void reset()
Reset all values to an empty range image.
bool getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
PCL_EXPORTS void setUnseenToMaxRange()
Sets all -INFINITY values to INFINITY.
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
Definition: range_image.h:768
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performance of multiplication compared to division
Definition: range_image.h:772
PCL_EXPORTS float getOverlap(const RangeImage &other_range_image, const Eigen::Affine3f &relative_transformation, int search_radius, float max_distance, int pixel_step=1) const
Calculates the overlap of two range images given the relative transformation (from the given image to...
void getImagePointFromAngles(float angle_x, float angle_y, float &image_x, float &image_y) const
Get the image point corresponding to the given angles.
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
static void createLookupTables()
Create lookup tables for trigonometric functions.
float getRangeDifference(const Eigen::Vector3f &point) const
Returns the difference in range between the given point and the range of the point in the image at th...
bool getNormal(int x, int y, int radius, Eigen::Vector3f &normal, int step_size=1) const
Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius.
PCL_EXPORTS float * getImpactAngleImageBasedOnLocalNormals(int radius) const
Uses the above function for every point in the image.
PCL_EXPORTS RangeImage()
Constructor.
float getAngularResolutionY() const
Getter for the angular resolution of the range image in y direction in radians per pixel.
Definition: range_image.h:360
static PCL_EXPORTS void getCoordinateFrameTransformation(RangeImage::CoordinateFrame coordinate_frame, Eigen::Affine3f &transformation)
Get the transformation that transforms the given coordinate frame into CAMERA_FRAME.
void createEmpty(float angular_resolution_x, float angular_resolution_y, const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad(360.0f), float angle_height=pcl::deg2rad(180.0f))
Create an empty depth image (filled with unobserved points)
void createFromPointCloudWithKnownSize(const PointCloudType &point_cloud, float angular_resolution, const Eigen::Vector3f &point_cloud_center, float point_cloud_radius, const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud, getting a hint about the size of the scene for faster calc...
void getSurfaceAngleChange(int x, int y, int radius, float &angle_change_x, float &angle_change_y) const
Calculates, how much the surface changes at a point.
const Eigen::Affine3f & getTransformationToWorldSystem() const
Getter for the transformation from the range image system (the sensor coordinate frame) into the worl...
Definition: range_image.h:347
Defines all the PCL implemented PointT point type structures.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
Defines all the PCL and non-PCL macros used.
#define PCL_EXPORTS
Definition: pcl_macros.h:325
#define RAD2DEG(x)
Definition: pcl_macros.h:229
A point structure representing Euclidean xyz coordinates, padded with an extra range float.