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