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);
217 template <
typename Po
intCloudType>
void
219 float angular_resolution_x,
float angular_resolution_y,
220 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
221 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
223 float noise_level=0.0f,
float min_range=0.0f,
int border_size=0);
240 template <
typename Po
intCloudTypeWithViewpo
ints>
void
242 float max_angle_width,
float max_angle_height,
244 float min_range=0.0f,
int border_size=0);
264 template <
typename Po
intCloudTypeWithViewpo
ints>
void
266 float angular_resolution_x,
float angular_resolution_y,
267 float max_angle_width,
float max_angle_height,
269 float min_range=0.0f,
int border_size=0);
279 createEmpty (
float angular_resolution,
const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
294 createEmpty (
float angular_resolution_x,
float angular_resolution_y,
295 const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
311 template <
typename Po
intCloudType>
void
312 doZBuffer (
const PointCloudType& point_cloud,
float noise_level,
313 float min_range,
int& top,
int& right,
int& bottom,
int& left);
316 template <
typename Po
intCloudType>
void
327 cropImage (
int border_size=0,
int top=-1,
int right=-1,
int bottom=-1,
int left=-1);
338 inline const Eigen::Affine3f&
348 inline const Eigen::Affine3f&
388 getPoint (
int image_x,
int image_y)
const;
392 getPoint (
int image_x,
int image_y);
396 getPoint (
float image_x,
float image_y)
const;
400 getPoint (
float image_x,
float image_y);
417 getPoint (
int image_x,
int image_y, Eigen::Vector3f& point)
const;
421 getPoint (
int index, Eigen::Vector3f& point)
const;
424 inline const Eigen::Map<const Eigen::Vector3f>
428 inline const Eigen::Map<const Eigen::Vector3f>
444 calculate3DPoint (
float image_x,
float image_y,
float range, Eigen::Vector3f& point)
const;
447 calculate3DPoint (
float image_x,
float image_y, Eigen::Vector3f& point)
const;
455 getImagePoint (
const Eigen::Vector3f& point,
float& image_x,
float& image_y,
float& range)
const;
459 getImagePoint (
const Eigen::Vector3f& point,
int& image_x,
int& image_y,
float& range)
const;
463 getImagePoint (
const Eigen::Vector3f& point,
float& image_x,
float& image_y)
const;
467 getImagePoint (
const Eigen::Vector3f& point,
int& image_x,
int& image_y)
const;
471 getImagePoint (
float x,
float y,
float z,
float& image_x,
float& image_y,
float& range)
const;
475 getImagePoint (
float x,
float y,
float z,
float& image_x,
float& image_y)
const;
479 getImagePoint (
float x,
float y,
float z,
int& image_x,
int& image_y)
const;
502 real2DToInt2D (
float x,
float y,
int& xInt,
int& yInt)
const;
528 getNormal (
int x,
int y,
int radius, Eigen::Vector3f& normal,
int step_size=1)
const;
533 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size=1)
const;
538 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
539 Eigen::Vector3f* point_on_plane=NULL,
int step_size=1)
const;
549 int no_of_closest_neighbors,
int step_size,
550 float& max_closest_neighbor_distance_squared,
551 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
552 Eigen::Vector3f* normal_all_neighbors=NULL,
553 Eigen::Vector3f* mean_all_neighbors=NULL,
554 Eigen::Vector3f* eigen_values_all_neighbors=NULL)
const;
593 float*& acuteness_value_image_y)
const;
612 getSurfaceAngleChange (
int x,
int y,
int radius,
float& angle_change_x,
float& angle_change_y)
const;
620 getCurvature (
int x,
int y,
int radius,
int step_size)
const;
623 inline const Eigen::Vector3f
657 getSubImage (
int sub_image_image_offset_x,
int sub_image_image_offset_y,
int sub_image_width,
658 int sub_image_height,
int combine_pixels,
RangeImage& sub_image)
const;
684 inline Eigen::Affine3f
689 Eigen::Affine3f& transformation)
const;
697 float max_dist, Eigen::Affine3f& transformation)
const;
702 getIntegralImage (
float*& integral_image,
int*& valid_points_num_image)
const;
710 PCL_EXPORTS
virtual void
737 getOverlap (
const RangeImage& other_range_image,
const Eigen::Affine3f& relative_transformation,
738 int search_radius,
float max_distance,
int pixel_step=1)
const;
746 getViewingDirection (
const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction)
const;
754 PCL_EXPORTS
virtual void
809 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
818 os <<
"header: " << std::endl;
820 os <<
"points[]: " << r.
points.size () << std::endl;
821 os <<
"width: " << r.
width << std::endl;
822 os <<
"height: " << r.
height << std::endl;
823 os <<
"sensor_origin_: "
827 os <<
"sensor_orientation_: "
832 os <<
"is_dense: " << r.
is_dense << std::endl;
833 os <<
"angular resolution: "
842 #include <pcl/range_image/impl/range_image.hpp>
844 #endif //#ifndef PCL_RANGE_IMAGE_H_
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...
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
Ptr makeShared()
Get a boost shared pointer of a copy of this.
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...
virtual void getHalfImage(RangeImage &half_image) const
Get a range image with half the resolution.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
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.
A point structure representing Euclidean xyz coordinates, padded with an extra range float...
virtual PCL_EXPORTS RangeImage * getNew() const
Return a newly created Range image.
virtual PCL_EXPORTS void getBlurredImage(int blur_radius, RangeImage &range_image) const
Get a blurred version of the range image using box filters.
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).
float getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
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.
void setImageOffsets(int offset_x, int offset_y)
Setter for image offsets.
RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where...
PCL_EXPORTS float * getImpactAngleImageBasedOnLocalNormals(int radius) const
Uses the above function for every point in the image.
pcl::PointCloud< PointWithRange > BaseClass
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.
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
int image_offset_y_
Position of the top left corner of the range image compared to an image of full size (360x180 degrees...
bool getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
const Eigen::Affine3f & getTransformationToRangeImageSystem() const
Getter for the transformation from the world system into the range image system (the sensor coordinat...
boost::shared_ptr< const RangeImage > ConstPtr
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...
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...
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
uint32_t width
The point cloud width (if organized as an image-structure).
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
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...
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...
static PCL_EXPORTS void getCoordinateFrameTransformation(RangeImage::CoordinateFrame coordinate_frame, Eigen::Affine3f &transformation)
Get the transformation that transforms the given coordinate frame into CAMERA_FRAME.
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.
static void createLookupTables()
Create lookup tables for trigonometric functions.
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.
static const int lookup_table_size
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
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.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
pcl::PCLHeader header
The point cloud header.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
void getAnglesFromImagePoint(float image_x, float image_y, float &angle_x, float &angle_y) const
Get the angles corresponding to the given image point.
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.
PCL_EXPORTS void reset()
Reset all values to an empty range image.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
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...
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.
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...
bool isInImage(int x, int y) const
Check if a point is inside of the image.
PCL_EXPORTS float * getSurfaceChangeImage(int radius) const
Uses the above function for every point in the image.
int getImageOffsetY() const
Getter for image_offset_y_.
int getImageOffsetX() const
Getter for image_offset_x_.
PCL_EXPORTS void setUnseenToMaxRange()
Sets all -INFINITY values to INFINITY.
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
static std::vector< float > cos_lookup_table
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)
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...
virtual PCL_EXPORTS void copyTo(RangeImage &other) const
Copy other to *this.
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...
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 getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
static float atan2LookUp(float y, float x)
Query the atan2 lookup table.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > VectorOfEigenVector3f
PCL_EXPORTS float getSurfaceChange(int x, int y, int radius) const
Calculates, how much the surface changes at a point.
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.
static std::vector< float > asin_lookup_table
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performace of multiplication compared to division ...
void getImagePointFromAngles(float angle_x, float angle_y, float &image_x, float &image_y) const
Get the image point corresponding to the given angles.
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.
float getAngularResolutionX() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performace of multiplication compared to division ...
const Eigen::Affine3f & getTransformationToWorldSystem() const
Getter for the transformation from the range image system (the sensor coordinate frame) into the worl...
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...
float getAngularResolutionY() const
Getter for the angular resolution of the range image in y direction in radians per pixel...
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
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...
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
static int max_no_of_threads
The maximum number of openmp threads that can be used in this class.
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...
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
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_EXPORTS RangeImage()
Constructor.
PCL_EXPORTS void change3dPointsToLocalCoordinateFrame()
This function sets the sensor pose to 0 and transforms all point positions to this local coordinate f...
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.
static float cosLookUp(float value)
Query the cos lookup table.
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 void getMinMaxRanges(float &min_range, float &max_range) const
Find the minimum and maximum range in the image.
static float asinLookUp(float value)
Query the asin lookup table.
PCL_EXPORTS float * getRangesArray() const
Get all the range values in one float array of size width*height.
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
boost::shared_ptr< RangeImage > Ptr
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
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.
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
static bool debug
Just for...
static std::vector< float > atan_lookup_table
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
PCL_EXPORTS void getAcutenessValueImages(int pixel_distance, float *´ness_value_image_x, float *´ness_value_image_y) const
Calculate getAcutenessValue for every point.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
virtual PCL_EXPORTS ~RangeImage()
Destructor.
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! ...
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...
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...
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...
void integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.
uint32_t height
The point cloud height (if organized as an image-structure).