41 #include <pcl/point_cloud.h>
44 #include <pcl/PCLPointCloud2.h>
46 #include <pcl/common/vector_average.h>
62 using Ptr = shared_ptr<RangeImage>;
90 getMaxAngleSize (
const Eigen::Affine3f& viewer_pose,
const Eigen::Vector3f& center,
97 static inline Eigen::Vector3f
106 Eigen::Affine3f& transformation);
113 template <
typename Po
intCloudTypeWithViewpo
ints>
static Eigen::Vector3f
146 template <
typename Po
intCloudType>
void
149 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
151 float min_range=0.0f,
int border_size=0);
170 template <
typename Po
intCloudType>
void
174 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
176 float noise_level=0.0f,
float min_range=0.0f,
int border_size=0);
193 template <
typename Po
intCloudType>
void
195 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
196 const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
198 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 (),
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=
nullptr,
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=
nullptr,
553 Eigen::Vector3f* mean_all_neighbors=
nullptr,
554 Eigen::Vector3f* eigen_values_all_neighbors=
nullptr)
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;
738 int search_radius,
float max_distance,
int pixel_step=1)
const;
746 getViewingDirection (
const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction)
const;
818 os <<
"header: " << std::endl;
820 os <<
"points[]: " << r.
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>
Define standard C methods to do angle calculations.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where...
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).
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.
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
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 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.
int image_offset_y_
Position of the top left corner of the range image compared to an image of full size (360x180 degrees...
PCL_EXPORTS float getSurfaceChange(int x, int y, int radius) const
Calculates, how much the surface changes at a point.
static float atan2LookUp(float y, float x)
Query the std::atan2 lookup table.
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.
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...
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.
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.
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 isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
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 * getSurfaceChangeImage(int radius) const
Uses the above function for every point in the image.
int getImageOffsetX() const
Getter for image_offset_x_.
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
static float cosLookUp(float value)
Query the cos 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.
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
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.
static int max_no_of_threads
The maximum number of openmp threads that can be used in this class.
PCL_EXPORTS void change3dPointsToLocalCoordinateFrame()
This function sets the sensor pose to 0 and transforms all point positions to this local coordinate f...
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,...
shared_ptr< RangeImage > Ptr
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.
int getImageOffsetY() const
Getter for image_offset_y_.
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.
virtual PCL_EXPORTS void copyTo(RangeImage &other) const
Copy other to *this.
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)
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,...
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.
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
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 getAcutenessValueImages(int pixel_distance, float *´ness_value_image_x, float *´ness_value_image_y) const
Calculate getAcutenessValue for every point.
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
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 getAngularResolutionX() const
Getter for the angular resolution of the range image in x direction in radians per pixel.
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 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...
void integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.
static float asinLookUp(float value)
Query the asin lookup table.
static std::vector< float > atan_lookup_table
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel.
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.
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...
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first!
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
virtual PCL_EXPORTS RangeImage * getNew() const
Return a newly created Range image.
Ptr makeShared()
Get a boost shared pointer of a copy of this.
bool isInImage(int x, int y) const
Check if a point is inside of the image.
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.
PCL_EXPORTS void getMinMaxRanges(float &min_range, float &max_range) const
Find the minimum and maximum range in the image.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performance of multiplication compared to division
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.
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,...
float getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
virtual PCL_EXPORTS ~RangeImage()=default
Destructor.
static const int lookup_table_size
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
shared_ptr< const RangeImage > ConstPtr
void setImageOffsets(int offset_x, int offset_y)
Setter for image offsets.
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=nullptr, Eigen::Vector3f *mean_all_neighbors=nullptr, Eigen::Vector3f *eigen_values_all_neighbors=nullptr) const
Same as above but extracts some more data and can also return the extracted information for all neigh...
static std::vector< float > cos_lookup_table
static std::vector< float > asin_lookup_table
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_EXPORTS float * getRangesArray() const
Get all the range values in one float array of size width*height.
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
virtual void getHalfImage(RangeImage &half_image) const
Get a range image with half the resolution.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > VectorOfEigenVector3f
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
const Eigen::Affine3f & getTransformationToRangeImageSystem() const
Getter for the transformation from the world system into the range image system (the sensor coordinat...
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.
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
PCL_EXPORTS float * getInterpolatedSurfaceProjection(const Eigen::Vector3f &point, int pixel_size, float world_size) const
Same as above, but using the local coordinate frame defined by point and the viewing direction.
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.
PCL_EXPORTS void reset()
Reset all values to an empty range image.
bool getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
PCL_EXPORTS void setUnseenToMaxRange()
Sets all -INFINITY values to INFINITY.
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performance of multiplication compared to division
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...
void getImagePointFromAngles(float angle_x, float angle_y, float &image_x, float &image_y) const
Get the image point corresponding to the given angles.
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
static void createLookupTables()
Create lookup tables for trigonometric functions.
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...
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.
PCL_EXPORTS float * getImpactAngleImageBasedOnLocalNormals(int radius) const
Uses the above function for every point in the image.
PCL_EXPORTS RangeImage()
Constructor.
float getAngularResolutionY() const
Getter for the angular resolution of the range image in y direction in radians per pixel.
static PCL_EXPORTS void getCoordinateFrameTransformation(RangeImage::CoordinateFrame coordinate_frame, Eigen::Affine3f &transformation)
Get the transformation that transforms the given coordinate frame into CAMERA_FRAME.
void createEmpty(float angular_resolution_x, float angular_resolution_y, 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 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...
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.
const Eigen::Affine3f & getTransformationToWorldSystem() const
Getter for the transformation from the range image system (the sensor coordinate frame) into the worl...
Defines all the PCL implemented PointT point type structures.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Defines functions, macros and traits for allocating and using memory.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, padded with an extra range float.