41 #include <pcl/range_image/range_image.h>
45 #include <pcl/common/point_tests.h>
46 #include <pcl/common/vector_average.h>
69 if (std::abs (x) < std::abs (y))
75 ret =
static_cast<float> (x*y > 0 ?
M_PI/2-ret : -
M_PI/2-ret);
83 ret =
static_cast<float> (y < 0 ? ret-
M_PI : ret+
M_PI);
97 template <
typename Po
intCloudType>
void
99 float max_angle_width,
float max_angle_height,
101 float noise_level,
float min_range,
int border_size)
103 createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height,
104 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
108 template <
typename Po
intCloudType>
void
110 float angular_resolution_x,
float angular_resolution_y,
111 float max_angle_width,
float max_angle_height,
113 float noise_level,
float min_range,
int border_size)
137 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
139 if (border_size != std::numeric_limits<int>::min()) {
140 cropImage (border_size, top, right, bottom, left);
147 template <
typename Po
intCloudType>
void
149 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
151 float noise_level,
float min_range,
int border_size)
154 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
158 template <
typename Po
intCloudType>
void
160 float angular_resolution_x,
float angular_resolution_y,
161 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
163 float noise_level,
float min_range,
int border_size)
170 if ((point_cloud_center-sensor_pose.translation()).norm() <= point_cloud_radius) {
173 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
183 float max_angle_size =
getMaxAngleSize (sensor_pose, point_cloud_center, point_cloud_radius);
186 width = 2*pixel_radius_x;
187 height = 2*pixel_radius_y;
191 int center_pixel_x, center_pixel_y;
192 getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y);
200 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
202 if (border_size != std::numeric_limits<int>::min()) {
203 cropImage (border_size, top, right, bottom, left);
210 template <
typename Po
intCloudTypeWithViewpo
ints>
void
212 float angular_resolution,
213 float max_angle_width,
float max_angle_height,
215 float noise_level,
float min_range,
int border_size)
218 max_angle_width, max_angle_height, coordinate_frame,
219 noise_level, min_range, border_size);
223 template <
typename Po
intCloudTypeWithViewpo
ints>
void
225 float angular_resolution_x,
float angular_resolution_y,
226 float max_angle_width,
float max_angle_height,
228 float noise_level,
float min_range,
int border_size)
231 Eigen::Affine3f sensor_pose =
static_cast<Eigen::Affine3f
> (Eigen::Translation3f (average_viewpoint));
232 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height,
233 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
237 template <
typename Po
intCloudType>
void
238 RangeImage::doZBuffer (
const PointCloudType& point_cloud,
float noise_level,
float min_range,
int& top,
int& right,
int& bottom,
int& left)
240 using PointType2 =
typename PointCloudType::PointType;
244 std::vector<int> counters(
size, 0);
248 float x_real, y_real, range_of_current_point;
250 for (
const auto& point: points2)
256 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
259 if (range_of_current_point < min_range|| !
isInImage (x, y))
264 int floor_x =
pcl_lrint (std::floor (x_real)), floor_y =
pcl_lrint (std::floor (y_real)),
267 int neighbor_x[4], neighbor_y[4];
268 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
269 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
270 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
271 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
274 for (
int i=0; i<4; ++i)
276 int n_x=neighbor_x[i], n_y=neighbor_y[i];
278 if (n_x==x && n_y==y)
282 int neighbor_array_pos = n_y*
width + n_x;
283 if (counters[neighbor_array_pos] == 0)
285 float& neighbor_range =
points[neighbor_array_pos].range;
286 neighbor_range = (std::isinf (neighbor_range) ? range_of_current_point : (std::min) (neighbor_range, range_of_current_point));
287 top= (std::min) (top, n_y); right= (std::max) (right, n_x); bottom= (std::max) (bottom, n_y); left= (std::min) (left, n_x);
294 int arrayPos = y*
width + x;
295 float& range_at_image_point =
points[arrayPos].range;
296 int& counter = counters[arrayPos];
297 bool addCurrentPoint=
false, replace_with_current_point=
false;
301 replace_with_current_point =
true;
305 if (range_of_current_point < range_at_image_point-noise_level)
307 replace_with_current_point =
true;
309 else if (std::fabs (range_of_current_point-range_at_image_point)<=noise_level)
311 addCurrentPoint =
true;
315 if (replace_with_current_point)
318 range_at_image_point = range_of_current_point;
319 top= (std::min) (top, y); right= (std::max) (right, x); bottom= (std::max) (bottom, y); left= (std::min) (left, x);
322 else if (addCurrentPoint)
325 range_at_image_point += (range_of_current_point-range_at_image_point)/counter;
334 Eigen::Vector3f point (x, y, z);
350 float image_x_float, image_y_float;
352 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
360 range = transformedPoint.norm ();
361 if (range < std::numeric_limits<float>::epsilon()) {
362 PCL_DEBUG (
"[pcl::RangeImage::getImagePoint] Transformed point is (0,0,0), cannot project it.\n");
363 image_x = image_y = 0.0f;
366 float angle_x =
atan2LookUp (transformedPoint[0], transformedPoint[2]),
367 angle_y =
asinLookUp (transformedPoint[1]/range);
377 float image_x_float, image_y_float;
379 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
394 float image_x_float, image_y_float;
396 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
403 int image_x, image_y;
409 point_in_image =
getPoint (image_x, image_y);
417 int image_x, image_y;
421 return -std::numeric_limits<float>::infinity ();
423 if (std::isinf (image_point_range))
425 if (image_point_range > 0.0f)
426 return std::numeric_limits<float>::infinity ();
427 return -std::numeric_limits<float>::infinity ();
429 return image_point_range - range;
452 return (x >= 0 && x <
static_cast<int> (
width) && y >= 0 && y <
static_cast<int> (
height));
466 return std::isfinite (
getPoint (index).range);
481 return std::isinf (range) && range>0.0f;
545 point =
getPoint (image_x, image_y).getVector3fMap ();
552 point =
getPoint (index).getVector3fMap ();
556 const Eigen::Map<const Eigen::Vector3f>
559 return getPoint (x, y).getVector3fMap ();
563 const Eigen::Map<const Eigen::Vector3f>
566 return getPoint (index).getVector3fMap ();
573 float angle_x, angle_y;
577 float cosY = std::cos (angle_y);
578 point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * std::cos (angle_x)*cosY);
594 Eigen::Vector3f tmp_point;
596 point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
612 float cos_angle_y = std::cos (angle_y);
621 return -std::numeric_limits<float>::infinity ();
628 if ( (std::isinf (point1.
range)&&point1.
range<0) || (std::isinf (point2.
range)&&point2.
range<0))
629 return -std::numeric_limits<float>::infinity ();
631 float r1 = (std::min) (point1.
range, point2.
range),
633 float impact_angle =
static_cast<float> (0.5f *
M_PI);
637 if (r2 > 0.0f && !std::isinf (r1))
640 else if (!std::isinf (r1))
645 d = std::sqrt (dSqr);
646 float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
647 cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
648 impact_angle = std::acos (cos_impact_angle);
652 impact_angle = -impact_angle;
662 if (std::isinf (impact_angle))
663 return -std::numeric_limits<float>::infinity ();
664 float ret = 1.0f -
static_cast<float>(std::fabs (impact_angle)/ (0.5f*
M_PI));
665 if (impact_angle < 0.0f)
677 return -std::numeric_limits<float>::infinity ();
682 const Eigen::Vector3f
692 angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
695 Eigen::Vector3f point;
701 Eigen::Vector3f transformed_left;
703 transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
706 Eigen::Vector3f left;
708 transformed_left = - (transformation * left);
710 transformed_left[1] = 0.0f;
711 transformed_left.normalize ();
714 Eigen::Vector3f transformed_right;
716 transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
719 Eigen::Vector3f right;
721 transformed_right = transformation * right;
723 transformed_right[1] = 0.0f;
724 transformed_right.normalize ();
726 angle_change_x = transformed_left.dot (transformed_right);
727 angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
728 angle_change_x = std::acos (angle_change_x);
733 Eigen::Vector3f transformed_top;
735 transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
740 transformed_top = - (transformation * top);
742 transformed_top[0] = 0.0f;
743 transformed_top.normalize ();
746 Eigen::Vector3f transformed_bottom;
748 transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
751 Eigen::Vector3f bottom;
753 transformed_bottom = transformation * bottom;
755 transformed_bottom[0] = 0.0f;
756 transformed_bottom.normalize ();
758 angle_change_y = transformed_top.dot (transformed_bottom);
759 angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
760 angle_change_y = std::acos (angle_change_y);
797 return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
804 return {point.x, point.y, point.z};
813 float weight_sum = 1.0f;
815 if (std::isinf (average_point.
range))
817 if (average_point.
range>0.0f)
820 average_point.x = average_point.y = average_point.z = average_point.
range = 0.0f;
824 Vector4fMap average_point_eigen = average_point.getVector4fMap ();
826 for (
int step=1; step<no_of_points; ++step)
829 x2+=delta_x; y2+=delta_y;
833 average_point_eigen+=p.getVector4fMap (); average_point.
range+=p.
range;
836 if (weight_sum<= 0.0f)
841 float normalization_factor = 1.0f/weight_sum;
842 average_point_eigen *= normalization_factor;
843 average_point.
range *= normalization_factor;
852 return -std::numeric_limits<float>::infinity ();
855 if (std::isinf (point1.
range) && std::isinf (point2.range))
857 if (std::isinf (point1.
range) || std::isinf (point2.range))
858 return std::numeric_limits<float>::infinity ();
866 float average_pixel_distance = 0.0f;
868 for (
int i=0; i<max_steps; ++i)
870 int x1=x+i*offset_x, y1=y+i*offset_y;
871 int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y;
873 if (!std::isfinite (pixel_distance))
877 return pixel_distance;
882 average_pixel_distance += std::sqrt (pixel_distance);
884 average_pixel_distance /= weight;
886 return average_pixel_distance;
894 return -std::numeric_limits<float>::infinity ();
896 int no_of_nearest_neighbors =
static_cast<int> (pow (
static_cast<double> ( (radius + 1.0)), 2.0));
897 Eigen::Vector3f normal;
899 return -std::numeric_limits<float>::infinity ();
909 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
911 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
916 if (!std::isfinite (point.
range))
918 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
923 Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
924 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
935 if (std::isinf (impact_angle))
936 return -std::numeric_limits<float>::infinity ();
937 float ret = 1.0f -
static_cast<float> ( (impact_angle / (0.5f *
M_PI)));
945 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size)
const
947 return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal,
nullptr, step_size);
956 int no_of_nearest_neighbors =
static_cast<int> (pow (
static_cast<double> (radius + 1.0), 2.0));
962 struct NeighborWithDistance
966 bool operator < (
const NeighborWithDistance& other)
const {
return distance<other.distance;}
973 float& max_closest_neighbor_distance_squared,
974 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
975 Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
976 Eigen::Vector3f* eigen_values_all_neighbors)
const
978 max_closest_neighbor_distance_squared=0.0f;
979 normal.setZero (); mean.setZero (); eigen_values.setZero ();
980 if (normal_all_neighbors!=
nullptr)
981 normal_all_neighbors->setZero ();
982 if (mean_all_neighbors!=
nullptr)
983 mean_all_neighbors->setZero ();
984 if (eigen_values_all_neighbors!=
nullptr)
985 eigen_values_all_neighbors->setZero ();
987 const auto sqrt_blocksize = 2 * radius + 1;
988 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
991 given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
993 std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
994 int neighbor_counter = 0;
995 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
997 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1001 NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
1002 neighbor_with_distance.neighbor = &
getPoint (x2, y2);
1007 no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
1009 std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter);
1013 max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1015 float max_distance_squared = max_closest_neighbor_distance_squared*4.0f;
1021 for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1023 if (ordered_neighbors[neighbor_idx].
distance > max_distance_squared)
1026 vector_average.
add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1032 Eigen::Vector3f eigen_vector2, eigen_vector3;
1033 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1034 Eigen::Vector3f viewing_direction = (
getSensorPos ()-point).normalized ();
1035 if (normal.dot (viewing_direction) < 0.0f)
1037 mean = vector_average.
getMean ();
1039 if (normal_all_neighbors==
nullptr)
1043 for (
int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1044 vector_average.
add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1046 vector_average.
doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1048 if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1049 *normal_all_neighbors *= -1.0f;
1050 *mean_all_neighbors = vector_average.
getMean ();
1062 if (!std::isfinite (point.
range))
1063 return -std::numeric_limits<float>::infinity ();
1065 const auto sqrt_blocksize = 2 * radius + 1;
1066 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
1067 std::vector<float> neighbor_distances (blocksize);
1068 int neighbor_counter = 0;
1069 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1071 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1073 if (!
isValid (x2, y2) || (x2==x&&y2==y))
1076 float& neighbor_distance = neighbor_distances[neighbor_counter++];
1080 std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter);
1082 n = (std::min) (neighbor_counter, n);
1083 return neighbor_distances[n-1];
1090 Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane,
int step_size)
const
1092 Eigen::Vector3f mean, eigen_values;
1093 float used_squared_max_distance;
1094 bool ret =
getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1095 normal, mean, eigen_values);
1099 if (point_on_plane !=
nullptr)
1100 *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1111 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1113 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1118 if (!std::isfinite (point.
range))
1120 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
1125 Eigen::Vector3f eigen_values;
1126 vector_average.
doPCA (eigen_values);
1127 return eigen_values[0]/eigen_values.sum ();
1132 template <
typename Po
intCloudTypeWithViewpo
ints> Eigen::Vector3f
1135 Eigen::Vector3f average_viewpoint (0,0,0);
1136 int point_counter = 0;
1137 for (
const auto& point: point_cloud.points)
1139 if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1141 average_viewpoint[0] += point.vp_x;
1142 average_viewpoint[1] += point.vp_y;
1143 average_viewpoint[2] += point.vp_z;
1146 average_viewpoint /= point_counter;
1148 return average_viewpoint;
1165 viewing_direction = (point-
getSensorPos ()).normalized ();
1172 Eigen::Affine3f transformation;
1174 return transformation;
1181 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1189 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1228 template <
typename Po
intCloudType>
void
1231 float x_real, y_real, range_of_current_point;
1232 for (
const auto& point: far_ranges.points)
1238 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
1240 int floor_x =
static_cast<int> (
pcl_lrint (std::floor (x_real))),
1241 floor_y =
static_cast<int> (
pcl_lrint (std::floor (y_real))),
1242 ceil_x =
static_cast<int> (
pcl_lrint (std::ceil (x_real))),
1243 ceil_y =
static_cast<int> (
pcl_lrint (std::ceil (y_real)));
1245 int neighbor_x[4], neighbor_y[4];
1246 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1247 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1248 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1249 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1251 for (
int i=0; i<4; ++i)
1253 int x=neighbor_x[i], y=neighbor_y[i];
1257 if (!std::isfinite (image_point.
range))
1258 image_point.
range = std::numeric_limits<float>::infinity ();
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.
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel.
static PCL_EXPORTS std::vector< float > cos_lookup_table
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.
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.
static PCL_EXPORTS std::vector< float > atan_lookup_table
static PCL_EXPORTS std::vector< float > asin_lookup_table
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...
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.
static PCL_EXPORTS const int lookup_table_size
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.
Calculates the weighted average and the covariance matrix.
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
void doPCA(VectorType &eigen_values, VectorType &eigen_vector1, VectorType &eigen_vector2, VectorType &eigen_vector3) const
Do Principal component analysis.
const VectorType & getMean() const
Get the mean of the added vectors.
unsigned int getNoOfSamples()
Get the number of added vectors.
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=...
float distance(const PointT &p1, const PointT &p2)
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
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...
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, padded with an extra range float.