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