00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef PCL_RANGE_IMAGE_H_
00038 #define PCL_RANGE_IMAGE_H_
00039
00040 #include <pcl/common/eigen.h>
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/pcl_macros.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/common/common_headers.h>
00045 #include <pcl/common/vector_average.h>
00046 #include <typeinfo>
00047
00048 namespace pcl
00049 {
00055 class RangeImage : public pcl::PointCloud<PointWithRange>
00056 {
00057 public:
00058
00059 typedef pcl::PointCloud<PointWithRange> BaseClass;
00060 typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > VectorOfEigenVector3f;
00061 typedef boost::shared_ptr<RangeImage> Ptr;
00062 typedef boost::shared_ptr<const RangeImage> ConstPtr;
00063
00064 enum CoordinateFrame
00065 {
00066 CAMERA_FRAME = 0,
00067 LASER_FRAME = 1
00068 };
00069
00070
00071
00073 PCL_EXPORTS RangeImage ();
00075 PCL_EXPORTS ~RangeImage ();
00076
00077
00079 static int max_no_of_threads;
00080
00081
00088 static inline float
00089 getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center,
00090 float radius);
00091
00096 static inline Eigen::Vector3f
00097 getEigenVector3f (const PointWithRange& point);
00098
00103 PCL_EXPORTS static void
00104 getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
00105 Eigen::Affine3f& transformation);
00106
00112 template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f
00113 getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
00114
00119 PCL_EXPORTS static void
00120 extractFarRanges (const sensor_msgs::PointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges);
00121
00122
00124 inline Ptr
00125 makeShared () { return Ptr (new RangeImage (*this)); }
00126
00128 PCL_EXPORTS void
00129 reset ();
00130
00145 template <typename PointCloudType> void
00146 createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution=pcl::deg2rad (0.5f),
00147 float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f),
00148 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00149 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00150 float min_range=0.0f, int border_size=0);
00151
00169 template <typename PointCloudType> void
00170 createFromPointCloud (const PointCloudType& point_cloud,
00171 float angular_resolution_x=pcl::deg2rad (0.5f), float angular_resolution_y=pcl::deg2rad (0.5f),
00172 float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f),
00173 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00174 CoordinateFrame coordinate_frame=CAMERA_FRAME,
00175 float noise_level=0.0f, float min_range=0.0f, int border_size=0);
00176
00192 template <typename PointCloudType> void
00193 createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution,
00194 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
00195 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00196 CoordinateFrame coordinate_frame=CAMERA_FRAME,
00197 float noise_level=0.0f, float min_range=0.0f, int border_size=0);
00198
00218 template <typename PointCloudType> void
00219 createFromPointCloudWithKnownSize (const PointCloudType& point_cloud,
00220 float angular_resolution_x, float angular_resolution_y,
00221 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
00222 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00223 CoordinateFrame coordinate_frame=CAMERA_FRAME,
00224 float noise_level=0.0f, float min_range=0.0f, int border_size=0);
00225
00241 template <typename PointCloudTypeWithViewpoints> void
00242 createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution,
00243 float max_angle_width, float max_angle_height,
00244 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00245 float min_range=0.0f, int border_size=0);
00246
00265 template <typename PointCloudTypeWithViewpoints> void
00266 createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
00267 float angular_resolution_x, float angular_resolution_y,
00268 float max_angle_width, float max_angle_height,
00269 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00270 float min_range=0.0f, int border_size=0);
00271
00279 void
00280 createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
00281 RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
00282 float angle_height=pcl::deg2rad (180.0f));
00283
00294 void
00295 createEmpty (float angular_resolution_x, float angular_resolution_y,
00296 const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
00297 RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
00298 float angle_height=pcl::deg2rad (180.0f));
00299
00312 template <typename PointCloudType> void
00313 doZBuffer (const PointCloudType& point_cloud, float noise_level,
00314 float min_range, int& top, int& right, int& bottom, int& left);
00315
00317 PCL_EXPORTS void
00318 integrateFarRanges (const PointCloud<PointWithViewpoint>& far_ranges);
00319
00327 PCL_EXPORTS void
00328 cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
00329
00334 PCL_EXPORTS float*
00335 getRangesArray () const;
00336
00339 inline const Eigen::Affine3f&
00340 getTransformationToRangeImageSystem () const { return (to_range_image_system_); }
00341
00344 inline void
00345 setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system);
00346
00349 inline const Eigen::Affine3f&
00350 getTransformationToWorldSystem () const { return to_world_system_;}
00351
00354 inline float
00355 getAngularResolution () const { return angular_resolution_x_;}
00356
00358 inline float
00359 getAngularResolutionX () const { return angular_resolution_x_;}
00360
00362 inline float
00363 getAngularResolutionY () const { return angular_resolution_y_;}
00364
00366 inline void
00367 getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const;
00368
00372 inline void
00373 setAngularResolution (float angular_resolution);
00374
00379 inline void
00380 setAngularResolution (float angular_resolution_x, float angular_resolution_y);
00381
00382
00388 inline const PointWithRange&
00389 getPoint (int image_x, int image_y) const;
00390
00392 inline PointWithRange&
00393 getPoint (int image_x, int image_y);
00394
00396 inline const PointWithRange&
00397 getPoint (float image_x, float image_y) const;
00398
00400 inline PointWithRange&
00401 getPoint (float image_x, float image_y);
00402
00409 inline const PointWithRange&
00410 getPointNoCheck (int image_x, int image_y) const;
00411
00413 inline PointWithRange&
00414 getPointNoCheck (int image_x, int image_y);
00415
00417 inline void
00418 getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
00419
00421 inline void
00422 getPoint (int index, Eigen::Vector3f& point) const;
00423
00425 inline const Eigen::Map<const Eigen::Vector3f>
00426 getEigenVector3f (int x, int y) const;
00427
00429 inline const Eigen::Map<const Eigen::Vector3f>
00430 getEigenVector3f (int index) const;
00431
00433 inline const PointWithRange&
00434 getPoint (int index) const;
00435
00437 inline void
00438 calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const;
00440 inline void
00441 calculate3DPoint (float image_x, float image_y, PointWithRange& point) const;
00442
00444 virtual inline void
00445 calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
00447 inline void
00448 calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
00449
00451 PCL_EXPORTS void
00452 recalculate3DPointPositions ();
00453
00455 inline virtual void
00456 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
00457
00459 inline void
00460 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
00461
00463 inline void
00464 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
00465
00467 inline void
00468 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
00469
00471 inline void
00472 getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
00473
00475 inline void
00476 getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
00477
00479 inline void
00480 getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
00481
00484 inline float
00485 checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const;
00486
00490 inline float
00491 getRangeDifference (const Eigen::Vector3f& point) const;
00492
00494 inline void
00495 getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
00496
00498 inline void
00499 getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
00500
00502 inline void
00503 real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
00504
00506 inline bool
00507 isInImage (int x, int y) const;
00508
00510 inline bool
00511 isValid (int x, int y) const;
00512
00514 inline bool
00515 isValid (int index) const;
00516
00518 inline bool
00519 isObserved (int x, int y) const;
00520
00522 inline bool
00523 isMaxRange (int x, int y) const;
00524
00528 inline bool
00529 getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
00530
00532 inline bool
00533 getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
00534 int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const;
00535
00537 inline bool
00538 getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point,
00539 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
00540 Eigen::Vector3f* point_on_plane=NULL, int step_size=1) const;
00541
00543 inline bool
00544 getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
00545
00548 inline bool
00549 getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point,
00550 int no_of_closest_neighbors, int step_size,
00551 float& max_closest_neighbor_distance_squared,
00552 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
00553 Eigen::Vector3f* normal_all_neighbors=NULL,
00554 Eigen::Vector3f* mean_all_neighbors=NULL,
00555 Eigen::Vector3f* eigen_values_all_neighbors=NULL) const;
00556
00557
00558 inline float
00559 getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const;
00560
00563 inline float
00564 getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const;
00566 inline float
00567 getImpactAngle (int x1, int y1, int x2, int y2) const;
00568
00571 inline float
00572 getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const;
00574 PCL_EXPORTS float*
00575 getImpactAngleImageBasedOnLocalNormals (int radius) const;
00576
00580 inline float
00581 getNormalBasedAcutenessValue (int x, int y, int radius) const;
00582
00585 inline float
00586 getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const;
00588 inline float
00589 getAcutenessValue (int x1, int y1, int x2, int y2) const;
00590
00592 PCL_EXPORTS void
00593 getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
00594 float*& acuteness_value_image_y) const;
00595
00598
00599
00600
00601
00603 PCL_EXPORTS float
00604 getSurfaceChange (int x, int y, int radius) const;
00605
00607 PCL_EXPORTS float*
00608 getSurfaceChangeImage (int radius) const;
00609
00612 inline void
00613 getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
00614
00616 PCL_EXPORTS void
00617 getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
00618
00620 inline float
00621 getCurvature (int x, int y, int radius, int step_size) const;
00622
00624 inline const Eigen::Vector3f
00625 getSensorPos () const;
00626
00628 PCL_EXPORTS void
00629 setUnseenToMaxRange ();
00630
00632 inline int
00633 getImageOffsetX () const { return image_offset_x_;}
00635 inline int
00636 getImageOffsetY () const { return image_offset_y_;}
00637
00639 inline void
00640 setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;}
00641
00642
00643
00657 virtual void
00658 getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
00659 int sub_image_height, int combine_pixels, RangeImage& sub_image) const;
00660
00662 virtual void
00663 getHalfImage (RangeImage& half_image) const;
00664
00666 PCL_EXPORTS void
00667 getMinMaxRanges (float& min_range, float& max_range) const;
00668
00670 PCL_EXPORTS void
00671 change3dPointsToLocalCoordinateFrame ();
00672
00677 PCL_EXPORTS float*
00678 getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
00679
00681 PCL_EXPORTS float*
00682 getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
00683
00685 inline Eigen::Affine3f
00686 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
00688 inline void
00689 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point,
00690 Eigen::Affine3f& transformation) const;
00692 inline void
00693 getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const;
00694
00696 PCL_EXPORTS bool
00697 getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
00698 float max_dist, Eigen::Affine3f& transformation) const;
00699
00702 PCL_EXPORTS void
00703 getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
00704
00706 PCL_EXPORTS void
00707 getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image,
00708 RangeImage& range_image) const;
00709
00711 PCL_EXPORTS void
00712 getBlurredImage (int blur_radius, RangeImage& range_image) const;
00713
00716 inline float
00717 getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const;
00719 inline float
00720 getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
00721
00723 PCL_EXPORTS void
00724 getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
00725
00726
00731 inline void
00732 get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points,
00733 PointWithRange& average_point) const;
00734
00737 PCL_EXPORTS float
00738 getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation,
00739 int search_radius, float max_distance, int pixel_step=1) const;
00740
00742 inline bool
00743 getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
00744
00746 inline void
00747 getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
00748
00751 virtual RangeImage*
00752 getNew () const { return new RangeImage; }
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763 static bool debug;
00765 protected:
00766
00767 Eigen::Affine3f to_range_image_system_;
00768 Eigen::Affine3f to_world_system_;
00769 float angular_resolution_x_;
00770 float angular_resolution_y_;
00771 float angular_resolution_x_reciprocal_;
00773 float angular_resolution_y_reciprocal_;
00775 int image_offset_x_, image_offset_y_;
00777 PointWithRange unobserved_point;
00780
00781
00782
00783
00784 static const int lookup_table_size;
00785 static std::vector<float> asin_lookup_table;
00786 static std::vector<float> atan_lookup_table;
00787 static std::vector<float> cos_lookup_table;
00789 static void
00790 createLookupTables ();
00791
00793 static inline float
00794 asinLookUp (float value);
00795
00797 static inline float
00798 atan2LookUp (float y, float x);
00799
00801 static inline float
00802 cosLookUp (float value);
00803
00804
00805 public:
00806 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00807 };
00808
00812 inline std::ostream&
00813 operator<< (std::ostream& os, const RangeImage& r)
00814 {
00815 os << "header: " << std::endl;
00816 os << r.header;
00817 os << "points[]: " << r.points.size () << std::endl;
00818 os << "width: " << r.width << std::endl;
00819 os << "height: " << r.height << std::endl;
00820 os << "sensor_origin_: "
00821 << r.sensor_origin_[0] << ' '
00822 << r.sensor_origin_[1] << ' '
00823 << r.sensor_origin_[2] << std::endl;
00824 os << "sensor_orientation_: "
00825 << r.sensor_orientation_.x () << ' '
00826 << r.sensor_orientation_.y () << ' '
00827 << r.sensor_orientation_.z () << ' '
00828 << r.sensor_orientation_.w () << std::endl;
00829 os << "is_dense: " << r.is_dense << std::endl;
00830 os << "angular resolution: "
00831 << RAD2DEG (r.getAngularResolutionX ()) << "deg/pixel in x and "
00832 << RAD2DEG (r.getAngularResolutionY ()) << "deg/pixel in y.\n" << std::endl;
00833 return (os);
00834 }
00835
00836 }
00837
00838
00839 #include <pcl/range_image/impl/range_image.hpp>
00840
00841 #endif //#ifndef PCL_RANGE_IMAGE_H_