Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 */ 00034 00035 #ifndef PCL_RANGE_IMAGE_H_ 00036 #define PCL_RANGE_IMAGE_H_ 00037 00038 #include <pcl/point_cloud.h> 00039 #include "pcl/pcl_macros.h" 00040 #include "pcl/point_types.h" 00041 #include "pcl/common/common_headers.h" 00042 #include <Eigen/Core> 00043 #include <Eigen/Geometry> 00044 #include <pcl/common/vector_average.h> 00045 #include <typeinfo> 00046 00047 00048 namespace pcl 00049 { 00050 00056 class RangeImage : public pcl::PointCloud<PointWithRange> 00057 { 00058 public: 00059 // =====TYPEDEFS===== 00060 typedef pcl::PointCloud<PointWithRange> BaseClass; 00061 typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > VectorOfEigenVector3f; 00062 typedef boost::shared_ptr<RangeImage> Ptr; 00063 typedef boost::shared_ptr<const RangeImage> ConstPtr; 00064 00065 enum CoordinateFrame 00066 { 00067 CAMERA_FRAME = 0, 00068 LASER_FRAME = 1 00069 }; 00070 00071 00072 // =====CONSTRUCTOR & DESTRUCTOR===== 00074 PCL_EXPORTS RangeImage (); 00076 PCL_EXPORTS ~RangeImage (); 00077 00078 // =====STATIC VARIABLES===== 00080 static int max_no_of_threads; 00081 00082 // =====STATIC METHODS===== 00089 static inline float 00090 getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, 00091 float radius); 00092 00097 static inline Eigen::Vector3f 00098 getEigenVector3f (const PointWithRange& point); 00099 00104 PCL_EXPORTS static void 00105 getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame, 00106 Eigen::Affine3f& transformation); 00107 00113 template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f 00114 getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud); 00115 00120 PCL_EXPORTS static void 00121 extractFarRanges (const sensor_msgs::PointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges); 00122 00123 // =====METHODS===== 00125 inline Ptr 00126 makeShared () { return Ptr (new RangeImage (*this)); } 00127 00129 PCL_EXPORTS void 00130 reset (); 00131 00145 template <typename PointCloudType> void 00146 createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution, float max_angle_width, 00147 float max_angle_height, const Eigen::Affine3f& sensor_pose, 00148 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, 00149 float min_range=0.0f, int border_size=0); 00150 00165 template <typename PointCloudType> void 00166 createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution, 00167 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, 00168 const Eigen::Affine3f& sensor_pose, 00169 CoordinateFrame coordinate_frame=CAMERA_FRAME, 00170 float noise_level=0.0f, float min_range=0.0f, int border_size=0); 00171 00187 template <typename PointCloudTypeWithViewpoints> void 00188 createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution, 00189 float max_angle_width, float max_angle_height, 00190 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, 00191 float min_range=0.0f, int border_size=0); 00192 00200 void 00201 createEmpty(float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity(), 00202 RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad(360.0f), float angle_height=pcl::deg2rad(180.0f)); 00203 00205 PCL_EXPORTS void 00206 integrateFarRanges (const PointCloud<PointWithViewpoint>& far_ranges); 00207 00215 PCL_EXPORTS void 00216 cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1); 00217 00222 PCL_EXPORTS float* 00223 getRangesArray () const; 00224 00227 inline const Eigen::Affine3f& 00228 getTransformationToRangeImageSystem () const { return (to_range_image_system_); } 00229 00232 inline void 00233 setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system); 00234 00237 inline const Eigen::Affine3f& 00238 getTransformationToWorldSystem () const { return to_world_system_;} 00239 00241 inline float 00242 getAngularResolution () const { return angular_resolution_;} 00243 00247 inline void 00248 setAngularResolution (float angular_resolution); 00249 00255 inline const PointWithRange& 00256 getPoint (int image_x, int image_y) const; 00257 00259 inline PointWithRange& 00260 getPoint (int image_x, int image_y); 00261 00263 inline const PointWithRange& 00264 getPoint (float image_x, float image_y) const; 00265 00267 inline PointWithRange& 00268 getPoint (float image_x, float image_y); 00269 00276 inline const PointWithRange& 00277 getPointNoCheck (int image_x, int image_y) const; 00278 00280 inline PointWithRange& 00281 getPointNoCheck (int image_x, int image_y); 00282 00284 inline void 00285 getPoint (int image_x, int image_y, Eigen::Vector3f& point) const; 00286 00288 inline void 00289 getPoint (int index, Eigen::Vector3f& point) const; 00290 00292 inline const Eigen::Map<const Eigen::Vector3f> 00293 getEigenVector3f (int x, int y) const; 00294 00296 inline const Eigen::Map<const Eigen::Vector3f> 00297 getEigenVector3f (int index) const; 00298 00300 inline const PointWithRange& 00301 getPoint (int index) const; 00302 00309 inline const PointWithRange& 00310 getPointConsideringWrapAround (int image_x, int image_y) const; 00311 00313 inline void 00314 calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const; 00316 inline void 00317 calculate3DPoint (float image_x, float image_y, PointWithRange& point) const; 00318 00320 virtual inline void 00321 calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const; 00323 inline void 00324 calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const; 00325 00327 PCL_EXPORTS void 00328 recalculate3DPointPositions (); 00329 00331 inline virtual void 00332 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const; 00333 00335 inline void 00336 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const; 00337 00339 inline void 00340 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const; 00341 00343 inline void 00344 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const; 00345 00347 inline void 00348 getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const; 00349 00351 inline void 00352 getImagePoint (float x, float y, float z, float& image_x, float& image_y) const; 00353 00355 inline void 00356 getImagePoint (float x, float y, float z, int& image_x, int& image_y) const; 00357 00360 inline float 00361 checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const; 00362 00366 inline float 00367 getRangeDifference (const Eigen::Vector3f& point) const; 00368 00370 inline void 00371 getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const; 00372 00374 inline void 00375 getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const; 00376 00378 inline void 00379 real2DToInt2D (float x, float y, int& xInt, int& yInt) const; 00380 00382 inline bool 00383 isInImage (int x, int y) const; 00384 00386 inline bool 00387 isValid (int x, int y) const; 00388 00390 inline bool 00391 isValid (int index) const; 00392 00394 inline bool 00395 isObserved (int x, int y) const; 00396 00398 inline bool 00399 isMaxRange (int x, int y) const; 00400 00404 inline bool 00405 getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const; 00406 00408 inline bool 00409 getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point, 00410 int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const; 00411 00413 inline bool 00414 getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, 00415 int no_of_nearest_neighbors, Eigen::Vector3f& normal, 00416 Eigen::Vector3f* point_on_plane=NULL, int step_size=1) const; 00417 00419 inline bool 00420 getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const; 00421 00424 inline bool 00425 getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, 00426 int no_of_closest_neighbors, int step_size, 00427 float& max_closest_neighbor_distance_squared, 00428 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values, 00429 Eigen::Vector3f* normal_all_neighbors=NULL, 00430 Eigen::Vector3f* mean_all_neighbors=NULL, 00431 Eigen::Vector3f* eigen_values_all_neighbors=NULL) const; 00432 00433 // Return the squared distance to the n-th neighbors of the point at x,y 00434 inline float 00435 getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const; 00436 00439 inline float 00440 getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const; 00442 inline float 00443 getImpactAngle (int x1, int y1, int x2, int y2) const; 00444 00447 inline float 00448 getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const; 00450 PCL_EXPORTS float* 00451 getImpactAngleImageBasedOnLocalNormals (int radius) const; 00452 00456 inline float 00457 getNormalBasedAcutenessValue (int x, int y, int radius) const; 00458 00461 inline float 00462 getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const; 00464 inline float 00465 getAcutenessValue (int x1, int y1, int x2, int y2) const; 00466 00468 PCL_EXPORTS void 00469 getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x, 00470 float*& acuteness_value_image_y) const; 00471 00474 //inline float 00475 // getSurfaceChange(const PointWithRange& point, const PointWithRange& neighbor1, 00476 // const PointWithRange& neighbor2) const; 00477 00479 PCL_EXPORTS float 00480 getSurfaceChange (int x, int y, int radius) const; 00481 00483 PCL_EXPORTS float* 00484 getSurfaceChangeImage (int radius) const; 00485 00488 inline void 00489 getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const; 00490 00492 PCL_EXPORTS void 00493 getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const; 00494 00496 inline float 00497 getCurvature (int x, int y, int radius, int step_size) const; 00498 00500 inline const Eigen::Vector3f 00501 getSensorPos () const; 00502 00504 PCL_EXPORTS void 00505 setUnseenToMaxRange (); 00506 00508 inline int 00509 getImageOffsetX () const { return image_offset_x_;} 00511 inline int 00512 getImageOffsetY () const { return image_offset_y_;} 00513 00515 inline void 00516 setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;} 00517 00518 00519 00533 virtual void 00534 getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width, 00535 int sub_image_height, int combine_pixels, RangeImage& sub_image) const; 00536 00538 virtual void 00539 getHalfImage(RangeImage& half_image) const; 00540 00542 PCL_EXPORTS void 00543 getMinMaxRanges (float& min_range, float& max_range) const; 00544 00546 PCL_EXPORTS void 00547 change3dPointsToLocalCoordinateFrame (); 00548 00553 PCL_EXPORTS float* 00554 getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const; 00555 00557 PCL_EXPORTS float* 00558 getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const; 00559 00561 inline Eigen::Affine3f 00562 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const; 00564 inline void 00565 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, 00566 Eigen::Affine3f& transformation) const; 00568 inline void 00569 getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const; 00570 00572 PCL_EXPORTS bool 00573 getNormalBasedUprightTransformation (const Eigen::Vector3f& point, 00574 float max_dist, Eigen::Affine3f& transformation) const; 00575 00578 PCL_EXPORTS void 00579 getIntegralImage (float*& integral_image, int*& valid_points_num_image) const; 00580 00582 PCL_EXPORTS void 00583 getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image, 00584 RangeImage& range_image) const; 00585 00587 PCL_EXPORTS void 00588 getBlurredImage (int blur_radius, RangeImage& range_image) const; 00589 00592 inline float 00593 getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const; 00595 inline float 00596 getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const; 00597 00599 PCL_EXPORTS void 00600 getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const; 00601 //void getLocalNormals(int radius) const; 00602 00607 inline void 00608 get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, 00609 PointWithRange& average_point) const; 00610 00612 PCL_EXPORTS Eigen::Affine3f 00613 doIcp (const VectorOfEigenVector3f& points, 00614 const Eigen::Affine3f& initial_guess, int search_radius, 00615 float max_distance_start, float max_distance_end, int num_iterations) const; 00616 00620 PCL_EXPORTS Eigen::Affine3f 00621 doIcp (const RangeImage& other_range_image, 00622 const Eigen::Affine3f& initial_guess, int search_radius, 00623 float max_distance_start, float max_distance_end, 00624 int num_iterations, int pixel_step_start=1, int pixel_step_end=1) const; 00625 00626 00628 struct ExtractedPlane { 00629 Eigen::Vector3f normal; 00630 float d; 00631 Eigen::Vector3f maximum_extensions; 00632 Eigen::Vector3f mean; 00633 Eigen::Vector3f eigen_values; 00634 Eigen::Vector3f eigen_vector1, 00635 eigen_vector2, 00636 eigen_vector3; 00637 std::vector<int> point_indices; 00638 }; 00639 00646 PCL_EXPORTS void 00647 extractPlanes (float initial_max_plane_error, std::vector<ExtractedPlane>& planes) const; 00648 00651 static inline bool 00652 isLargerPlane (const ExtractedPlane& p1, const ExtractedPlane& p2) 00653 { return p1.maximum_extensions[2] > p2.maximum_extensions[2]; } 00654 00657 PCL_EXPORTS float 00658 getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation, 00659 int search_radius, float max_distance, int pixel_step=1) const; 00660 00662 inline bool 00663 getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const; 00664 00666 inline void 00667 getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const; 00668 00671 virtual RangeImage* 00672 getNew () const { return new RangeImage; } 00673 00674 00675 // =====MEMBER VARIABLES===== 00676 // BaseClass: 00677 // roslib::Header header; 00678 // std::vector<PointT> points; 00679 // uint32_t width; 00680 // uint32_t height; 00681 // bool is_dense; 00682 00683 static bool debug; 00685 protected: 00686 // =====PROTECTED MEMBER VARIABLES===== 00687 Eigen::Affine3f to_range_image_system_; 00688 Eigen::Affine3f to_world_system_; 00689 float angular_resolution_; 00690 float angular_resolution_reciprocal_; 00692 int image_offset_x_, image_offset_y_; 00694 PointWithRange unobserved_point; 00697 // =====PROTECTED METHODS===== 00698 template <typename PointCloudType> void 00699 doZBuffer (const PointCloudType& point_cloud, float noise_level, 00700 float min_range, int& top, int& right, int& bottom, int& left); 00701 00702 // =====STATIC PROTECTED===== 00703 static const int lookup_table_size; 00704 static std::vector<float> asin_lookup_table; 00705 static std::vector<float> atan_lookup_table; 00706 static std::vector<float> cos_lookup_table; 00708 static void 00709 createLookupTables (); 00710 00712 static inline float 00713 asinLookUp (float value); 00714 00716 static inline float 00717 atan2LookUp (float y, float x); 00718 00720 static inline float 00721 cosLookUp (float value); 00722 00723 00724 public: 00725 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00726 }; 00727 00731 inline std::ostream& 00732 operator<< (std::ostream& os, const RangeImage& r) 00733 { 00734 os << "header: " << std::endl; 00735 os << r.header; 00736 os << "points[]: " << r.points.size () << std::endl; 00737 os << "width: " << r.width << std::endl; 00738 os << "height: " << r.height << std::endl; 00739 os << "sensor_origin_: " 00740 << r.sensor_origin_[0] << ' ' 00741 << r.sensor_origin_[1] << ' ' 00742 << r.sensor_origin_[2] << std::endl; 00743 os << "sensor_orientation_: " 00744 << r.sensor_orientation_.x () << ' ' 00745 << r.sensor_orientation_.y () << ' ' 00746 << r.sensor_orientation_.z () << ' ' 00747 << r.sensor_orientation_.w () << std::endl; 00748 os << "is_dense: " << r.is_dense << std::endl; 00749 os << "angular resolution: " 00750 << RAD2DEG (r.getAngularResolution ()) << "deg/pixel" << std::endl; 00751 return (os); 00752 } 00753 00754 } // namespace end 00755 00756 00757 #include "pcl/range_image/impl/range_image.hpp" // Definitions of templated and inline functions 00758 00759 #endif //#ifndef PCL_RANGE_IMAGE_H_