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/common_headers.h>
45 #include <pcl/common/vector_average.h>
61 using Ptr = shared_ptr<RangeImage>;
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 (),
310 template <
typename Po
intCloudType>
void
311 doZBuffer (
const PointCloudType& point_cloud,
float noise_level,
312 float min_range,
int& top,
int& right,
int& bottom,
int& left);
315 template <
typename Po
intCloudType>
void
326 cropImage (
int border_size=0,
int top=-1,
int right=-1,
int bottom=-1,
int left=-1);
337 inline const Eigen::Affine3f&
347 inline const Eigen::Affine3f&
387 getPoint (
int image_x,
int image_y)
const;
391 getPoint (
int image_x,
int image_y);
395 getPoint (
float image_x,
float image_y)
const;
399 getPoint (
float image_x,
float image_y);
416 getPoint (
int image_x,
int image_y, Eigen::Vector3f& point)
const;
420 getPoint (
int index, Eigen::Vector3f& point)
const;
423 inline const Eigen::Map<const Eigen::Vector3f>
427 inline const Eigen::Map<const Eigen::Vector3f>
443 calculate3DPoint (
float image_x,
float image_y,
float range, Eigen::Vector3f& point)
const;
446 calculate3DPoint (
float image_x,
float image_y, Eigen::Vector3f& point)
const;
454 getImagePoint (
const Eigen::Vector3f& point,
float& image_x,
float& image_y,
float& range)
const;
458 getImagePoint (
const Eigen::Vector3f& point,
int& image_x,
int& image_y,
float& range)
const;
462 getImagePoint (
const Eigen::Vector3f& point,
float& image_x,
float& image_y)
const;
466 getImagePoint (
const Eigen::Vector3f& point,
int& image_x,
int& image_y)
const;
470 getImagePoint (
float x,
float y,
float z,
float& image_x,
float& image_y,
float& range)
const;
474 getImagePoint (
float x,
float y,
float z,
float& image_x,
float& image_y)
const;
478 getImagePoint (
float x,
float y,
float z,
int& image_x,
int& image_y)
const;
501 real2DToInt2D (
float x,
float y,
int& xInt,
int& yInt)
const;
527 getNormal (
int x,
int y,
int radius, Eigen::Vector3f& normal,
int step_size=1)
const;
532 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size=1)
const;
537 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
538 Eigen::Vector3f* point_on_plane=
nullptr,
int step_size=1)
const;
548 int no_of_closest_neighbors,
int step_size,
549 float& max_closest_neighbor_distance_squared,
550 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
551 Eigen::Vector3f* normal_all_neighbors=
nullptr,
552 Eigen::Vector3f* mean_all_neighbors=
nullptr,
553 Eigen::Vector3f* eigen_values_all_neighbors=
nullptr)
const;
592 float*& acuteness_value_image_y)
const;
611 getSurfaceAngleChange (
int x,
int y,
int radius,
float& angle_change_x,
float& angle_change_y)
const;
619 getCurvature (
int x,
int y,
int radius,
int step_size)
const;
622 inline const Eigen::Vector3f
656 getSubImage (
int sub_image_image_offset_x,
int sub_image_image_offset_y,
int sub_image_width,
657 int sub_image_height,
int combine_pixels,
RangeImage& sub_image)
const;
683 inline Eigen::Affine3f
688 Eigen::Affine3f& transformation)
const;
696 float max_dist, Eigen::Affine3f& transformation)
const;
701 getIntegralImage (
float*& integral_image,
int*& valid_points_num_image)
const;
709 PCL_EXPORTS
virtual void
736 getOverlap (
const RangeImage& other_range_image,
const Eigen::Affine3f& relative_transformation,
737 int search_radius,
float max_distance,
int pixel_step=1)
const;
745 getViewingDirection (
const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction)
const;
753 PCL_EXPORTS
virtual void
817 os <<
"header: " << std::endl;
819 os <<
"points[]: " << r.
points.size () << std::endl;
820 os <<
"width: " << r.
width << std::endl;
821 os <<
"height: " << r.
height << std::endl;
822 os <<
"sensor_origin_: "
826 os <<
"sensor_orientation_: "
831 os <<
"is_dense: " << r.
is_dense << std::endl;
832 os <<
"angular resolution: "
841 #include <pcl/range_image/impl/range_image.hpp>
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.
virtual PCL_EXPORTS ~RangeImage()=default
Destructor.
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
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.
shared_ptr< RangeImage > Ptr
const Eigen::Affine3f & getTransformationToRangeImageSystem() const
Getter for the transformation from the world system into the range image system (the sensor coordinat...
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.
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.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
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)
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::uint32_t width
The point cloud width (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
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_.
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...
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.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
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 std::atan2 lookup table.
PCL_EXPORTS float getSurfaceChange(int x, int y, int radius) const
Calculates, how much the surface changes at a point.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > VectorOfEigenVector3f
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 performance of multiplication compared to division ...
pcl::PCLHeader header
The point cloud header.
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 performance 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_.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
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.
shared_ptr< const RangeImage > ConstPtr
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.
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 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.