Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
range_image.h
Go to the documentation of this file.
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  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
37 #ifndef PCL_RANGE_IMAGE_H_
38 #define PCL_RANGE_IMAGE_H_
39 
40 #include <pcl/common/eigen.h>
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 {
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=====
73  PCL_EXPORTS RangeImage ();
75  PCL_EXPORTS ~RangeImage ();
76 
77  // =====STATIC VARIABLES=====
79  static int max_no_of_threads;
80 
81  // =====STATIC METHODS=====
88  static inline float
89  getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center,
90  float radius);
91 
96  static inline Eigen::Vector3f
97  getEigenVector3f (const PointWithRange& point);
98 
103  PCL_EXPORTS static void
104  getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
105  Eigen::Affine3f& transformation);
106 
112  template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f
113  getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
114 
119  PCL_EXPORTS static void
120  extractFarRanges (const sensor_msgs::PointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges);
121 
122  // =====METHODS=====
124  inline Ptr
125  makeShared () { return Ptr (new RangeImage (*this)); }
126 
128  PCL_EXPORTS void
129  reset ();
130 
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 
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 
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 
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 
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 
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 
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 
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 
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 
317  PCL_EXPORTS void
318  integrateFarRanges (const PointCloud<PointWithViewpoint>& far_ranges);
319 
327  PCL_EXPORTS void
328  cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
329 
334  PCL_EXPORTS float*
335  getRangesArray () const;
336 
339  inline const Eigen::Affine3f&
340  getTransformationToRangeImageSystem () const { return (to_range_image_system_); }
341 
344  inline void
345  setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system);
346 
349  inline const Eigen::Affine3f&
350  getTransformationToWorldSystem () const { return to_world_system_;}
351 
354  inline float
355  getAngularResolution () const { return angular_resolution_x_;}
356 
358  inline float
359  getAngularResolutionX () const { return angular_resolution_x_;}
360 
362  inline float
363  getAngularResolutionY () const { return angular_resolution_y_;}
364 
366  inline void
367  getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const;
368 
372  inline void
373  setAngularResolution (float angular_resolution);
374 
379  inline void
380  setAngularResolution (float angular_resolution_x, float angular_resolution_y);
381 
382 
388  inline const PointWithRange&
389  getPoint (int image_x, int image_y) const;
390 
392  inline PointWithRange&
393  getPoint (int image_x, int image_y);
394 
396  inline const PointWithRange&
397  getPoint (float image_x, float image_y) const;
398 
400  inline PointWithRange&
401  getPoint (float image_x, float image_y);
402 
409  inline const PointWithRange&
410  getPointNoCheck (int image_x, int image_y) const;
411 
413  inline PointWithRange&
414  getPointNoCheck (int image_x, int image_y);
415 
417  inline void
418  getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
419 
421  inline void
422  getPoint (int index, Eigen::Vector3f& point) const;
423 
425  inline const Eigen::Map<const Eigen::Vector3f>
426  getEigenVector3f (int x, int y) const;
427 
429  inline const Eigen::Map<const Eigen::Vector3f>
430  getEigenVector3f (int index) const;
431 
433  inline const PointWithRange&
434  getPoint (int index) const;
435 
437  inline void
438  calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const;
440  inline void
441  calculate3DPoint (float image_x, float image_y, PointWithRange& point) const;
442 
444  virtual inline void
445  calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
447  inline void
448  calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
449 
451  PCL_EXPORTS void
452  recalculate3DPointPositions ();
453 
455  inline virtual void
456  getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
457 
459  inline void
460  getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
461 
463  inline void
464  getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
465 
467  inline void
468  getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
469 
471  inline void
472  getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
473 
475  inline void
476  getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
477 
479  inline void
480  getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
481 
484  inline float
485  checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const;
486 
490  inline float
491  getRangeDifference (const Eigen::Vector3f& point) const;
492 
494  inline void
495  getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
496 
498  inline void
499  getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
500 
502  inline void
503  real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
504 
506  inline bool
507  isInImage (int x, int y) const;
508 
510  inline bool
511  isValid (int x, int y) const;
512 
514  inline bool
515  isValid (int index) const;
516 
518  inline bool
519  isObserved (int x, int y) const;
520 
522  inline bool
523  isMaxRange (int x, int y) const;
524 
528  inline bool
529  getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
530 
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 
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 
543  inline bool
544  getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
545 
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 
563  inline float
564  getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const;
566  inline float
567  getImpactAngle (int x1, int y1, int x2, int y2) const;
568 
571  inline float
572  getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const;
574  PCL_EXPORTS float*
575  getImpactAngleImageBasedOnLocalNormals (int radius) const;
576 
580  inline float
581  getNormalBasedAcutenessValue (int x, int y, int radius) const;
582 
585  inline float
586  getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const;
588  inline float
589  getAcutenessValue (int x1, int y1, int x2, int y2) const;
590 
592  PCL_EXPORTS void
593  getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
594  float*& acuteness_value_image_y) const;
595 
598  //inline float
599  // getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1,
600  // const PointWithRange& neighbor2) const;
601 
603  PCL_EXPORTS float
604  getSurfaceChange (int x, int y, int radius) const;
605 
607  PCL_EXPORTS float*
608  getSurfaceChangeImage (int radius) const;
609 
612  inline void
613  getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
614 
616  PCL_EXPORTS void
617  getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
618 
620  inline float
621  getCurvature (int x, int y, int radius, int step_size) const;
622 
624  inline const Eigen::Vector3f
625  getSensorPos () const;
626 
628  PCL_EXPORTS void
629  setUnseenToMaxRange ();
630 
632  inline int
633  getImageOffsetX () const { return image_offset_x_;}
635  inline int
636  getImageOffsetY () const { return image_offset_y_;}
637 
639  inline void
640  setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;}
641 
642 
643 
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 
662  virtual void
663  getHalfImage (RangeImage& half_image) const;
664 
666  PCL_EXPORTS void
667  getMinMaxRanges (float& min_range, float& max_range) const;
668 
670  PCL_EXPORTS void
671  change3dPointsToLocalCoordinateFrame ();
672 
677  PCL_EXPORTS float*
678  getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
679 
681  PCL_EXPORTS float*
682  getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
683 
685  inline Eigen::Affine3f
686  getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
688  inline void
689  getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point,
690  Eigen::Affine3f& transformation) const;
692  inline void
693  getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const;
694 
696  PCL_EXPORTS bool
697  getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
698  float max_dist, Eigen::Affine3f& transformation) const;
699 
702  PCL_EXPORTS void
703  getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
704 
706  PCL_EXPORTS void
707  getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image,
708  RangeImage& range_image) const;
709 
711  PCL_EXPORTS void
712  getBlurredImage (int blur_radius, RangeImage& range_image) const;
713 
716  inline float
717  getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const;
719  inline float
720  getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
721 
723  PCL_EXPORTS void
724  getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
725  //void getLocalNormals (int radius) const;
726 
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 
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 
742  inline bool
743  getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
744 
746  inline void
747  getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
748 
751  virtual RangeImage*
752  getNew () const { return new RangeImage; }
753 
754 
755  // =====MEMBER VARIABLES=====
756  // BaseClass:
757  // roslib::Header header;
758  // std::vector<PointT> points;
759  // uint32_t width;
760  // uint32_t height;
761  // bool is_dense;
762 
763  static bool debug;
765  protected:
766  // =====PROTECTED MEMBER VARIABLES=====
767  Eigen::Affine3f to_range_image_system_;
768  Eigen::Affine3f to_world_system_;
769  float angular_resolution_x_;
770  float angular_resolution_y_;
771  float angular_resolution_x_reciprocal_;
773  float angular_resolution_y_reciprocal_;
775  int image_offset_x_, image_offset_y_;
777  PointWithRange unobserved_point;
780  // =====PROTECTED METHODS=====
781 
782 
783  // =====STATIC PROTECTED=====
784  static const int lookup_table_size;
785  static std::vector<float> asin_lookup_table;
786  static std::vector<float> atan_lookup_table;
787  static std::vector<float> cos_lookup_table;
789  static void
790  createLookupTables ();
791 
793  static inline float
794  asinLookUp (float value);
795 
797  static inline float
798  atan2LookUp (float y, float x);
799 
801  static inline float
802  cosLookUp (float value);
803 
804 
805  public:
806  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
807  };
808 
812  inline std::ostream&
813  operator<< (std::ostream& os, const RangeImage& r)
814  {
815  os << "header: " << std::endl;
816  os << r.header;
817  os << "points[]: " << r.points.size () << std::endl;
818  os << "width: " << r.width << std::endl;
819  os << "height: " << r.height << std::endl;
820  os << "sensor_origin_: "
821  << r.sensor_origin_[0] << ' '
822  << r.sensor_origin_[1] << ' '
823  << r.sensor_origin_[2] << std::endl;
824  os << "sensor_orientation_: "
825  << r.sensor_orientation_.x () << ' '
826  << r.sensor_orientation_.y () << ' '
827  << r.sensor_orientation_.z () << ' '
828  << r.sensor_orientation_.w () << std::endl;
829  os << "is_dense: " << r.is_dense << std::endl;
830  os << "angular resolution: "
831  << RAD2DEG (r.getAngularResolutionX ()) << "deg/pixel in x and "
832  << RAD2DEG (r.getAngularResolutionY ()) << "deg/pixel in y.\n" << std::endl;
833  return (os);
834  }
835 
836 } // namespace end
837 
838 
839 #include <pcl/range_image/impl/range_image.hpp> // Definitions of templated and inline functions
840 
841 #endif //#ifndef PCL_RANGE_IMAGE_H_
Generated on Fri Mar 8 2013 12:50:45 for Point Cloud Library (PCL) by   doxygen 1.8.3.1