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_