41#include <pcl/range_image/range_image.h>
45#include <pcl/common/point_tests.h>
46#include <pcl/common/vector_average.h>
68 if (std::abs (x) < std::abs (y))
96template <
typename Po
intCloudType>
void
107template <
typename Po
intCloudType>
void
144template <
typename Po
intCloudType>
void
155template <
typename Po
intCloudType>
void
205template <
typename Po
intCloudTypeWithViewpo
ints>
void
218template <
typename Po
intCloudTypeWithViewpo
ints>
void
232template <
typename Po
intCloudType>
void
235 using PointType2 =
typename PointCloudType::PointType;
246 for (
const auto& point:
points2)
270 for (
int i=0; i<4; ++i)
283 top= (std::min) (top,
n_y); right= (std::max) (right,
n_x); bottom= (std::max) (bottom,
n_y); left= (std::min) (left,
n_x);
315 top= (std::min) (top, y); right= (std::max) (right, x); bottom= (std::max) (bottom, y); left= (std::min) (left, x);
332 Eigen::Vector3f point (x, y, z);
414 return -std::numeric_limits<float>::infinity ();
419 return std::numeric_limits<float>::infinity ();
420 return -std::numeric_limits<float>::infinity ();
445 return (x >= 0 && x <
static_cast<int> (
width) && y >= 0 && y <
static_cast<int> (
height));
459 return std::isfinite (
getPoint (index).range);
474 return std::isinf (range) && range>0.0f;
545 point =
getPoint (index).getVector3fMap ();
549const Eigen::Map<const Eigen::Vector3f>
552 return getPoint (x, y).getVector3fMap ();
556const Eigen::Map<const Eigen::Vector3f>
559 return getPoint (index).getVector3fMap ();
614 return -std::numeric_limits<float>::infinity ();
621 if ( (std::isinf (point1.
range)&&point1.
range<0) || (std::isinf (point2.
range)&&point2.
range<0))
622 return -std::numeric_limits<float>::infinity ();
630 if (
r2 > 0.0f && !std::isinf (
r1))
633 else if (!std::isinf (
r1))
638 d = std::sqrt (
dSqr);
656 return -std::numeric_limits<float>::infinity ();
670 return -std::numeric_limits<float>::infinity ();
688 Eigen::Vector3f point;
699 Eigen::Vector3f left;
712 Eigen::Vector3f right;
744 Eigen::Vector3f bottom;
797 return Eigen::Vector3f (point.x, point.y, point.z);
845 return -std::numeric_limits<float>::infinity ();
848 if (std::isinf (point1.
range) && std::isinf (point2.range))
850 if (std::isinf (point1.
range) || std::isinf (point2.range))
851 return std::numeric_limits<float>::infinity ();
863 int x1=x+i*offset_x,
y1=y+i*offset_y;
864 int x2=x+ (i+1)*offset_x,
y2=y+ (i+1)*offset_y;
887 return -std::numeric_limits<float>::infinity ();
890 Eigen::Vector3f normal;
892 return -std::numeric_limits<float>::infinity ();
902 for (
int y2=y-radius;
y2<=y+radius;
y2+=step_size)
904 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
909 if (!std::isfinite (point.
range))
929 return -std::numeric_limits<float>::infinity ();
955 struct NeighborWithDistance
967 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
972 normal.setZero (); mean.setZero (); eigen_values.setZero ();
988 for (
int y2=y-radius;
y2<=y+radius;
y2+=step_size)
990 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1055 if (!std::isfinite (point.
range))
1056 return -std::numeric_limits<float>::infinity ();
1062 for (
int y2=y-radius;
y2<=y+radius;
y2+=step_size)
1064 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1083 Eigen::Vector3f& normal, Eigen::Vector3f*
point_on_plane,
int step_size)
const
1085 Eigen::Vector3f mean, eigen_values;
1088 normal, mean, eigen_values);
1093 *
point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1104 for (
int y2=y-radius;
y2<=y+radius;
y2+=step_size)
1106 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1111 if (!std::isfinite (point.
range))
1113 vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
1118 Eigen::Vector3f eigen_values;
1120 return eigen_values[0]/eigen_values.sum ();
1125template <
typename Po
intCloudTypeWithViewpo
ints> Eigen::Vector3f
1132 if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1165 Eigen::Affine3f transformation;
1167 return transformation;
1221template <
typename Po
intCloudType>
void
1244 for (
int i=0; i<4; ++i)
1251 image_point.range = std::numeric_limits<float>::infinity ();
Iterator class for point clouds with or without given indices.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
std::vector< PointWithRange, Eigen::aligned_allocator< PointWithRange > > points
The point data.
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
int image_offset_y_
Position of the top left corner of the range image compared to an image of full size (360x180 degrees...
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.
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.
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.
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
static float cosLookUp(float value)
Query the cos lookup table.
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
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.
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.
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...
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.
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.
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.
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.
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.
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...
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
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.
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 getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
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
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
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.
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 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.
Define standard C methods to do distance calculations.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
void getTransformationFromTwoUnitVectorsAndOrigin(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, const Eigen::Vector3f &origin, Eigen::Affine3f &transformation)
Get the transformation that will translate origin to (0,0,0) and rotate z_axis into (0,...
void getTransformationFromTwoUnitVectors(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate z_axis into (0,0,1) and y_direction into a vector with x=...
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Defines all the PCL and non-PCL macros used.
#define ERASE_ARRAY(var, size)
A point structure representing Euclidean xyz coordinates, padded with an extra range float.