|
Point Cloud Library (PCL)
1.5.1
|
|
Point Cloud Library (PCL)
1.5.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 00146 template <typename PointCloudType> void 00147 createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution, float max_angle_width, 00148 float max_angle_height, 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 00167 template <typename PointCloudType> void 00168 createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution, 00169 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, 00170 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity(), 00171 CoordinateFrame coordinate_frame=CAMERA_FRAME, 00172 float noise_level=0.0f, float min_range=0.0f, int border_size=0); 00173 00189 template <typename PointCloudTypeWithViewpoints> void 00190 createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution, 00191 float max_angle_width, float max_angle_height, 00192 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, 00193 float min_range=0.0f, int border_size=0); 00194 00202 void 00203 createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity(), 00204 RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad(360.0f), float angle_height=pcl::deg2rad(180.0f)); 00205 00207 PCL_EXPORTS void 00208 integrateFarRanges (const PointCloud<PointWithViewpoint>& far_ranges); 00209 00217 PCL_EXPORTS void 00218 cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1); 00219 00224 PCL_EXPORTS float* 00225 getRangesArray () const; 00226 00229 inline const Eigen::Affine3f& 00230 getTransformationToRangeImageSystem () const { return (to_range_image_system_); } 00231 00234 inline void 00235 setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system); 00236 00239 inline const Eigen::Affine3f& 00240 getTransformationToWorldSystem () const { return to_world_system_;} 00241 00243 inline float 00244 getAngularResolution () const { return angular_resolution_;} 00245 00249 inline void 00250 setAngularResolution (float angular_resolution); 00251 00257 inline const PointWithRange& 00258 getPoint (int image_x, int image_y) const; 00259 00261 inline PointWithRange& 00262 getPoint (int image_x, int image_y); 00263 00265 inline const PointWithRange& 00266 getPoint (float image_x, float image_y) const; 00267 00269 inline PointWithRange& 00270 getPoint (float image_x, float image_y); 00271 00278 inline const PointWithRange& 00279 getPointNoCheck (int image_x, int image_y) const; 00280 00282 inline PointWithRange& 00283 getPointNoCheck (int image_x, int image_y); 00284 00286 inline void 00287 getPoint (int image_x, int image_y, Eigen::Vector3f& point) const; 00288 00290 inline void 00291 getPoint (int index, Eigen::Vector3f& point) const; 00292 00294 inline const Eigen::Map<const Eigen::Vector3f> 00295 getEigenVector3f (int x, int y) const; 00296 00298 inline const Eigen::Map<const Eigen::Vector3f> 00299 getEigenVector3f (int index) const; 00300 00302 inline const PointWithRange& 00303 getPoint (int index) const; 00304 00311 inline const PointWithRange& 00312 getPointConsideringWrapAround (int image_x, int image_y) const; 00313 00315 inline void 00316 calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const; 00318 inline void 00319 calculate3DPoint (float image_x, float image_y, PointWithRange& point) const; 00320 00322 virtual inline void 00323 calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const; 00325 inline void 00326 calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const; 00327 00329 PCL_EXPORTS void 00330 recalculate3DPointPositions (); 00331 00333 inline virtual void 00334 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const; 00335 00337 inline void 00338 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const; 00339 00341 inline void 00342 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const; 00343 00345 inline void 00346 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const; 00347 00349 inline void 00350 getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const; 00351 00353 inline void 00354 getImagePoint (float x, float y, float z, float& image_x, float& image_y) const; 00355 00357 inline void 00358 getImagePoint (float x, float y, float z, int& image_x, int& image_y) const; 00359 00362 inline float 00363 checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const; 00364 00368 inline float 00369 getRangeDifference (const Eigen::Vector3f& point) const; 00370 00372 inline void 00373 getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const; 00374 00376 inline void 00377 getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const; 00378 00380 inline void 00381 real2DToInt2D (float x, float y, int& xInt, int& yInt) const; 00382 00384 inline bool 00385 isInImage (int x, int y) const; 00386 00388 inline bool 00389 isValid (int x, int y) const; 00390 00392 inline bool 00393 isValid (int index) const; 00394 00396 inline bool 00397 isObserved (int x, int y) const; 00398 00400 inline bool 00401 isMaxRange (int x, int y) const; 00402 00406 inline bool 00407 getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const; 00408 00410 inline bool 00411 getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point, 00412 int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const; 00413 00415 inline bool 00416 getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, 00417 int no_of_nearest_neighbors, Eigen::Vector3f& normal, 00418 Eigen::Vector3f* point_on_plane=NULL, int step_size=1) const; 00419 00421 inline bool 00422 getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const; 00423 00426 inline bool 00427 getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, 00428 int no_of_closest_neighbors, int step_size, 00429 float& max_closest_neighbor_distance_squared, 00430 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values, 00431 Eigen::Vector3f* normal_all_neighbors=NULL, 00432 Eigen::Vector3f* mean_all_neighbors=NULL, 00433 Eigen::Vector3f* eigen_values_all_neighbors=NULL) const; 00434 00435 // Return the squared distance to the n-th neighbors of the point at x,y 00436 inline float 00437 getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const; 00438 00441 inline float 00442 getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const; 00444 inline float 00445 getImpactAngle (int x1, int y1, int x2, int y2) const; 00446 00449 inline float 00450 getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const; 00452 PCL_EXPORTS float* 00453 getImpactAngleImageBasedOnLocalNormals (int radius) const; 00454 00458 inline float 00459 getNormalBasedAcutenessValue (int x, int y, int radius) const; 00460 00463 inline float 00464 getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const; 00466 inline float 00467 getAcutenessValue (int x1, int y1, int x2, int y2) const; 00468 00470 PCL_EXPORTS void 00471 getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x, 00472 float*& acuteness_value_image_y) const; 00473 00476 //inline float 00477 // getSurfaceChange(const PointWithRange& point, const PointWithRange& neighbor1, 00478 // const PointWithRange& neighbor2) const; 00479 00481 PCL_EXPORTS float 00482 getSurfaceChange (int x, int y, int radius) const; 00483 00485 PCL_EXPORTS float* 00486 getSurfaceChangeImage (int radius) const; 00487 00490 inline void 00491 getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const; 00492 00494 PCL_EXPORTS void 00495 getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const; 00496 00498 inline float 00499 getCurvature (int x, int y, int radius, int step_size) const; 00500 00502 inline const Eigen::Vector3f 00503 getSensorPos () const; 00504 00506 PCL_EXPORTS void 00507 setUnseenToMaxRange (); 00508 00510 inline int 00511 getImageOffsetX () const { return image_offset_x_;} 00513 inline int 00514 getImageOffsetY () const { return image_offset_y_;} 00515 00517 inline void 00518 setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;} 00519 00520 00521 00535 virtual void 00536 getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width, 00537 int sub_image_height, int combine_pixels, RangeImage& sub_image) const; 00538 00540 virtual void 00541 getHalfImage(RangeImage& half_image) const; 00542 00544 PCL_EXPORTS void 00545 getMinMaxRanges (float& min_range, float& max_range) const; 00546 00548 PCL_EXPORTS void 00549 change3dPointsToLocalCoordinateFrame (); 00550 00555 PCL_EXPORTS float* 00556 getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const; 00557 00559 PCL_EXPORTS float* 00560 getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const; 00561 00563 inline Eigen::Affine3f 00564 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const; 00566 inline void 00567 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, 00568 Eigen::Affine3f& transformation) const; 00570 inline void 00571 getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const; 00572 00574 PCL_EXPORTS bool 00575 getNormalBasedUprightTransformation (const Eigen::Vector3f& point, 00576 float max_dist, Eigen::Affine3f& transformation) const; 00577 00580 PCL_EXPORTS void 00581 getIntegralImage (float*& integral_image, int*& valid_points_num_image) const; 00582 00584 PCL_EXPORTS void 00585 getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image, 00586 RangeImage& range_image) const; 00587 00589 PCL_EXPORTS void 00590 getBlurredImage (int blur_radius, RangeImage& range_image) const; 00591 00594 inline float 00595 getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const; 00597 inline float 00598 getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const; 00599 00601 PCL_EXPORTS void 00602 getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const; 00603 //void getLocalNormals(int radius) const; 00604 00609 inline void 00610 get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, 00611 PointWithRange& average_point) const; 00612 00614 PCL_EXPORTS Eigen::Affine3f 00615 doIcp (const VectorOfEigenVector3f& points, 00616 const Eigen::Affine3f& initial_guess, int search_radius, 00617 float max_distance_start, float max_distance_end, int num_iterations) const; 00618 00622 PCL_EXPORTS Eigen::Affine3f 00623 doIcp (const RangeImage& other_range_image, 00624 const Eigen::Affine3f& initial_guess, int search_radius, 00625 float max_distance_start, float max_distance_end, 00626 int num_iterations, int pixel_step_start=1, int pixel_step_end=1) const; 00627 00628 00630 struct ExtractedPlane { 00631 Eigen::Vector3f normal; 00632 float d; 00633 Eigen::Vector3f maximum_extensions; 00634 Eigen::Vector3f mean; 00635 Eigen::Vector3f eigen_values; 00636 Eigen::Vector3f eigen_vector1, 00637 eigen_vector2, 00638 eigen_vector3; 00639 std::vector<int> point_indices; 00640 }; 00641 00648 PCL_EXPORTS void 00649 extractPlanes (float initial_max_plane_error, std::vector<ExtractedPlane>& planes) const; 00650 00653 static inline bool 00654 isLargerPlane (const ExtractedPlane& p1, const ExtractedPlane& p2) 00655 { return p1.maximum_extensions[2] > p2.maximum_extensions[2]; } 00656 00659 PCL_EXPORTS float 00660 getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation, 00661 int search_radius, float max_distance, int pixel_step=1) const; 00662 00664 inline bool 00665 getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const; 00666 00668 inline void 00669 getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const; 00670 00673 virtual RangeImage* 00674 getNew () const { return new RangeImage; } 00675 00676 00677 // =====MEMBER VARIABLES===== 00678 // BaseClass: 00679 // roslib::Header header; 00680 // std::vector<PointT> points; 00681 // uint32_t width; 00682 // uint32_t height; 00683 // bool is_dense; 00684 00685 static bool debug; 00687 protected: 00688 // =====PROTECTED MEMBER VARIABLES===== 00689 Eigen::Affine3f to_range_image_system_; 00690 Eigen::Affine3f to_world_system_; 00691 float angular_resolution_; 00692 float angular_resolution_reciprocal_; 00694 int image_offset_x_, image_offset_y_; 00696 PointWithRange unobserved_point; 00699 // =====PROTECTED METHODS===== 00700 template <typename PointCloudType> void 00701 doZBuffer (const PointCloudType& point_cloud, float noise_level, 00702 float min_range, int& top, int& right, int& bottom, int& left); 00703 00704 // =====STATIC PROTECTED===== 00705 static const int lookup_table_size; 00706 static std::vector<float> asin_lookup_table; 00707 static std::vector<float> atan_lookup_table; 00708 static std::vector<float> cos_lookup_table; 00710 static void 00711 createLookupTables (); 00712 00714 static inline float 00715 asinLookUp (float value); 00716 00718 static inline float 00719 atan2LookUp (float y, float x); 00720 00722 static inline float 00723 cosLookUp (float value); 00724 00725 00726 public: 00727 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00728 }; 00729 00733 inline std::ostream& 00734 operator<< (std::ostream& os, const RangeImage& r) 00735 { 00736 os << "header: " << std::endl; 00737 os << r.header; 00738 os << "points[]: " << r.points.size () << std::endl; 00739 os << "width: " << r.width << std::endl; 00740 os << "height: " << r.height << std::endl; 00741 os << "sensor_origin_: " 00742 << r.sensor_origin_[0] << ' ' 00743 << r.sensor_origin_[1] << ' ' 00744 << r.sensor_origin_[2] << std::endl; 00745 os << "sensor_orientation_: " 00746 << r.sensor_orientation_.x () << ' ' 00747 << r.sensor_orientation_.y () << ' ' 00748 << r.sensor_orientation_.z () << ' ' 00749 << r.sensor_orientation_.w () << std::endl; 00750 os << "is_dense: " << r.is_dense << std::endl; 00751 os << "angular resolution: " 00752 << RAD2DEG (r.getAngularResolution ()) << "deg/pixel" << std::endl; 00753 return (os); 00754 } 00755 00756 } // namespace end 00757 00758 00759 #include "pcl/range_image/impl/range_image.hpp" // Definitions of templated and inline functions 00760 00761 #endif //#ifndef PCL_RANGE_IMAGE_H_
1.8.0
