Point Cloud Library (PCL)  1.7.2
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=====
59  typedef pcl::PointCloud<PointWithRange> BaseClass;
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 
64  enum CoordinateFrame
65  {
66  CAMERA_FRAME = 0,
67  LASER_FRAME = 1
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
104  getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
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 point_cloud_center the center of bounding sphere
207  * \param point_cloud_radius the radius of the bounding sphere
208  * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
209  * Eigen::Affine3f::Identity () )
210  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
211  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
212  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
213  * will always take the minimum per cell.
214  * \param min_range the minimum visible range (defaults to 0)
215  * \param border_size the border size (defaults to 0)
216  */
217  template <typename PointCloudType> void
218  createFromPointCloudWithKnownSize (const PointCloudType& point_cloud,
219  float angular_resolution_x, float angular_resolution_y,
220  const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
221  const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
222  CoordinateFrame coordinate_frame=CAMERA_FRAME,
223  float noise_level=0.0f, float min_range=0.0f, int border_size=0);
224 
225  /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
226  * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
227  * \param point_cloud the input point cloud
228  * \param angular_resolution the angle (in radians) between each sample in the depth image
229  * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
230  * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
231  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
232  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
233  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
234  * will always take the minimum per cell.
235  * \param min_range the minimum visible range (defaults to 0)
236  * \param border_size the border size (defaults to 0)
237  * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
238  * 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
239  * to the bottom and z to the front) */
240  template <typename PointCloudTypeWithViewpoints> void
241  createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution,
242  float max_angle_width, float max_angle_height,
243  CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
244  float min_range=0.0f, int border_size=0);
245 
246  /** \brief Create the depth image from a point cloud, using the average viewpoint of the points
247  * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
248  * \param point_cloud the input point cloud
249  * \param angular_resolution_x the angular difference (in radians) between the
250  * individual pixels in the image in the x-direction
251  * \param angular_resolution_y the angular difference (in radians) between the
252  * individual pixels in the image in the y-direction
253  * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
254  * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
255  * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
256  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
257  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
258  * will always take the minimum per cell.
259  * \param min_range the minimum visible range (defaults to 0)
260  * \param border_size the border size (defaults to 0)
261  * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
262  * 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
263  * to the bottom and z to the front) */
264  template <typename PointCloudTypeWithViewpoints> void
265  createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
266  float angular_resolution_x, float angular_resolution_y,
267  float max_angle_width, float max_angle_height,
268  CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
269  float min_range=0.0f, int border_size=0);
270 
271  /** \brief Create an empty depth image (filled with unobserved points)
272  * \param[in] angular_resolution the angle (in radians) between each sample in the depth image
273  * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () )
274  * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
275  * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg))
276  * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg))
277  */
278  void
279  createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
280  RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
281  float angle_height=pcl::deg2rad (180.0f));
282 
283  /** \brief Create an empty depth image (filled with unobserved points)
284  * \param angular_resolution_x the angular difference (in radians) between the
285  * individual pixels in the image in the x-direction
286  * \param angular_resolution_y the angular difference (in radians) between the
287  * individual pixels in the image in the y-direction
288  * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () )
289  * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
290  * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg))
291  * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg))
292  */
293  void
294  createEmpty (float angular_resolution_x, float angular_resolution_y,
295  const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
296  RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
297  float angle_height=pcl::deg2rad (180.0f));
298 
299  /** \brief Integrate the given point cloud into the current range image using a z-buffer
300  * \param point_cloud the input point cloud
301  * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
302  * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
303  * will always take the minimum per cell.
304  * \param min_range the minimum visible range
305  * \param top returns the minimum y pixel position in the image where a point was added
306  * \param right returns the maximum x pixel position in the image where a point was added
307  * \param bottom returns the maximum y pixel position in the image where a point was added
308  * \param top returns the minimum y 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&
339  getTransformationToRangeImageSystem () const { return (to_range_image_system_); }
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&
349  getTransformationToWorldSystem () const { return to_world_system_;}
350 
351  /** Getter for the angular resolution of the range image in x direction in radians per pixel.
352  * Provided for downwards compatability */
353  inline float
354  getAngularResolution () const { return angular_resolution_x_;}
355 
356  /** Getter for the angular resolution of the range image in x direction in radians per pixel. */
357  inline float
358  getAngularResolutionX () const { return angular_resolution_x_;}
359 
360  /** Getter for the angular resolution of the range image in y direction in radians per pixel. */
361  inline float
362  getAngularResolutionY () const { return angular_resolution_y_;}
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
451  recalculate3DPointPositions ();
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=NULL, 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=NULL,
553  Eigen::Vector3f* mean_all_neighbors=NULL,
554  Eigen::Vector3f* eigen_values_all_neighbors=NULL) 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*
574  getImpactAngleImageBasedOnLocalNormals (int radius) const;
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
628  setUnseenToMaxRange ();
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
670  change3dPointsToLocalCoordinateFrame ();
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 reimplmented 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  // uint32_t width;
763  // 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 performace of
775  * multiplication compared to division */
776  float angular_resolution_y_reciprocal_; /**< 1.0/angular_resolution_y_ - provided for better performace 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
793  createLookupTables ();
794 
795  /** Query the asin lookup table */
796  static inline float
797  asinLookUp (float value);
798 
799  /** Query the 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:
809  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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.points.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
843 
844 #endif //#ifndef PCL_RANGE_IMAGE_H_