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_
static int max_no_of_threads
The maximum number of openmp threads that can be used in this class.
boost::shared_ptr< const RangeImage > ConstPtr
static std::vector< float > atan_lookup_table
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 const int lookup_table_size
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
PCL_EXPORTS float * getSurfaceChangeImage(int radius) const
Uses the above function for every point in the image.
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 getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
PCL_EXPORTS void getMinMaxRanges(float &min_range, float &max_range) const
Find the minimum and maximum range in the image.
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
float getAngularResolutionY() const
Getter for the angular resolution of the range image in y direction in radians per pixel...
Ptr makeShared()
Get a boost shared pointer of a copy of this.
float getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
A point structure representing Euclidean xyz coordinates, padded with an extra range float...
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.
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...
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)
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
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 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...
static float cosLookUp(float value)
Query the cos lookup table.
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
virtual PCL_EXPORTS ~RangeImage()
Destructor.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
pcl::PCLHeader header
The point cloud header.
const Eigen::Affine3f & getTransformationToWorldSystem() const
Getter for the transformation from the range image system (the sensor coordinate frame) into the worl...
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
void getImagePointFromAngles(float angle_x, float angle_y, float &image_x, float &image_y) const
Get the image point corresponding to the given angles.
bool getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
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...
pcl::PointCloud< PointWithRange > BaseClass
static float atan2LookUp(float y, float x)
Query the atan2 lookup table.
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.
uint32_t width
The point cloud width (if organized as an image-structure).
PCL_EXPORTS float * getImpactAngleImageBasedOnLocalNormals(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_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).
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...
virtual PCL_EXPORTS void getBlurredImage(int blur_radius, RangeImage &range_image) const
Get a blurred version of the range image using box filters.
static float asinLookUp(float value)
Query the asin lookup table.
static void createLookupTables()
Create lookup tables for trigonometric functions.
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
void setImageOffsets(int offset_x, int offset_y)
Setter for image offsets.
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
static PCL_EXPORTS void getCoordinateFrameTransformation(RangeImage::CoordinateFrame coordinate_frame, Eigen::Affine3f &transformation)
Get the transformation that transforms the given coordinate frame into CAMERA_FRAME.
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
boost::shared_ptr< RangeImage > Ptr
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...
static std::vector< float > cos_lookup_table
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! ...
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > VectorOfEigenVector3f
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...
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...
bool isInImage(int x, int y) const
Check if a point is inside of the image.
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...
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
uint32_t height
The point cloud height (if organized as an image-structure).
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.
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performace of multiplication compared to division ...
virtual PCL_EXPORTS RangeImage * getNew() const
Return a newly created Range image.
PCL_EXPORTS void getAcutenessValueImages(int pixel_distance, float *´ness_value_image_x, float *´ness_value_image_y) const
Calculate getAcutenessValue for every point.
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...
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where...
int getImageOffsetY() const
Getter for image_offset_y_.
virtual void getHalfImage(RangeImage &half_image) const
Get a range image with half the resolution.
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
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.
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...
int image_offset_y_
Position of the top left corner of the range image compared to an image of full size (360x180 degrees...
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...
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...
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.
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 integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.
static bool debug
Just for...
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 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...
int getImageOffsetX() const
Getter for image_offset_x_.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performace of multiplication compared to division ...
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.
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.
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.
virtual PCL_EXPORTS void copyTo(RangeImage &other) const
Copy other to *this.
PCL_EXPORTS void setUnseenToMaxRange()
Sets all -INFINITY values to INFINITY.
static std::vector< float > asin_lookup_table
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.
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 getSurfaceChange(int x, int y, int radius) const
Calculates, how much the surface changes at a point.
float getAngularResolutionX() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
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 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...
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
const Eigen::Affine3f & getTransformationToRangeImageSystem() const
Getter for the transformation from the world system into the range image system (the sensor coordinat...
PCL_EXPORTS void reset()
Reset all values to an empty range image.