Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 */ 00037 00038 #ifndef PCL_RANGE_IMAGE_H_ 00039 #define PCL_RANGE_IMAGE_H_ 00040 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 { 00050 /** \brief RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where 00051 * a 3D scene was captured from a specific view point. 00052 * \author Bastian Steder 00053 * \ingroup range_image 00054 */ 00055 class RangeImage : public pcl::PointCloud<PointWithRange> 00056 { 00057 public: 00058 // =====TYPEDEFS===== 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 // =====CONSTRUCTOR & DESTRUCTOR===== 00072 /** Constructor */ 00073 PCL_EXPORTS RangeImage (); 00074 /** Destructor */ 00075 PCL_EXPORTS virtual ~RangeImage (); 00076 00077 // =====STATIC VARIABLES===== 00078 /** The maximum number of openmp threads that can be used in this class */ 00079 static int max_no_of_threads; 00080 00081 // =====STATIC METHODS===== 00082 /** \brief Get the size of a certain area when seen from the given pose 00083 * \param viewer_pose an affine matrix defining the pose of the viewer 00084 * \param center the center of the area 00085 * \param radius the radius of the area 00086 * \return the size of the area as viewed according to \a viewer_pose 00087 */ 00088 static inline float 00089 getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, 00090 float radius); 00091 00092 /** \brief Get Eigen::Vector3f from PointWithRange 00093 * \param point the input point 00094 * \return an Eigen::Vector3f representation of the input point 00095 */ 00096 static inline Eigen::Vector3f 00097 getEigenVector3f (const PointWithRange& point); 00098 00099 /** \brief Get the transformation that transforms the given coordinate frame into CAMERA_FRAME 00100 * \param coordinate_frame the input coordinate frame 00101 * \param transformation the resulting transformation that warps \a coordinate_frame into CAMERA_FRAME 00102 */ 00103 PCL_EXPORTS static void 00104 getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame, 00105 Eigen::Affine3f& transformation); 00106 00107 /** \brief Get the average viewpoint of a point cloud where each point carries viewpoint information as 00108 * vp_x, vp_y, vp_z 00109 * \param point_cloud the input point cloud 00110 * \return the average viewpoint (as an Eigen::Vector3f) 00111 */ 00112 template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f 00113 getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud); 00114 00115 /** \brief Check if the provided data includes far ranges and add them to far_ranges 00116 * \param point_cloud_data a PCLPointCloud2 message containing the input cloud 00117 * \param far_ranges the resulting cloud containing those points with far ranges 00118 */ 00119 PCL_EXPORTS static void 00120 extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges); 00121 00122 // =====METHODS===== 00123 /** \brief Get a boost shared pointer of a copy of this */ 00124 inline Ptr 00125 makeShared () { return Ptr (new RangeImage (*this)); } 00126 00127 /** \brief Reset all values to an empty range image */ 00128 PCL_EXPORTS void 00129 reset (); 00130 00131 /** \brief Create the depth image from a point cloud 00132 * \param point_cloud the input point cloud 00133 * \param angular_resolution the angular difference (in radians) between the individual pixels in the image 00134 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 00135 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 00136 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 00137 * Eigen::Affine3f::Identity () ) 00138 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00139 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 00140 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 00141 * will always take the minimum per cell. 00142 * \param min_range the minimum visible range (defaults to 0) 00143 * \param border_size the border size (defaults to 0) 00144 */ 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 00152 /** \brief Create the depth image from a point cloud 00153 * \param point_cloud the input point cloud 00154 * \param angular_resolution_x the angular difference (in radians) between the 00155 * individual pixels in the image in the x-direction 00156 * \param angular_resolution_y the angular difference (in radians) between the 00157 * individual pixels in the image in the y-direction 00158 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 00159 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 00160 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 00161 * Eigen::Affine3f::Identity () ) 00162 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00163 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 00164 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 00165 * will always take the minimum per cell. 00166 * \param min_range the minimum visible range (defaults to 0) 00167 * \param border_size the border size (defaults to 0) 00168 */ 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 00177 /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for 00178 * faster calculation. 00179 * \param point_cloud the input point cloud 00180 * \param angular_resolution the angle (in radians) between each sample in the depth image 00181 * \param point_cloud_center the center of bounding sphere 00182 * \param point_cloud_radius the radius of the bounding sphere 00183 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 00184 * Eigen::Affine3f::Identity () ) 00185 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00186 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 00187 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 00188 * will always take the minimum per cell. 00189 * \param min_range the minimum visible range (defaults to 0) 00190 * \param border_size the border size (defaults to 0) 00191 */ 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 00199 /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for 00200 * faster calculation. 00201 * \param point_cloud the input point cloud 00202 * \param angular_resolution_x the angular difference (in radians) between the 00203 * individual pixels in the image in the x-direction 00204 * \param angular_resolution_y the angular difference (in radians) between the 00205 * individual pixels in the image in the y-direction 00206 * \param angular_resolution the angle (in radians) between each sample in the depth image 00207 * \param point_cloud_center the center of bounding sphere 00208 * \param point_cloud_radius the radius of the bounding sphere 00209 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 00210 * Eigen::Affine3f::Identity () ) 00211 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00212 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 00213 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 00214 * will always take the minimum per cell. 00215 * \param min_range the minimum visible range (defaults to 0) 00216 * \param border_size the border size (defaults to 0) 00217 */ 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 00226 /** \brief Create the depth image from a point cloud, using the average viewpoint of the points 00227 * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)). 00228 * \param point_cloud the input point cloud 00229 * \param angular_resolution the angle (in radians) between each sample in the depth image 00230 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 00231 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 00232 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00233 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 00234 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 00235 * will always take the minimum per cell. 00236 * \param min_range the minimum visible range (defaults to 0) 00237 * \param border_size the border size (defaults to 0) 00238 * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame 00239 * 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 00240 * to the bottom and z to the front) */ 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 00247 /** \brief Create the depth image from a point cloud, using the average viewpoint of the points 00248 * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)). 00249 * \param point_cloud the input point cloud 00250 * \param angular_resolution_x the angular difference (in radians) between the 00251 * individual pixels in the image in the x-direction 00252 * \param angular_resolution_y the angular difference (in radians) between the 00253 * individual pixels in the image in the y-direction 00254 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 00255 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 00256 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00257 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 00258 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 00259 * will always take the minimum per cell. 00260 * \param min_range the minimum visible range (defaults to 0) 00261 * \param border_size the border size (defaults to 0) 00262 * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame 00263 * 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 00264 * to the bottom and z to the front) */ 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 00272 /** \brief Create an empty depth image (filled with unobserved points) 00273 * \param[in] angular_resolution the angle (in radians) between each sample in the depth image 00274 * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () ) 00275 * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00276 * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg)) 00277 * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg)) 00278 */ 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 00284 /** \brief Create an empty depth image (filled with unobserved points) 00285 * \param angular_resolution_x the angular difference (in radians) between the 00286 * individual pixels in the image in the x-direction 00287 * \param angular_resolution_y the angular difference (in radians) between the 00288 * individual pixels in the image in the y-direction 00289 * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () ) 00290 * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 00291 * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg)) 00292 * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg)) 00293 */ 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 00300 /** \brief Integrate the given point cloud into the current range image using a z-buffer 00301 * \param point_cloud the input point cloud 00302 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 00303 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 00304 * will always take the minimum per cell. 00305 * \param min_range the minimum visible range 00306 * \param top returns the minimum y pixel position in the image where a point was added 00307 * \param right returns the maximum x pixel position in the image where a point was added 00308 * \param bottom returns the maximum y pixel position in the image where a point was added 00309 * \param top returns the minimum y position in the image where a point was added 00310 * \param left returns the minimum x pixel position in the image where a point was added 00311 */ 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 00316 /** \brief Integrates the given far range measurements into the range image */ 00317 template <typename PointCloudType> void 00318 integrateFarRanges (const PointCloudType& far_ranges); 00319 00320 /** \brief Cut the range image to the minimal size so that it still contains all actual range readings. 00321 * \param border_size allows increase from the minimal size by the specified number of pixels (defaults to 0) 00322 * \param top if positive, this value overrides the position of the top edge (defaults to -1) 00323 * \param right if positive, this value overrides the position of the right edge (defaults to -1) 00324 * \param bottom if positive, this value overrides the position of the bottom edge (defaults to -1) 00325 * \param left if positive, this value overrides the position of the left edge (defaults to -1) 00326 */ 00327 PCL_EXPORTS void 00328 cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1); 00329 00330 /** \brief Get all the range values in one float array of size width*height 00331 * \return a pointer to a new float array containing the range values 00332 * \note This method allocates a new float array; the caller is responsible for freeing this memory. 00333 */ 00334 PCL_EXPORTS float* 00335 getRangesArray () const; 00336 00337 /** Getter for the transformation from the world system into the range image system 00338 * (the sensor coordinate frame) */ 00339 inline const Eigen::Affine3f& 00340 getTransformationToRangeImageSystem () const { return (to_range_image_system_); } 00341 00342 /** Setter for the transformation from the range image system 00343 * (the sensor coordinate frame) into the world system */ 00344 inline void 00345 setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system); 00346 00347 /** Getter for the transformation from the range image system 00348 * (the sensor coordinate frame) into the world system */ 00349 inline const Eigen::Affine3f& 00350 getTransformationToWorldSystem () const { return to_world_system_;} 00351 00352 /** Getter for the angular resolution of the range image in x direction in radians per pixel. 00353 * Provided for downwards compatability */ 00354 inline float 00355 getAngularResolution () const { return angular_resolution_x_;} 00356 00357 /** Getter for the angular resolution of the range image in x direction in radians per pixel. */ 00358 inline float 00359 getAngularResolutionX () const { return angular_resolution_x_;} 00360 00361 /** Getter for the angular resolution of the range image in y direction in radians per pixel. */ 00362 inline float 00363 getAngularResolutionY () const { return angular_resolution_y_;} 00364 00365 /** Getter for the angular resolution of the range image in x and y direction (in radians). */ 00366 inline void 00367 getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const; 00368 00369 /** \brief Set the angular resolution of the range image 00370 * \param angular_resolution the new angular resolution in x and y direction (in radians per pixel) 00371 */ 00372 inline void 00373 setAngularResolution (float angular_resolution); 00374 00375 /** \brief Set the angular resolution of the range image 00376 * \param angular_resolution_x the new angular resolution in x direction (in radians per pixel) 00377 * \param angular_resolution_y the new angular resolution in y direction (in radians per pixel) 00378 */ 00379 inline void 00380 setAngularResolution (float angular_resolution_x, float angular_resolution_y); 00381 00382 00383 /** \brief Return the 3D point with range at the given image position 00384 * \param image_x the x coordinate 00385 * \param image_y the y coordinate 00386 * \return the point at the specified location (returns unobserved_point if outside of the image bounds) 00387 */ 00388 inline const PointWithRange& 00389 getPoint (int image_x, int image_y) const; 00390 00391 /** \brief Non-const-version of getPoint */ 00392 inline PointWithRange& 00393 getPoint (int image_x, int image_y); 00394 00395 /** Return the 3d point with range at the given image position */ 00396 inline const PointWithRange& 00397 getPoint (float image_x, float image_y) const; 00398 00399 /** Non-const-version of the above */ 00400 inline PointWithRange& 00401 getPoint (float image_x, float image_y); 00402 00403 /** \brief Return the 3D point with range at the given image position. This methd performs no error checking 00404 * to make sure the specified image position is inside of the image! 00405 * \param image_x the x coordinate 00406 * \param image_y the y coordinate 00407 * \return the point at the specified location (program may fail if the location is outside of the image bounds) 00408 */ 00409 inline const PointWithRange& 00410 getPointNoCheck (int image_x, int image_y) const; 00411 00412 /** Non-const-version of getPointNoCheck */ 00413 inline PointWithRange& 00414 getPointNoCheck (int image_x, int image_y); 00415 00416 /** Same as above */ 00417 inline void 00418 getPoint (int image_x, int image_y, Eigen::Vector3f& point) const; 00419 00420 /** Same as above */ 00421 inline void 00422 getPoint (int index, Eigen::Vector3f& point) const; 00423 00424 /** Same as above */ 00425 inline const Eigen::Map<const Eigen::Vector3f> 00426 getEigenVector3f (int x, int y) const; 00427 00428 /** Same as above */ 00429 inline const Eigen::Map<const Eigen::Vector3f> 00430 getEigenVector3f (int index) const; 00431 00432 /** Return the 3d point with range at the given index (whereas index=y*width+x) */ 00433 inline const PointWithRange& 00434 getPoint (int index) const; 00435 00436 /** Calculate the 3D point according to the given image point and range */ 00437 inline void 00438 calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const; 00439 /** Calculate the 3D point according to the given image point and the range value at the closest pixel */ 00440 inline void 00441 calculate3DPoint (float image_x, float image_y, PointWithRange& point) const; 00442 00443 /** Calculate the 3D point according to the given image point and range */ 00444 virtual inline void 00445 calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const; 00446 /** Calculate the 3D point according to the given image point and the range value at the closest pixel */ 00447 inline void 00448 calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const; 00449 00450 /** Recalculate all 3D point positions according to their pixel position and range */ 00451 PCL_EXPORTS void 00452 recalculate3DPointPositions (); 00453 00454 /** Get imagePoint from 3D point in world coordinates */ 00455 inline virtual void 00456 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const; 00457 00458 /** Same as above */ 00459 inline void 00460 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const; 00461 00462 /** Same as above */ 00463 inline void 00464 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const; 00465 00466 /** Same as above */ 00467 inline void 00468 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const; 00469 00470 /** Same as above */ 00471 inline void 00472 getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const; 00473 00474 /** Same as above */ 00475 inline void 00476 getImagePoint (float x, float y, float z, float& image_x, float& image_y) const; 00477 00478 /** Same as above */ 00479 inline void 00480 getImagePoint (float x, float y, float z, int& image_x, int& image_y) const; 00481 00482 /** point_in_image will be the point in the image at the position the given point would be. Returns 00483 * the range of the given point. */ 00484 inline float 00485 checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const; 00486 00487 /** Returns the difference in range between the given point and the range of the point in the image 00488 * at the position the given point would be. 00489 * (Return value is point_in_image.range-given_point.range) */ 00490 inline float 00491 getRangeDifference (const Eigen::Vector3f& point) const; 00492 00493 /** Get the image point corresponding to the given angles */ 00494 inline void 00495 getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const; 00496 00497 /** Get the angles corresponding to the given image point */ 00498 inline void 00499 getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const; 00500 00501 /** Transforms an image point in float values to an image point in int values */ 00502 inline void 00503 real2DToInt2D (float x, float y, int& xInt, int& yInt) const; 00504 00505 /** Check if a point is inside of the image */ 00506 inline bool 00507 isInImage (int x, int y) const; 00508 00509 /** Check if a point is inside of the image and has a finite range */ 00510 inline bool 00511 isValid (int x, int y) const; 00512 00513 /** Check if a point has a finite range */ 00514 inline bool 00515 isValid (int index) const; 00516 00517 /** Check if a point is inside of the image and has either a finite range or a max reading (range=INFINITY) */ 00518 inline bool 00519 isObserved (int x, int y) const; 00520 00521 /** Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! */ 00522 inline bool 00523 isMaxRange (int x, int y) const; 00524 00525 /** Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius. 00526 * step_size determines how many pixels are used. 1 means all, 2 only every second, etc.. 00527 * Returns false if it was unable to calculate a normal.*/ 00528 inline bool 00529 getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const; 00530 00531 /** Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered. */ 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 00536 /** Same as above */ 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 00542 /** Same as above, using default values */ 00543 inline bool 00544 getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const; 00545 00546 /** Same as above but extracts some more data and can also return the extracted 00547 * information for all neighbors in radius if normal_all_neighbors is not NULL */ 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 // Return the squared distance to the n-th neighbors of the point at x,y 00558 inline float 00559 getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const; 00560 00561 /** Calculate the impact angle based on the sensor position and the two given points - will return 00562 * -INFINITY if one of the points is unobserved */ 00563 inline float 00564 getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const; 00565 //! Same as above 00566 inline float 00567 getImpactAngle (int x1, int y1, int x2, int y2) const; 00568 00569 /** Extract a local normal (with a heuristic not to include background points) and calculate the impact 00570 * angle based on this */ 00571 inline float 00572 getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const; 00573 /** Uses the above function for every point in the image */ 00574 PCL_EXPORTS float* 00575 getImpactAngleImageBasedOnLocalNormals (int radius) const; 00576 00577 /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) 00578 * This uses getImpactAngleBasedOnLocalNormal 00579 * Will return -INFINITY if no normal could be calculated */ 00580 inline float 00581 getNormalBasedAcutenessValue (int x, int y, int radius) const; 00582 00583 /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) 00584 * will return -INFINITY if one of the points is unobserved */ 00585 inline float 00586 getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const; 00587 //! Same as above 00588 inline float 00589 getAcutenessValue (int x1, int y1, int x2, int y2) const; 00590 00591 /** Calculate getAcutenessValue for every point */ 00592 PCL_EXPORTS void 00593 getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x, 00594 float*& acuteness_value_image_y) const; 00595 00596 /** Calculates, how much the surface changes at a point. Pi meaning a flat suface and 0.0f 00597 * would be a needle point */ 00598 //inline float 00599 // getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, 00600 // const PointWithRange& neighbor2) const; 00601 00602 /** Calculates, how much the surface changes at a point. 1 meaning a 90deg angle and 0 a flat suface */ 00603 PCL_EXPORTS float 00604 getSurfaceChange (int x, int y, int radius) const; 00605 00606 /** Uses the above function for every point in the image */ 00607 PCL_EXPORTS float* 00608 getSurfaceChangeImage (int radius) const; 00609 00610 /** Calculates, how much the surface changes at a point. Returns an angle [0.0f, PI] for x and y direction. 00611 * A return value of -INFINITY means that a point was unobserved. */ 00612 inline void 00613 getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const; 00614 00615 /** Uses the above function for every point in the image */ 00616 PCL_EXPORTS void 00617 getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const; 00618 00619 /** Calculates the curvature in a point using pca */ 00620 inline float 00621 getCurvature (int x, int y, int radius, int step_size) const; 00622 00623 //! Get the sensor position 00624 inline const Eigen::Vector3f 00625 getSensorPos () const; 00626 00627 /** Sets all -INFINITY values to INFINITY */ 00628 PCL_EXPORTS void 00629 setUnseenToMaxRange (); 00630 00631 //! Getter for image_offset_x_ 00632 inline int 00633 getImageOffsetX () const { return image_offset_x_;} 00634 //! Getter for image_offset_y_ 00635 inline int 00636 getImageOffsetY () const { return image_offset_y_;} 00637 00638 //! Setter for image offsets 00639 inline void 00640 setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;} 00641 00642 00643 00644 /** Get a sub part of the complete image as a new range image. 00645 * \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image. 00646 * This is always according to absolute 0,0 meaning -180°,-90° 00647 * and it is already in the system of the new image, so the 00648 * actual pixel used in the original image is 00649 * combine_pixels* (image_offset_x-image_offset_x_) 00650 * \param sub_image_image_offset_y - Same as image_offset_x for the y coordinate 00651 * \param sub_image_width - width of the new image 00652 * \param sub_image_height - height of the new image 00653 * \param combine_pixels - shrinking factor, meaning the new angular resolution 00654 * is combine_pixels times the old one 00655 * \param sub_image - the output image 00656 */ 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 00661 //! Get a range image with half the resolution 00662 virtual void 00663 getHalfImage (RangeImage& half_image) const; 00664 00665 //! Find the minimum and maximum range in the image 00666 PCL_EXPORTS void 00667 getMinMaxRanges (float& min_range, float& max_range) const; 00668 00669 //! This function sets the sensor pose to 0 and transforms all point positions to this local coordinate frame 00670 PCL_EXPORTS void 00671 change3dPointsToLocalCoordinateFrame (); 00672 00673 /** Calculate a range patch as the z values of the coordinate frame given by pose. 00674 * The patch will have size pixel_size x pixel_size and each pixel 00675 * covers world_size/pixel_size meters in the world 00676 * You are responsible for deleting the structure afterwards! */ 00677 PCL_EXPORTS float* 00678 getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const; 00679 00680 //! Same as above, but using the local coordinate frame defined by point and the viewing direction 00681 PCL_EXPORTS float* 00682 getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const; 00683 00684 //! Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction 00685 inline Eigen::Affine3f 00686 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const; 00687 //! Same as above, using a reference for the retrurn value 00688 inline void 00689 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, 00690 Eigen::Affine3f& transformation) const; 00691 //! Same as above, but only returning the rotation 00692 inline void 00693 getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const; 00694 00695 /** Get a local coordinate frame at the given point based on the normal. */ 00696 PCL_EXPORTS bool 00697 getNormalBasedUprightTransformation (const Eigen::Vector3f& point, 00698 float max_dist, Eigen::Affine3f& transformation) const; 00699 00700 /** Get the integral image of the range values (used for fast blur operations). 00701 * You are responsible for deleting it after usage! */ 00702 PCL_EXPORTS void 00703 getIntegralImage (float*& integral_image, int*& valid_points_num_image) const; 00704 00705 /** Get a blurred version of the range image using box filters on the provided integral image*/ 00706 PCL_EXPORTS void // Template necessary so that this function also works in derived classes 00707 getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image, 00708 RangeImage& range_image) const; 00709 00710 /** Get a blurred version of the range image using box filters */ 00711 PCL_EXPORTS virtual void // Template necessary so that this function also works in derived classes 00712 getBlurredImage (int blur_radius, RangeImage& range_image) const; 00713 00714 /** Get the squared euclidean distance between the two image points. 00715 * Returns -INFINITY if one of the points was not observed */ 00716 inline float 00717 getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const; 00718 //! Doing the above for some steps in the given direction and averaging 00719 inline float 00720 getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const; 00721 00722 //! Project all points on the local plane approximation, thereby smoothing the surface of the scan 00723 PCL_EXPORTS void 00724 getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const; 00725 //void getLocalNormals (int radius) const; 00726 00727 /** Calculates the average 3D position of the no_of_points points described by the start 00728 * point x,y in the direction delta. 00729 * Returns a max range point (range=INFINITY) if the first point is max range and an 00730 * unobserved point (range=-INFINITY) if non of the points is observed. */ 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 00735 /** Calculates the overlap of two range images given the relative transformation 00736 * (from the given image to *this) */ 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 00741 /** Get the viewing direction for the given point */ 00742 inline bool 00743 getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const; 00744 00745 /** Get the viewing direction for the given point */ 00746 inline void 00747 getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const; 00748 00749 /** Return a newly created Range image. 00750 * Can be reimplmented in derived classes like RangeImagePlanar to return an image of the same type. */ 00751 PCL_EXPORTS virtual RangeImage* 00752 getNew () const { return new RangeImage; } 00753 00754 /** Copy other to *this. Necessary for use in virtual functions that need to copy derived RangeImage classes (like RangeImagePlanar) */ 00755 PCL_EXPORTS virtual void 00756 copyTo (RangeImage& other) const; 00757 00758 00759 // =====MEMBER VARIABLES===== 00760 // BaseClass: 00761 // roslib::Header header; 00762 // std::vector<PointT> points; 00763 // uint32_t width; 00764 // uint32_t height; 00765 // bool is_dense; 00766 00767 static bool debug; /**< Just for... well... debugging purposes. :-) */ 00768 00769 protected: 00770 // =====PROTECTED MEMBER VARIABLES===== 00771 Eigen::Affine3f to_range_image_system_; /**< Inverse of to_world_system_ */ 00772 Eigen::Affine3f to_world_system_; /**< Inverse of to_range_image_system_ */ 00773 float angular_resolution_x_; /**< Angular resolution of the range image in x direction in radians per pixel */ 00774 float angular_resolution_y_; /**< Angular resolution of the range image in y direction in radians per pixel */ 00775 float angular_resolution_x_reciprocal_; /**< 1.0/angular_resolution_x_ - provided for better performace of 00776 * multiplication compared to division */ 00777 float angular_resolution_y_reciprocal_; /**< 1.0/angular_resolution_y_ - provided for better performace of 00778 * multiplication compared to division */ 00779 int image_offset_x_, image_offset_y_; /**< Position of the top left corner of the range image compared to 00780 * an image of full size (360x180 degrees) */ 00781 PointWithRange unobserved_point; /**< This point is used to be able to return 00782 * a reference to a non-existing point */ 00783 00784 // =====PROTECTED METHODS===== 00785 00786 00787 // =====STATIC PROTECTED===== 00788 static const int lookup_table_size; 00789 static std::vector<float> asin_lookup_table; 00790 static std::vector<float> atan_lookup_table; 00791 static std::vector<float> cos_lookup_table; 00792 /** Create lookup tables for trigonometric functions */ 00793 static void 00794 createLookupTables (); 00795 00796 /** Query the asin lookup table */ 00797 static inline float 00798 asinLookUp (float value); 00799 00800 /** Query the atan2 lookup table */ 00801 static inline float 00802 atan2LookUp (float y, float x); 00803 00804 /** Query the cos lookup table */ 00805 static inline float 00806 cosLookUp (float value); 00807 00808 00809 public: 00810 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00811 }; 00812 00813 /** 00814 * /ingroup range_image 00815 */ 00816 inline std::ostream& 00817 operator<< (std::ostream& os, const RangeImage& r) 00818 { 00819 os << "header: " << std::endl; 00820 os << r.header; 00821 os << "points[]: " << r.points.size () << std::endl; 00822 os << "width: " << r.width << std::endl; 00823 os << "height: " << r.height << std::endl; 00824 os << "sensor_origin_: " 00825 << r.sensor_origin_[0] << ' ' 00826 << r.sensor_origin_[1] << ' ' 00827 << r.sensor_origin_[2] << std::endl; 00828 os << "sensor_orientation_: " 00829 << r.sensor_orientation_.x () << ' ' 00830 << r.sensor_orientation_.y () << ' ' 00831 << r.sensor_orientation_.z () << ' ' 00832 << r.sensor_orientation_.w () << std::endl; 00833 os << "is_dense: " << r.is_dense << std::endl; 00834 os << "angular resolution: " 00835 << RAD2DEG (r.getAngularResolutionX ()) << "deg/pixel in x and " 00836 << RAD2DEG (r.getAngularResolutionY ()) << "deg/pixel in y.\n" << std::endl; 00837 return (os); 00838 } 00839 00840 } // namespace end 00841 00842 00843 #include <pcl/range_image/impl/range_image.hpp> // Definitions of templated and inline functions 00844 00845 #endif //#ifndef PCL_RANGE_IMAGE_H_