38 #ifndef PCL_RANGE_IMAGE_H_
39 #define PCL_RANGE_IMAGE_H_
41 #include <pcl/point_cloud.h>
42 #include <pcl/pcl_macros.h>
43 #include <pcl/point_types.h>
44 #include <pcl/common/common_headers.h>
45 #include <pcl/common/vector_average.h>
61 typedef boost::shared_ptr<RangeImage>
Ptr;
62 typedef boost::shared_ptr<const RangeImage>
ConstPtr;
89 getMaxAngleSize (
const Eigen::Affine3f& viewer_pose,
const Eigen::Vector3f& center,
96 static inline Eigen::Vector3f
103 PCL_EXPORTS
static void
105 Eigen::Affine3f& transformation);
112 template <
typename Po
intCloudTypeWithViewpo
ints>
static Eigen::Vector3f
119 PCL_EXPORTS
static void
145 template <
typename Po
intCloudType>
void
148 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
150 float min_range=0.0f,
int border_size=0);
169 template <
typename Po
intCloudType>
void
173 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
175 float noise_level=0.0f,
float min_range=0.0f,
int border_size=0);
192 template <
typename Po
intCloudType>
void
194 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
195 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
197 float noise_level=0.0f,
float min_range=0.0f,
int border_size=0);
218 template <
typename Po
intCloudType>
void
220 float angular_resolution_x,
float angular_resolution_y,
221 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
222 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
224 float noise_level=0.0f,
float min_range=0.0f,
int border_size=0);
241 template <
typename Po
intCloudTypeWithViewpo
ints>
void
243 float max_angle_width,
float max_angle_height,
245 float min_range=0.0f,
int border_size=0);
265 template <
typename Po
intCloudTypeWithViewpo
ints>
void
267 float angular_resolution_x,
float angular_resolution_y,
268 float max_angle_width,
float max_angle_height,
270 float min_range=0.0f,
int border_size=0);
280 createEmpty (
float angular_resolution,
const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
295 createEmpty (
float angular_resolution_x,
float angular_resolution_y,
296 const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
312 template <
typename Po
intCloudType>
void
313 doZBuffer (
const PointCloudType& point_cloud,
float noise_level,
314 float min_range,
int& top,
int& right,
int& bottom,
int& left);
317 template <
typename Po
intCloudType>
void
328 cropImage (
int border_size=0,
int top=-1,
int right=-1,
int bottom=-1,
int left=-1);
339 inline const Eigen::Affine3f&
349 inline const Eigen::Affine3f&
389 getPoint (
int image_x,
int image_y)
const;
393 getPoint (
int image_x,
int image_y);
397 getPoint (
float image_x,
float image_y)
const;
401 getPoint (
float image_x,
float image_y);
418 getPoint (
int image_x,
int image_y, Eigen::Vector3f& point)
const;
422 getPoint (
int index, Eigen::Vector3f& point)
const;
425 inline const Eigen::Map<const Eigen::Vector3f>
429 inline const Eigen::Map<const Eigen::Vector3f>
445 calculate3DPoint (
float image_x,
float image_y,
float range, Eigen::Vector3f& point)
const;
448 calculate3DPoint (
float image_x,
float image_y, Eigen::Vector3f& point)
const;
456 getImagePoint (
const Eigen::Vector3f& point,
float& image_x,
float& image_y,
float& range)
const;
460 getImagePoint (
const Eigen::Vector3f& point,
int& image_x,
int& image_y,
float& range)
const;
464 getImagePoint (
const Eigen::Vector3f& point,
float& image_x,
float& image_y)
const;
468 getImagePoint (
const Eigen::Vector3f& point,
int& image_x,
int& image_y)
const;
472 getImagePoint (
float x,
float y,
float z,
float& image_x,
float& image_y,
float& range)
const;
476 getImagePoint (
float x,
float y,
float z,
float& image_x,
float& image_y)
const;
480 getImagePoint (
float x,
float y,
float z,
int& image_x,
int& image_y)
const;
503 real2DToInt2D (
float x,
float y,
int& xInt,
int& yInt)
const;
529 getNormal (
int x,
int y,
int radius, Eigen::Vector3f& normal,
int step_size=1)
const;
534 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size=1)
const;
539 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
540 Eigen::Vector3f* point_on_plane=NULL,
int step_size=1)
const;
550 int no_of_closest_neighbors,
int step_size,
551 float& max_closest_neighbor_distance_squared,
552 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
553 Eigen::Vector3f* normal_all_neighbors=NULL,
554 Eigen::Vector3f* mean_all_neighbors=NULL,
555 Eigen::Vector3f* eigen_values_all_neighbors=NULL)
const;
594 float*& acuteness_value_image_y)
const;
613 getSurfaceAngleChange (
int x,
int y,
int radius,
float& angle_change_x,
float& angle_change_y)
const;
621 getCurvature (
int x,
int y,
int radius,
int step_size)
const;
624 inline const Eigen::Vector3f
658 getSubImage (
int sub_image_image_offset_x,
int sub_image_image_offset_y,
int sub_image_width,
659 int sub_image_height,
int combine_pixels,
RangeImage& sub_image)
const;
685 inline Eigen::Affine3f
690 Eigen::Affine3f& transformation)
const;
698 float max_dist, Eigen::Affine3f& transformation)
const;
703 getIntegralImage (
float*& integral_image,
int*& valid_points_num_image)
const;
711 PCL_EXPORTS
virtual void
738 getOverlap (
const RangeImage& other_range_image,
const Eigen::Affine3f& relative_transformation,
739 int search_radius,
float max_distance,
int pixel_step=1)
const;
747 getViewingDirection (
const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction)
const;
755 PCL_EXPORTS
virtual void
810 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
819 os <<
"header: " << std::endl;
821 os <<
"points[]: " << r.
points.size () << std::endl;
822 os <<
"width: " << r.
width << std::endl;
823 os <<
"height: " << r.
height << std::endl;
824 os <<
"sensor_origin_: "
828 os <<
"sensor_orientation_: "
833 os <<
"is_dense: " << r.
is_dense << std::endl;
834 os <<
"angular resolution: "
843 #include <pcl/range_image/impl/range_image.hpp>
845 #endif //#ifndef PCL_RANGE_IMAGE_H_
PCL_EXPORTS float * getInterpolatedSurfaceProjection(const Eigen::Affine3f &pose, int pixel_size, float world_size) const
Calculate a range patch as the z values of the coordinate frame given by pose.
PCL_EXPORTS void getAcutenessValueImages(int pixel_distance, float *´ness_value_image_x, float *´ness_value_image_y) const
Calculate getAcutenessValue for every point.
A point structure representing Euclidean xyz coordinates, padded with an extra range float...
void createEmpty(float angular_resolution, const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad(360.0f), float angle_height=pcl::deg2rad(180.0f))
Create an empty depth image (filled with unobserved points)
float getAngularResolutionX() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
bool isObserved(int x, int y) const
Check if a point is inside of the image and has either a finite range or a max reading (range=INFINIT...
pcl::PointCloud< PointWithRange > BaseClass
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > VectorOfEigenVector3f
PCL_EXPORTS RangeImage()
Constructor.
void real2DToInt2D(float x, float y, int &xInt, int &yInt) const
Transforms an image point in float values to an image point in int values.
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
void calculate3DPoint(float image_x, float image_y, float range, PointWithRange &point) const
Calculate the 3D point according to the given image point and range.
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
PCL_EXPORTS float * getSurfaceChangeImage(int radius) const
Uses the above function for every point in the image.
virtual void getSubImage(int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width, int sub_image_height, int combine_pixels, RangeImage &sub_image) const
Get a sub part of the complete image as a new range image.
pcl::PCLHeader header
The point cloud header.
boost::shared_ptr< RangeImage > Ptr
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
const Eigen::Affine3f & getTransformationToWorldSystem() const
Getter for the transformation from the range image system (the sensor coordinate frame) into the worl...
void createFromPointCloud(const PointCloudType &point_cloud, float angular_resolution=pcl::deg2rad(0.5f), float max_angle_width=pcl::deg2rad(360.0f), float max_angle_height=pcl::deg2rad(180.0f), const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud.
void getSurfaceAngleChange(int x, int y, int radius, float &angle_change_x, float &angle_change_y) const
Calculates, how much the surface changes at a point.
static std::vector< float > atan_lookup_table
PCL_EXPORTS float * getImpactAngleImageBasedOnLocalNormals(int radius) const
Uses the above function for every point in the image.
void doZBuffer(const PointCloudType &point_cloud, float noise_level, float min_range, int &top, int &right, int &bottom, int &left)
Integrate the given point cloud into the current range image using a z-buffer.
void setTransformationToRangeImageSystem(const Eigen::Affine3f &to_range_image_system)
Setter for the transformation from the range image system (the sensor coordinate frame) into the worl...
uint32_t height
The point cloud height (if organized as an image-structure).
static const int lookup_table_size
const Eigen::Affine3f & getTransformationToRangeImageSystem() const
Getter for the transformation from the world system into the range image system (the sensor coordinat...
PCL_EXPORTS void getMinMaxRanges(float &min_range, float &max_range) const
Find the minimum and maximum range in the image.
virtual PCL_EXPORTS ~RangeImage()
Destructor.
int getImageOffsetY() const
Getter for image_offset_y_.
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
float getImpactAngle(const PointWithRange &point1, const PointWithRange &point2) const
Calculate the impact angle based on the sensor position and the two given points - will return -INFIN...
PCL_EXPORTS void getSurfaceAngleChangeImages(int radius, float *&angle_change_image_x, float *&angle_change_image_y) const
Uses the above function for every point in the image.
RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where...
void getAnglesFromImagePoint(float image_x, float image_y, float &angle_x, float &angle_y) const
Get the angles corresponding to the given image point.
PCL_EXPORTS float getOverlap(const RangeImage &other_range_image, const Eigen::Affine3f &relative_transformation, int search_radius, float max_distance, int pixel_step=1) const
Calculates the overlap of two range images given the relative transformation (from the given image to...
PCL_EXPORTS void getIntegralImage(float *&integral_image, int *&valid_points_num_image) const
Get the integral image of the range values (used for fast blur operations).
static PCL_EXPORTS void extractFarRanges(const pcl::PCLPointCloud2 &point_cloud_data, PointCloud< PointWithViewpoint > &far_ranges)
Check if the provided data includes far ranges and add them to far_ranges.
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
static std::vector< float > asin_lookup_table
virtual PCL_EXPORTS void getBlurredImage(int blur_radius, RangeImage &range_image) const
Get a blurred version of the range image using box filters.
float getImpactAngleBasedOnLocalNormal(int x, int y, int radius) const
Extract a local normal (with a heuristic not to include background points) and calculate the impact a...
float checkPoint(const Eigen::Vector3f &point, PointWithRange &point_in_image) const
point_in_image will be the point in the image at the position the given point would be...
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void createFromPointCloudWithViewpoints(const PointCloudTypeWithViewpoints &point_cloud, float angular_resolution, float max_angle_width, float max_angle_height, CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud, using the average viewpoint of the points (vp_x...
static float atan2LookUp(float y, float x)
Query the atan2 lookup table.
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
PCL_EXPORTS void cropImage(int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1)
Cut the range image to the minimal size so that it still contains all actual range readings...
int image_offset_y_
Position of the top left corner of the range image compared to an image of full size (360x180 degrees...
void getImagePointFromAngles(float angle_x, float angle_y, float &image_x, float &image_y) const
Get the image point corresponding to the given angles.
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
PCL_EXPORTS void change3dPointsToLocalCoordinateFrame()
This function sets the sensor pose to 0 and transforms all point positions to this local coordinate f...
static float cosLookUp(float value)
Query the cos lookup table.
float getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
float getRangeDifference(const Eigen::Vector3f &point) const
Returns the difference in range between the given point and the range of the point in the image at th...
PCL_EXPORTS float * getRangesArray() const
Get all the range values in one float array of size width*height.
Ptr makeShared()
Get a boost shared pointer of a copy of this.
static PCL_EXPORTS void getCoordinateFrameTransformation(RangeImage::CoordinateFrame coordinate_frame, Eigen::Affine3f &transformation)
Get the transformation that transforms the given coordinate frame into CAMERA_FRAME.
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performace of multiplication compared to division ...
bool getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
void setImageOffsets(int offset_x, int offset_y)
Setter for image offsets.
float getAngularResolutionY() const
Getter for the angular resolution of the range image in y direction in radians per pixel...
PCL_EXPORTS void getRangeImageWithSmoothedSurface(int radius, RangeImage &smoothed_range_image) const
Project all points on the local plane approximation, thereby smoothing the surface of the scan...
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
static float asinLookUp(float value)
Query the asin lookup table.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
static void createLookupTables()
Create lookup tables for trigonometric functions.
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
virtual PCL_EXPORTS RangeImage * getNew() const
Return a newly created Range image.
void integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.
Eigen::Affine3f getTransformationToViewerCoordinateFrame(const Eigen::Vector3f &point) const
Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performace of multiplication compared to division ...
static float getMaxAngleSize(const Eigen::Affine3f &viewer_pose, const Eigen::Vector3f ¢er, float radius)
Get the size of a certain area when seen from the given pose.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
void createFromPointCloudWithKnownSize(const PointCloudType &point_cloud, float angular_resolution, const Eigen::Vector3f &point_cloud_center, float point_cloud_radius, const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud, getting a hint about the size of the scene for faster calc...
int getImageOffsetX() const
Getter for image_offset_x_.
PCL_EXPORTS void getBlurredImageUsingIntegralImage(int blur_radius, float *integral_image, int *valid_points_num_image, RangeImage &range_image) const
Get a blurred version of the range image using box filters on the provided integral image...
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
uint32_t width
The point cloud width (if organized as an image-structure).
PCL_EXPORTS void setUnseenToMaxRange()
Sets all -INFINITY values to INFINITY.
PCL_EXPORTS bool getNormalBasedUprightTransformation(const Eigen::Vector3f &point, float max_dist, Eigen::Affine3f &transformation) const
Get a local coordinate frame at the given point based on the normal.
PCL_EXPORTS void reset()
Reset all values to an empty range image.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void get1dPointAverage(int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange &average_point) const
Calculates the average 3D position of the no_of_points points described by the start point x...
float getAcutenessValue(const PointWithRange &point1, const PointWithRange &point2) const
Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) will r...
bool getNormal(int x, int y, int radius, Eigen::Vector3f &normal, int step_size=1) const
Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius...
static int max_no_of_threads
The maximum number of openmp threads that can be used in this class.
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
static bool debug
Just for...
bool getSurfaceInformation(int x, int y, int radius, const Eigen::Vector3f &point, int no_of_closest_neighbors, int step_size, float &max_closest_neighbor_distance_squared, Eigen::Vector3f &normal, Eigen::Vector3f &mean, Eigen::Vector3f &eigen_values, Eigen::Vector3f *normal_all_neighbors=NULL, Eigen::Vector3f *mean_all_neighbors=NULL, Eigen::Vector3f *eigen_values_all_neighbors=NULL) const
Same as above but extracts some more data and can also return the extracted information for all neigh...
boost::shared_ptr< const RangeImage > ConstPtr
static std::vector< float > cos_lookup_table
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool isInImage(int x, int y) const
Check if a point is inside of the image.
float getNormalBasedAcutenessValue(int x, int y, int radius) const
Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) This u...
float getAverageEuclideanDistance(int x, int y, int offset_x, int offset_y, int max_steps) const
Doing the above for some steps in the given direction and averaging.
static Eigen::Vector3f getAverageViewPoint(const PointCloudTypeWithViewpoints &point_cloud)
Get the average viewpoint of a point cloud where each point carries viewpoint information as vp_x...
PCL_EXPORTS float getSurfaceChange(int x, int y, int radius) const
Calculates, how much the surface changes at a point.
virtual PCL_EXPORTS void copyTo(RangeImage &other) const
Copy other to *this.
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! ...
bool getNormalForClosestNeighbors(int x, int y, int radius, const PointWithRange &point, int no_of_nearest_neighbors, Eigen::Vector3f &normal, int step_size=1) const
Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered...
virtual void getHalfImage(RangeImage &half_image) const
Get a range image with half the resolution.