1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2012, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 */ 37 38 #pragma once 39 40 #include <pcl/memory.h> 41 #include <pcl/point_cloud.h> 42 #include <pcl/pcl_macros.h> 43 #include <pcl/point_types.h> 44 #include <pcl/common/angles.h> // for deg2rad 45 namespace pcl { struct PCLPointCloud2; } 46 47 namespace pcl 48 { 49 /** \brief RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where 50 * a 3D scene was captured from a specific view point. 51 * \author Bastian Steder 52 * \ingroup range_image 53 */ 54 class RangeImage : public pcl::PointCloud<PointWithRange> 55 { 56 public: 57 // =====TYPEDEFS===== 58 using BaseClass = pcl::PointCloud<PointWithRange>; 59 using VectorOfEigenVector3f = std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >; 60 using Ptr = shared_ptr<RangeImage>; 61 using ConstPtr = shared_ptr<const RangeImage>; 62 63 enum CoordinateFrame 64 { 65 CAMERA_FRAME = 0, 66 LASER_FRAME = 1 67 }; 68 69 70 // =====CONSTRUCTOR & DESTRUCTOR===== 71 /** Constructor */ 72 PCL_EXPORTS RangeImage (); 73 /** Destructor */ 74 PCL_EXPORTS virtual ~RangeImage () = default; 75 76 // =====STATIC VARIABLES===== 77 /** The maximum number of openmp threads that can be used in this class */ 78 static int max_no_of_threads; 79 80 // =====STATIC METHODS===== 81 /** \brief Get the size of a certain area when seen from the given pose 82 * \param viewer_pose an affine matrix defining the pose of the viewer 83 * \param center the center of the area 84 * \param radius the radius of the area 85 * \return the size of the area as viewed according to \a viewer_pose 86 */ 87 static inline float 88 getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, 89 float radius); 90 91 /** \brief Get Eigen::Vector3f from PointWithRange 92 * \param point the input point 93 * \return an Eigen::Vector3f representation of the input point 94 */ 95 static inline Eigen::Vector3f 96 getEigenVector3f (const PointWithRange& point); 97 98 /** \brief Get the transformation that transforms the given coordinate frame into CAMERA_FRAME 99 * \param coordinate_frame the input coordinate frame 100 * \param transformation the resulting transformation that warps \a coordinate_frame into CAMERA_FRAME 101 */ 102 PCL_EXPORTS static void 103 getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame, 104 Eigen::Affine3f& transformation); 105 106 /** \brief Get the average viewpoint of a point cloud where each point carries viewpoint information as 107 * vp_x, vp_y, vp_z 108 * \param point_cloud the input point cloud 109 * \return the average viewpoint (as an Eigen::Vector3f) 110 */ 111 template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f 112 getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud); 113 114 /** \brief Check if the provided data includes far ranges and add them to far_ranges 115 * \param point_cloud_data a PCLPointCloud2 message containing the input cloud 116 * \param far_ranges the resulting cloud containing those points with far ranges 117 */ 118 PCL_EXPORTS static void 119 extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges); 120 121 // =====METHODS===== 122 /** \brief Get a boost shared pointer of a copy of this */ 123 inline Ptr makeShared()124 makeShared () { return Ptr (new RangeImage (*this)); } 125 126 /** \brief Reset all values to an empty range image */ 127 PCL_EXPORTS void 128 reset (); 129 130 /** \brief Create the depth image from a point cloud 131 * \param point_cloud the input point cloud 132 * \param angular_resolution the angular difference (in radians) between the individual pixels in the image 133 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 134 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 135 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 136 * Eigen::Affine3f::Identity () ) 137 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 138 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 139 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 140 * will always take the minimum per cell. 141 * \param min_range the minimum visible range (defaults to 0) 142 * \param border_size the border size (defaults to 0) 143 */ 144 template <typename PointCloudType> void 145 createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution=pcl::deg2rad (0.5f), 146 float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f), 147 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), 148 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, 149 float min_range=0.0f, int border_size=0); 150 151 /** \brief Create the depth image from a point cloud 152 * \param point_cloud the input point cloud 153 * \param angular_resolution_x the angular difference (in radians) between the 154 * individual pixels in the image in the x-direction 155 * \param angular_resolution_y the angular difference (in radians) between the 156 * individual pixels in the image in the y-direction 157 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 158 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 159 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 160 * Eigen::Affine3f::Identity () ) 161 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 162 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 163 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 164 * will always take the minimum per cell. 165 * \param min_range the minimum visible range (defaults to 0) 166 * \param border_size the border size (defaults to 0) 167 */ 168 template <typename PointCloudType> void 169 createFromPointCloud (const PointCloudType& point_cloud, 170 float angular_resolution_x=pcl::deg2rad (0.5f), float angular_resolution_y=pcl::deg2rad (0.5f), 171 float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f), 172 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), 173 CoordinateFrame coordinate_frame=CAMERA_FRAME, 174 float noise_level=0.0f, float min_range=0.0f, int border_size=0); 175 176 /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for 177 * faster calculation. 178 * \param point_cloud the input point cloud 179 * \param angular_resolution the angle (in radians) between each sample in the depth image 180 * \param point_cloud_center the center of bounding sphere 181 * \param point_cloud_radius the radius of the bounding sphere 182 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 183 * Eigen::Affine3f::Identity () ) 184 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 185 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 186 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 187 * will always take the minimum per cell. 188 * \param min_range the minimum visible range (defaults to 0) 189 * \param border_size the border size (defaults to 0) 190 */ 191 template <typename PointCloudType> void 192 createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution, 193 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, 194 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), 195 CoordinateFrame coordinate_frame=CAMERA_FRAME, 196 float noise_level=0.0f, float min_range=0.0f, int border_size=0); 197 198 /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for 199 * faster calculation. 200 * \param point_cloud the input point cloud 201 * \param angular_resolution_x the angular difference (in radians) between the 202 * individual pixels in the image in the x-direction 203 * \param angular_resolution_y the angular difference (in radians) between the 204 * individual pixels in the image in the y-direction 205 * \param point_cloud_center the center of bounding sphere 206 * \param point_cloud_radius the radius of the bounding sphere 207 * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to 208 * Eigen::Affine3f::Identity () ) 209 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 210 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 211 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 212 * will always take the minimum per cell. 213 * \param min_range the minimum visible range (defaults to 0) 214 * \param border_size the border size (defaults to 0) 215 */ 216 template <typename PointCloudType> void 217 createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, 218 float angular_resolution_x, float angular_resolution_y, 219 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, 220 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), 221 CoordinateFrame coordinate_frame=CAMERA_FRAME, 222 float noise_level=0.0f, float min_range=0.0f, int border_size=0); 223 224 /** \brief Create the depth image from a point cloud, using the average viewpoint of the points 225 * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)). 226 * \param point_cloud the input point cloud 227 * \param angular_resolution the angle (in radians) between each sample in the depth image 228 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 229 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 230 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 231 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 232 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 233 * will always take the minimum per cell. 234 * \param min_range the minimum visible range (defaults to 0) 235 * \param border_size the border size (defaults to 0) 236 * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame 237 * 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 238 * to the bottom and z to the front) */ 239 template <typename PointCloudTypeWithViewpoints> void 240 createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution, 241 float max_angle_width, float max_angle_height, 242 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, 243 float min_range=0.0f, int border_size=0); 244 245 /** \brief Create the depth image from a point cloud, using the average viewpoint of the points 246 * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)). 247 * \param point_cloud the input point cloud 248 * \param angular_resolution_x the angular difference (in radians) between the 249 * individual pixels in the image in the x-direction 250 * \param angular_resolution_y the angular difference (in radians) between the 251 * individual pixels in the image in the y-direction 252 * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor 253 * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor 254 * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 255 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 256 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 257 * will always take the minimum per cell. 258 * \param min_range the minimum visible range (defaults to 0) 259 * \param border_size the border size (defaults to 0) 260 * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame 261 * 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 262 * to the bottom and z to the front) */ 263 template <typename PointCloudTypeWithViewpoints> void 264 createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, 265 float angular_resolution_x, float angular_resolution_y, 266 float max_angle_width, float max_angle_height, 267 CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, 268 float min_range=0.0f, int border_size=0); 269 270 /** \brief Create an empty depth image (filled with unobserved points) 271 * \param[in] angular_resolution the angle (in radians) between each sample in the depth image 272 * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () ) 273 * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 274 * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg)) 275 * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg)) 276 */ 277 void 278 createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (), 279 RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f), 280 float angle_height=pcl::deg2rad (180.0f)); 281 282 /** \brief Create an empty depth image (filled with unobserved points) 283 * \param angular_resolution_x the angular difference (in radians) between the 284 * individual pixels in the image in the x-direction 285 * \param angular_resolution_y the angular difference (in radians) between the 286 * individual pixels in the image in the y-direction 287 * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () ) 288 * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) 289 * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg)) 290 * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg)) 291 */ 292 void 293 createEmpty (float angular_resolution_x, float angular_resolution_y, 294 const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (), 295 RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f), 296 float angle_height=pcl::deg2rad (180.0f)); 297 298 /** \brief Integrate the given point cloud into the current range image using a z-buffer 299 * \param point_cloud the input point cloud 300 * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, 301 * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and 302 * will always take the minimum per cell. 303 * \param min_range the minimum visible range 304 * \param top returns the minimum y pixel position in the image where a point was added 305 * \param right returns the maximum x pixel position in the image where a point was added 306 * \param bottom returns the maximum y pixel position in the image where a point was added 307 * \param left returns the minimum x pixel position in the image where a point was added 308 */ 309 template <typename PointCloudType> void 310 doZBuffer (const PointCloudType& point_cloud, float noise_level, 311 float min_range, int& top, int& right, int& bottom, int& left); 312 313 /** \brief Integrates the given far range measurements into the range image */ 314 template <typename PointCloudType> void 315 integrateFarRanges (const PointCloudType& far_ranges); 316 317 /** \brief Cut the range image to the minimal size so that it still contains all actual range readings. 318 * \param border_size allows increase from the minimal size by the specified number of pixels (defaults to 0) 319 * \param top if positive, this value overrides the position of the top edge (defaults to -1) 320 * \param right if positive, this value overrides the position of the right edge (defaults to -1) 321 * \param bottom if positive, this value overrides the position of the bottom edge (defaults to -1) 322 * \param left if positive, this value overrides the position of the left edge (defaults to -1) 323 */ 324 PCL_EXPORTS void 325 cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1); 326 327 /** \brief Get all the range values in one float array of size width*height 328 * \return a pointer to a new float array containing the range values 329 * \note This method allocates a new float array; the caller is responsible for freeing this memory. 330 */ 331 PCL_EXPORTS float* 332 getRangesArray () const; 333 334 /** Getter for the transformation from the world system into the range image system 335 * (the sensor coordinate frame) */ 336 inline const Eigen::Affine3f& getTransformationToRangeImageSystem()337 getTransformationToRangeImageSystem () const { return (to_range_image_system_); } 338 339 /** Setter for the transformation from the range image system 340 * (the sensor coordinate frame) into the world system */ 341 inline void 342 setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system); 343 344 /** Getter for the transformation from the range image system 345 * (the sensor coordinate frame) into the world system */ 346 inline const Eigen::Affine3f& getTransformationToWorldSystem()347 getTransformationToWorldSystem () const { return to_world_system_;} 348 349 /** Getter for the angular resolution of the range image in x direction in radians per pixel. 350 * Provided for downwards compatibility */ 351 inline float getAngularResolution()352 getAngularResolution () const { return angular_resolution_x_;} 353 354 /** Getter for the angular resolution of the range image in x direction in radians per pixel. */ 355 inline float getAngularResolutionX()356 getAngularResolutionX () const { return angular_resolution_x_;} 357 358 /** Getter for the angular resolution of the range image in y direction in radians per pixel. */ 359 inline float getAngularResolutionY()360 getAngularResolutionY () const { return angular_resolution_y_;} 361 362 /** Getter for the angular resolution of the range image in x and y direction (in radians). */ 363 inline void 364 getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const; 365 366 /** \brief Set the angular resolution of the range image 367 * \param angular_resolution the new angular resolution in x and y direction (in radians per pixel) 368 */ 369 inline void 370 setAngularResolution (float angular_resolution); 371 372 /** \brief Set the angular resolution of the range image 373 * \param angular_resolution_x the new angular resolution in x direction (in radians per pixel) 374 * \param angular_resolution_y the new angular resolution in y direction (in radians per pixel) 375 */ 376 inline void 377 setAngularResolution (float angular_resolution_x, float angular_resolution_y); 378 379 380 /** \brief Return the 3D point with range at the given image position 381 * \param image_x the x coordinate 382 * \param image_y the y coordinate 383 * \return the point at the specified location (returns unobserved_point if outside of the image bounds) 384 */ 385 inline const PointWithRange& 386 getPoint (int image_x, int image_y) const; 387 388 /** \brief Non-const-version of getPoint */ 389 inline PointWithRange& 390 getPoint (int image_x, int image_y); 391 392 /** Return the 3d point with range at the given image position */ 393 inline const PointWithRange& 394 getPoint (float image_x, float image_y) const; 395 396 /** Non-const-version of the above */ 397 inline PointWithRange& 398 getPoint (float image_x, float image_y); 399 400 /** \brief Return the 3D point with range at the given image position. This methd performs no error checking 401 * to make sure the specified image position is inside of the image! 402 * \param image_x the x coordinate 403 * \param image_y the y coordinate 404 * \return the point at the specified location (program may fail if the location is outside of the image bounds) 405 */ 406 inline const PointWithRange& 407 getPointNoCheck (int image_x, int image_y) const; 408 409 /** Non-const-version of getPointNoCheck */ 410 inline PointWithRange& 411 getPointNoCheck (int image_x, int image_y); 412 413 /** Same as above */ 414 inline void 415 getPoint (int image_x, int image_y, Eigen::Vector3f& point) const; 416 417 /** Same as above */ 418 inline void 419 getPoint (int index, Eigen::Vector3f& point) const; 420 421 /** Same as above */ 422 inline const Eigen::Map<const Eigen::Vector3f> 423 getEigenVector3f (int x, int y) const; 424 425 /** Same as above */ 426 inline const Eigen::Map<const Eigen::Vector3f> 427 getEigenVector3f (int index) const; 428 429 /** Return the 3d point with range at the given index (whereas index=y*width+x) */ 430 inline const PointWithRange& 431 getPoint (int index) const; 432 433 /** Calculate the 3D point according to the given image point and range */ 434 inline void 435 calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const; 436 /** Calculate the 3D point according to the given image point and the range value at the closest pixel */ 437 inline void 438 calculate3DPoint (float image_x, float image_y, PointWithRange& point) const; 439 440 /** Calculate the 3D point according to the given image point and range */ 441 virtual inline void 442 calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const; 443 /** Calculate the 3D point according to the given image point and the range value at the closest pixel */ 444 inline void 445 calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const; 446 447 /** Recalculate all 3D point positions according to their pixel position and range */ 448 PCL_EXPORTS void 449 recalculate3DPointPositions (); 450 451 /** Get imagePoint from 3D point in world coordinates */ 452 inline virtual void 453 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const; 454 455 /** Same as above */ 456 inline void 457 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const; 458 459 /** Same as above */ 460 inline void 461 getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const; 462 463 /** Same as above */ 464 inline void 465 getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const; 466 467 /** Same as above */ 468 inline void 469 getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const; 470 471 /** Same as above */ 472 inline void 473 getImagePoint (float x, float y, float z, float& image_x, float& image_y) const; 474 475 /** Same as above */ 476 inline void 477 getImagePoint (float x, float y, float z, int& image_x, int& image_y) const; 478 479 /** point_in_image will be the point in the image at the position the given point would be. Returns 480 * the range of the given point. */ 481 inline float 482 checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const; 483 484 /** Returns the difference in range between the given point and the range of the point in the image 485 * at the position the given point would be. 486 * (Return value is point_in_image.range-given_point.range) */ 487 inline float 488 getRangeDifference (const Eigen::Vector3f& point) const; 489 490 /** Get the image point corresponding to the given angles */ 491 inline void 492 getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const; 493 494 /** Get the angles corresponding to the given image point */ 495 inline void 496 getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const; 497 498 /** Transforms an image point in float values to an image point in int values */ 499 inline void 500 real2DToInt2D (float x, float y, int& xInt, int& yInt) const; 501 502 /** Check if a point is inside of the image */ 503 inline bool 504 isInImage (int x, int y) const; 505 506 /** Check if a point is inside of the image and has a finite range */ 507 inline bool 508 isValid (int x, int y) const; 509 510 /** Check if a point has a finite range */ 511 inline bool 512 isValid (int index) const; 513 514 /** Check if a point is inside of the image and has either a finite range or a max reading (range=INFINITY) */ 515 inline bool 516 isObserved (int x, int y) const; 517 518 /** Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! */ 519 inline bool 520 isMaxRange (int x, int y) const; 521 522 /** Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius. 523 * step_size determines how many pixels are used. 1 means all, 2 only every second, etc.. 524 * Returns false if it was unable to calculate a normal.*/ 525 inline bool 526 getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const; 527 528 /** Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered. */ 529 inline bool 530 getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point, 531 int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const; 532 533 /** Same as above */ 534 inline bool 535 getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, 536 int no_of_nearest_neighbors, Eigen::Vector3f& normal, 537 Eigen::Vector3f* point_on_plane=nullptr, int step_size=1) const; 538 539 /** Same as above, using default values */ 540 inline bool 541 getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const; 542 543 /** Same as above but extracts some more data and can also return the extracted 544 * information for all neighbors in radius if normal_all_neighbors is not NULL */ 545 inline bool 546 getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, 547 int no_of_closest_neighbors, int step_size, 548 float& max_closest_neighbor_distance_squared, 549 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values, 550 Eigen::Vector3f* normal_all_neighbors=nullptr, 551 Eigen::Vector3f* mean_all_neighbors=nullptr, 552 Eigen::Vector3f* eigen_values_all_neighbors=nullptr) const; 553 554 // Return the squared distance to the n-th neighbors of the point at x,y 555 inline float 556 getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const; 557 558 /** Calculate the impact angle based on the sensor position and the two given points - will return 559 * -INFINITY if one of the points is unobserved */ 560 inline float 561 getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const; 562 //! Same as above 563 inline float 564 getImpactAngle (int x1, int y1, int x2, int y2) const; 565 566 /** Extract a local normal (with a heuristic not to include background points) and calculate the impact 567 * angle based on this */ 568 inline float 569 getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const; 570 /** Uses the above function for every point in the image */ 571 PCL_EXPORTS float* 572 getImpactAngleImageBasedOnLocalNormals (int radius) const; 573 574 /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) 575 * This uses getImpactAngleBasedOnLocalNormal 576 * Will return -INFINITY if no normal could be calculated */ 577 inline float 578 getNormalBasedAcutenessValue (int x, int y, int radius) const; 579 580 /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) 581 * will return -INFINITY if one of the points is unobserved */ 582 inline float 583 getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const; 584 //! Same as above 585 inline float 586 getAcutenessValue (int x1, int y1, int x2, int y2) const; 587 588 /** Calculate getAcutenessValue for every point */ 589 PCL_EXPORTS void 590 getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x, 591 float*& acuteness_value_image_y) const; 592 593 /** Calculates, how much the surface changes at a point. Pi meaning a flat suface and 0.0f 594 * would be a needle point */ 595 //inline float 596 // getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, 597 // const PointWithRange& neighbor2) const; 598 599 /** Calculates, how much the surface changes at a point. 1 meaning a 90deg angle and 0 a flat suface */ 600 PCL_EXPORTS float 601 getSurfaceChange (int x, int y, int radius) const; 602 603 /** Uses the above function for every point in the image */ 604 PCL_EXPORTS float* 605 getSurfaceChangeImage (int radius) const; 606 607 /** Calculates, how much the surface changes at a point. Returns an angle [0.0f, PI] for x and y direction. 608 * A return value of -INFINITY means that a point was unobserved. */ 609 inline void 610 getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const; 611 612 /** Uses the above function for every point in the image */ 613 PCL_EXPORTS void 614 getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const; 615 616 /** Calculates the curvature in a point using pca */ 617 inline float 618 getCurvature (int x, int y, int radius, int step_size) const; 619 620 //! Get the sensor position 621 inline const Eigen::Vector3f 622 getSensorPos () const; 623 624 /** Sets all -INFINITY values to INFINITY */ 625 PCL_EXPORTS void 626 setUnseenToMaxRange (); 627 628 //! Getter for image_offset_x_ 629 inline int getImageOffsetX()630 getImageOffsetX () const { return image_offset_x_;} 631 //! Getter for image_offset_y_ 632 inline int getImageOffsetY()633 getImageOffsetY () const { return image_offset_y_;} 634 635 //! Setter for image offsets 636 inline void setImageOffsets(int offset_x,int offset_y)637 setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;} 638 639 640 641 /** Get a sub part of the complete image as a new range image. 642 * \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image. 643 * This is always according to absolute 0,0 meaning -180°,-90° 644 * and it is already in the system of the new image, so the 645 * actual pixel used in the original image is 646 * combine_pixels* (image_offset_x-image_offset_x_) 647 * \param sub_image_image_offset_y - Same as image_offset_x for the y coordinate 648 * \param sub_image_width - width of the new image 649 * \param sub_image_height - height of the new image 650 * \param combine_pixels - shrinking factor, meaning the new angular resolution 651 * is combine_pixels times the old one 652 * \param sub_image - the output image 653 */ 654 virtual void 655 getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width, 656 int sub_image_height, int combine_pixels, RangeImage& sub_image) const; 657 658 //! Get a range image with half the resolution 659 virtual void 660 getHalfImage (RangeImage& half_image) const; 661 662 //! Find the minimum and maximum range in the image 663 PCL_EXPORTS void 664 getMinMaxRanges (float& min_range, float& max_range) const; 665 666 //! This function sets the sensor pose to 0 and transforms all point positions to this local coordinate frame 667 PCL_EXPORTS void 668 change3dPointsToLocalCoordinateFrame (); 669 670 /** Calculate a range patch as the z values of the coordinate frame given by pose. 671 * The patch will have size pixel_size x pixel_size and each pixel 672 * covers world_size/pixel_size meters in the world 673 * You are responsible for deleting the structure afterwards! */ 674 PCL_EXPORTS float* 675 getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const; 676 677 //! Same as above, but using the local coordinate frame defined by point and the viewing direction 678 PCL_EXPORTS float* 679 getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const; 680 681 //! Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction 682 inline Eigen::Affine3f 683 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const; 684 //! Same as above, using a reference for the retrurn value 685 inline void 686 getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, 687 Eigen::Affine3f& transformation) const; 688 //! Same as above, but only returning the rotation 689 inline void 690 getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const; 691 692 /** Get a local coordinate frame at the given point based on the normal. */ 693 PCL_EXPORTS bool 694 getNormalBasedUprightTransformation (const Eigen::Vector3f& point, 695 float max_dist, Eigen::Affine3f& transformation) const; 696 697 /** Get the integral image of the range values (used for fast blur operations). 698 * You are responsible for deleting it after usage! */ 699 PCL_EXPORTS void 700 getIntegralImage (float*& integral_image, int*& valid_points_num_image) const; 701 702 /** Get a blurred version of the range image using box filters on the provided integral image*/ 703 PCL_EXPORTS void // Template necessary so that this function also works in derived classes 704 getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image, 705 RangeImage& range_image) const; 706 707 /** Get a blurred version of the range image using box filters */ 708 PCL_EXPORTS virtual void // Template necessary so that this function also works in derived classes 709 getBlurredImage (int blur_radius, RangeImage& range_image) const; 710 711 /** Get the squared euclidean distance between the two image points. 712 * Returns -INFINITY if one of the points was not observed */ 713 inline float 714 getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const; 715 //! Doing the above for some steps in the given direction and averaging 716 inline float 717 getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const; 718 719 //! Project all points on the local plane approximation, thereby smoothing the surface of the scan 720 PCL_EXPORTS void 721 getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const; 722 //void getLocalNormals (int radius) const; 723 724 /** Calculates the average 3D position of the no_of_points points described by the start 725 * point x,y in the direction delta. 726 * Returns a max range point (range=INFINITY) if the first point is max range and an 727 * unobserved point (range=-INFINITY) if non of the points is observed. */ 728 inline void 729 get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, 730 PointWithRange& average_point) const; 731 732 /** Calculates the overlap of two range images given the relative transformation 733 * (from the given image to *this) */ 734 PCL_EXPORTS float 735 getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation, 736 int search_radius, float max_distance, int pixel_step=1) const; 737 738 /** Get the viewing direction for the given point */ 739 inline bool 740 getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const; 741 742 /** Get the viewing direction for the given point */ 743 inline void 744 getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const; 745 746 /** Return a newly created Range image. 747 * Can be reimplemented in derived classes like RangeImagePlanar to return an image of the same type. */ 748 PCL_EXPORTS virtual RangeImage* getNew()749 getNew () const { return new RangeImage; } 750 751 /** Copy other to *this. Necessary for use in virtual functions that need to copy derived RangeImage classes (like RangeImagePlanar) */ 752 PCL_EXPORTS virtual void 753 copyTo (RangeImage& other) const; 754 755 756 // =====MEMBER VARIABLES===== 757 // BaseClass: 758 // roslib::Header header; 759 // std::vector<PointT> points; 760 // std::uint32_t width; 761 // std::uint32_t height; 762 // bool is_dense; 763 764 static bool debug; /**< Just for... well... debugging purposes. :-) */ 765 766 protected: 767 // =====PROTECTED MEMBER VARIABLES===== 768 Eigen::Affine3f to_range_image_system_; /**< Inverse of to_world_system_ */ 769 Eigen::Affine3f to_world_system_; /**< Inverse of to_range_image_system_ */ 770 float angular_resolution_x_; /**< Angular resolution of the range image in x direction in radians per pixel */ 771 float angular_resolution_y_; /**< Angular resolution of the range image in y direction in radians per pixel */ 772 float angular_resolution_x_reciprocal_; /**< 1.0/angular_resolution_x_ - provided for better performance of 773 * multiplication compared to division */ 774 float angular_resolution_y_reciprocal_; /**< 1.0/angular_resolution_y_ - provided for better performance of 775 * multiplication compared to division */ 776 int image_offset_x_, image_offset_y_; /**< Position of the top left corner of the range image compared to 777 * an image of full size (360x180 degrees) */ 778 PointWithRange unobserved_point; /**< This point is used to be able to return 779 * a reference to a non-existing point */ 780 781 // =====PROTECTED METHODS===== 782 783 784 // =====STATIC PROTECTED===== 785 static const int lookup_table_size; 786 static std::vector<float> asin_lookup_table; 787 static std::vector<float> atan_lookup_table; 788 static std::vector<float> cos_lookup_table; 789 /** Create lookup tables for trigonometric functions */ 790 static void 791 createLookupTables (); 792 793 /** Query the asin lookup table */ 794 static inline float 795 asinLookUp (float value); 796 797 /** Query the std::atan2 lookup table */ 798 static inline float 799 atan2LookUp (float y, float x); 800 801 /** Query the cos lookup table */ 802 static inline float 803 cosLookUp (float value); 804 805 806 public: 807 PCL_MAKE_ALIGNED_OPERATOR_NEW 808 }; 809 810 /** 811 * /ingroup range_image 812 */ 813 inline std::ostream& 814 operator<< (std::ostream& os, const RangeImage& r) 815 { 816 os << "header: " << std::endl; 817 os << r.header; 818 os << "points[]: " << r.size () << std::endl; 819 os << "width: " << r.width << std::endl; 820 os << "height: " << r.height << std::endl; 821 os << "sensor_origin_: " 822 << r.sensor_origin_[0] << ' ' 823 << r.sensor_origin_[1] << ' ' 824 << r.sensor_origin_[2] << std::endl; 825 os << "sensor_orientation_: " 826 << r.sensor_orientation_.x () << ' ' 827 << r.sensor_orientation_.y () << ' ' 828 << r.sensor_orientation_.z () << ' ' 829 << r.sensor_orientation_.w () << std::endl; 830 os << "is_dense: " << r.is_dense << std::endl; 831 os << "angular resolution: " 832 << RAD2DEG (r.getAngularResolutionX ()) << "deg/pixel in x and " 833 << RAD2DEG (r.getAngularResolutionY ()) << "deg/pixel in y.\n" << std::endl; 834 return (os); 835 } 836 837 } // namespace end 838 839 840 #include <pcl/range_image/impl/range_image.hpp> // Definitions of templated and inline functions 841