41 #include <pcl/range_image/range_image.h>
43 #include <pcl/pcl_macros.h>
44 #include <pcl/common/distances.h>
45 #include <pcl/common/point_tests.h>
57 static_cast<float> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1) / 2.0f) * value)) +
68 if (std::abs (x) < std::abs (y))
72 static_cast<float> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1) / 2.0f) * (x / y))) +
74 ret =
static_cast<float> (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret);
79 static_cast<float> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1) / 2.0f) * (y / x))) +
82 ret =
static_cast<float> (y < 0 ? ret-M_PI : ret+M_PI);
91 int cell_idx =
static_cast<int> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1)) * std::abs (value) / (2.0f * static_cast<float> (M_PI))));
96 template <
typename Po
intCloudType>
void
98 float max_angle_width,
float max_angle_height,
100 float noise_level,
float min_range,
int border_size)
102 createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height,
103 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
107 template <
typename Po
intCloudType>
void
109 float angular_resolution_x,
float angular_resolution_y,
110 float max_angle_width,
float max_angle_height,
112 float noise_level,
float min_range,
int border_size)
119 int full_width =
static_cast<int> (pcl_lrint (std::floor (
pcl::deg2rad (360.0f)*angular_resolution_x_reciprocal_))),
120 full_height = static_cast<int> (pcl_lrint (std::floor (
pcl::deg2rad (180.0f)*angular_resolution_y_reciprocal_)));
136 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
138 cropImage (border_size, top, right, bottom, left);
144 template <
typename Po
intCloudType>
void
146 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
148 float noise_level,
float min_range,
int border_size)
151 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
155 template <
typename Po
intCloudType>
void
157 float angular_resolution_x,
float angular_resolution_y,
158 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
160 float noise_level,
float min_range,
int border_size)
167 if ((point_cloud_center-sensor_pose.translation()).norm() <= point_cloud_radius) {
170 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
180 float max_angle_size =
getMaxAngleSize (sensor_pose, point_cloud_center, point_cloud_radius);
183 width = 2*pixel_radius_x;
184 height = 2*pixel_radius_y;
188 int center_pixel_x, center_pixel_y;
189 getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y);
197 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
199 cropImage (border_size, top, right, bottom, left);
205 template <
typename Po
intCloudTypeWithViewpo
ints>
void
207 float angular_resolution,
208 float max_angle_width,
float max_angle_height,
210 float noise_level,
float min_range,
int border_size)
213 max_angle_width, max_angle_height, coordinate_frame,
214 noise_level, min_range, border_size);
218 template <
typename Po
intCloudTypeWithViewpo
ints>
void
220 float angular_resolution_x,
float angular_resolution_y,
221 float max_angle_width,
float max_angle_height,
223 float noise_level,
float min_range,
int border_size)
226 Eigen::Affine3f sensor_pose =
static_cast<Eigen::Affine3f
> (Eigen::Translation3f (average_viewpoint));
227 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height,
228 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
232 template <
typename Po
intCloudType>
void
233 RangeImage::doZBuffer (
const PointCloudType& point_cloud,
float noise_level,
float min_range,
int& top,
int& right,
int& bottom,
int& left)
235 using PointType2 =
typename PointCloudType::PointType;
239 int* counters =
new int[
size];
240 ERASE_ARRAY (counters, size);
244 float x_real, y_real, range_of_current_point;
246 for (
const auto& point: points2)
252 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
255 if (range_of_current_point < min_range|| !
isInImage (x, y))
260 int floor_x = pcl_lrint (std::floor (x_real)), floor_y = pcl_lrint (std::floor (y_real)),
261 ceil_x = pcl_lrint (std::ceil (x_real)), ceil_y = pcl_lrint (std::ceil (y_real));
263 int neighbor_x[4], neighbor_y[4];
264 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
265 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
266 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
267 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
270 for (
int i=0; i<4; ++i)
272 int n_x=neighbor_x[i], n_y=neighbor_y[i];
274 if (n_x==x && n_y==y)
278 int neighbor_array_pos = n_y*
width + n_x;
279 if (counters[neighbor_array_pos]==0)
281 float& neighbor_range =
points[neighbor_array_pos].range;
282 neighbor_range = (std::isinf (neighbor_range) ? range_of_current_point : (std::min) (neighbor_range, range_of_current_point));
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);
290 int arrayPos = y*
width + x;
291 float& range_at_image_point =
points[arrayPos].range;
292 int& counter = counters[arrayPos];
293 bool addCurrentPoint=
false, replace_with_current_point=
false;
297 replace_with_current_point =
true;
301 if (range_of_current_point < range_at_image_point-noise_level)
303 replace_with_current_point =
true;
305 else if (std::fabs (range_of_current_point-range_at_image_point)<=noise_level)
307 addCurrentPoint =
true;
311 if (replace_with_current_point)
314 range_at_image_point = range_of_current_point;
315 top= (std::min) (top, y); right= (std::max) (right, x); bottom= (std::max) (bottom, y); left= (std::min) (left, x);
318 else if (addCurrentPoint)
321 range_at_image_point += (range_of_current_point-range_at_image_point)/counter;
332 Eigen::Vector3f point (x, y, z);
348 float image_x_float, image_y_float;
350 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
358 range = transformedPoint.norm ();
359 float angle_x =
atan2LookUp (transformedPoint[0], transformedPoint[2]),
360 angle_y =
asinLookUp (transformedPoint[1]/range);
370 float image_x_float, image_y_float;
372 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
387 float image_x_float, image_y_float;
389 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
396 int image_x, image_y;
402 point_in_image =
getPoint (image_x, image_y);
410 int image_x, image_y;
414 return -std::numeric_limits<float>::infinity ();
416 if (std::isinf (image_point_range))
418 if (image_point_range > 0.0f)
419 return std::numeric_limits<float>::infinity ();
420 return -std::numeric_limits<float>::infinity ();
422 return image_point_range - range;
437 xInt =
static_cast<int> (pcl_lrintf (x));
438 yInt =
static_cast<int> (pcl_lrintf (y));
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;
538 point =
getPoint (image_x, image_y).getVector3fMap ();
545 point =
getPoint (index).getVector3fMap ();
549 const Eigen::Map<const Eigen::Vector3f>
552 return getPoint (x, y).getVector3fMap ();
556 const Eigen::Map<const Eigen::Vector3f>
559 return getPoint (index).getVector3fMap ();
566 float angle_x, angle_y;
570 float cosY = std::cos (angle_y);
571 point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * std::cos (angle_x)*cosY);
587 Eigen::Vector3f tmp_point;
589 point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
605 float cos_angle_y = std::cos (angle_y);
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 ();
624 float r1 = (std::min) (point1.
range, point2.
range),
626 float impact_angle =
static_cast<float> (0.5f * M_PI);
630 if (r2 > 0.0f && !std::isinf (r1))
633 else if (!std::isinf (r1))
638 d = std::sqrt (dSqr);
639 float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
640 cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
641 impact_angle = std::acos (cos_impact_angle);
645 impact_angle = -impact_angle;
655 if (std::isinf (impact_angle))
656 return -std::numeric_limits<float>::infinity ();
657 float ret = 1.0f - float (std::fabs (impact_angle)/ (0.5f*M_PI));
658 if (impact_angle < 0.0f)
670 return -std::numeric_limits<float>::infinity ();
675 const Eigen::Vector3f
685 angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
688 Eigen::Vector3f point;
694 Eigen::Vector3f transformed_left;
696 transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
699 Eigen::Vector3f left;
701 transformed_left = - (transformation * left);
703 transformed_left[1] = 0.0f;
704 transformed_left.normalize ();
707 Eigen::Vector3f transformed_right;
709 transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
712 Eigen::Vector3f right;
714 transformed_right = transformation * right;
716 transformed_right[1] = 0.0f;
717 transformed_right.normalize ();
719 angle_change_x = transformed_left.dot (transformed_right);
720 angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
721 angle_change_x = std::acos (angle_change_x);
726 Eigen::Vector3f transformed_top;
728 transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
733 transformed_top = - (transformation * top);
735 transformed_top[0] = 0.0f;
736 transformed_top.normalize ();
739 Eigen::Vector3f transformed_bottom;
741 transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
744 Eigen::Vector3f bottom;
746 transformed_bottom = transformation * bottom;
748 transformed_bottom[0] = 0.0f;
749 transformed_bottom.normalize ();
751 angle_change_y = transformed_top.dot (transformed_bottom);
752 angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
753 angle_change_y = std::acos (angle_change_y);
790 return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
797 return Eigen::Vector3f (point.x, point.y, point.z);
806 float weight_sum = 1.0f;
808 if (std::isinf (average_point.
range))
810 if (average_point.
range>0.0f)
813 average_point.x = average_point.y = average_point.z = average_point.
range = 0.0f;
817 Vector4fMap average_point_eigen = average_point.getVector4fMap ();
819 for (
int step=1; step<no_of_points; ++step)
822 x2+=delta_x; y2+=delta_y;
826 average_point_eigen+=p.getVector4fMap (); average_point.
range+=p.
range;
829 if (weight_sum<= 0.0f)
834 float normalization_factor = 1.0f/weight_sum;
835 average_point_eigen *= normalization_factor;
836 average_point.
range *= normalization_factor;
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 ();
859 float average_pixel_distance = 0.0f;
861 for (
int i=0; i<max_steps; ++i)
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;
866 if (!std::isfinite (pixel_distance))
870 return pixel_distance;
875 average_pixel_distance += std::sqrt (pixel_distance);
877 average_pixel_distance /= weight;
879 return average_pixel_distance;
887 return -std::numeric_limits<float>::infinity ();
889 int no_of_nearest_neighbors =
static_cast<int> (pow (static_cast<double> ( (radius + 1.0)), 2.0));
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))
911 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
916 Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
917 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
928 if (std::isinf (impact_angle))
929 return -std::numeric_limits<float>::infinity ();
930 float ret = 1.0f -
static_cast<float> ( (impact_angle / (0.5f * M_PI)));
938 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size)
const
940 return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal,
nullptr, step_size);
949 int no_of_nearest_neighbors =
static_cast<int> (pow (static_cast<double> (radius + 1.0), 2.0));
955 struct NeighborWithDistance
959 bool operator < (
const NeighborWithDistance& other)
const {
return distance<other.distance;}
966 float& max_closest_neighbor_distance_squared,
967 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
968 Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
969 Eigen::Vector3f* eigen_values_all_neighbors)
const
971 max_closest_neighbor_distance_squared=0.0f;
972 normal.setZero (); mean.setZero (); eigen_values.setZero ();
973 if (normal_all_neighbors!=
nullptr)
974 normal_all_neighbors->setZero ();
975 if (mean_all_neighbors!=
nullptr)
976 mean_all_neighbors->setZero ();
977 if (eigen_values_all_neighbors!=
nullptr)
978 eigen_values_all_neighbors->setZero ();
980 const auto sqrt_blocksize = 2 * radius + 1;
981 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
984 given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
986 std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
987 int neighbor_counter = 0;
988 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
990 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
994 NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
995 neighbor_with_distance.neighbor = &
getPoint (x2, y2);
1000 no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
1002 std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter);
1006 max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1008 float max_distance_squared = max_closest_neighbor_distance_squared*4.0f;
1014 for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1016 if (ordered_neighbors[neighbor_idx].distance > max_distance_squared)
1019 vector_average.
add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1025 Eigen::Vector3f eigen_vector2, eigen_vector3;
1026 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1027 Eigen::Vector3f viewing_direction = (
getSensorPos ()-point).normalized ();
1028 if (normal.dot (viewing_direction) < 0.0f)
1030 mean = vector_average.
getMean ();
1032 if (normal_all_neighbors==
nullptr)
1036 for (
int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1037 vector_average.
add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1039 vector_average.
doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1041 if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1042 *normal_all_neighbors *= -1.0f;
1043 *mean_all_neighbors = vector_average.
getMean ();
1055 if (!std::isfinite (point.
range))
1056 return -std::numeric_limits<float>::infinity ();
1058 const auto sqrt_blocksize = 2 * radius + 1;
1059 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
1060 std::vector<float> neighbor_distances (blocksize);
1061 int neighbor_counter = 0;
1062 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1064 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1066 if (!
isValid (x2, y2) || (x2==x&&y2==y))
1069 float& neighbor_distance = neighbor_distances[neighbor_counter++];
1073 std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter);
1075 n = (std::min) (neighbor_counter, n);
1076 return neighbor_distances[n-1];
1083 Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane,
int step_size)
const
1085 Eigen::Vector3f mean, eigen_values;
1086 float used_squared_max_distance;
1087 bool ret =
getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1088 normal, mean, eigen_values);
1092 if (point_on_plane !=
nullptr)
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;
1119 vector_average.
doPCA (eigen_values);
1120 return eigen_values[0]/eigen_values.sum ();
1125 template <
typename Po
intCloudTypeWithViewpo
ints> Eigen::Vector3f
1128 Eigen::Vector3f average_viewpoint (0,0,0);
1129 int point_counter = 0;
1130 for (
const auto& point: point_cloud.points)
1132 if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1134 average_viewpoint[0] += point.vp_x;
1135 average_viewpoint[1] += point.vp_y;
1136 average_viewpoint[2] += point.vp_z;
1139 average_viewpoint /= point_counter;
1141 return average_viewpoint;
1158 viewing_direction = (point-
getSensorPos ()).normalized ();
1165 Eigen::Affine3f transformation;
1167 return transformation;
1174 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1182 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1221 template <
typename Po
intCloudType>
void
1224 float x_real, y_real, range_of_current_point;
1225 for (
const auto& point: far_ranges.points)
1231 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
1233 int floor_x =
static_cast<int> (pcl_lrint (std::floor (x_real))),
1234 floor_y = static_cast<int> (pcl_lrint (std::floor (y_real))),
1235 ceil_x = static_cast<int> (pcl_lrint (std::ceil (x_real))),
1236 ceil_y = static_cast<int> (pcl_lrint (std::ceil (y_real)));
1238 int neighbor_x[4], neighbor_y[4];
1239 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1240 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1241 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1242 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1244 for (
int i=0; i<4; ++i)
1246 int x=neighbor_x[i], y=neighbor_y[i];
1250 if (!std::isfinite (image_point.
range))
1251 image_point.
range = std::numeric_limits<float>::infinity ();
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.
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...
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...
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...
std::vector< PointWithRange, Eigen::aligned_allocator< PointWithRange > > points
The point data.
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.
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.
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...
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...
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 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.
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.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
static const int lookup_table_size
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
void doPCA(VectorType &eigen_values, VectorType &eigen_vector1, VectorType &eigen_vector2, VectorType &eigen_vector3) const
Do Principal component analysis.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
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.
bool isInImage(int x, int y) const
Check if a point is inside of the image.
unsigned int getNoOfSamples()
Get the number of added vectors.
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...
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 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...
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.
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
static float atan2LookUp(float y, float x)
Query the std::atan2 lookup table.
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
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
const VectorType & getMean() const
Get the mean of the added vectors.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - 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 angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performance of multiplication compared to division ...
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
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...
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...
static float cosLookUp(float value)
Query the cos lookup table.
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
static float asinLookUp(float value)
Query the asin lookup table.
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...
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
Calculates the weighted average and the covariance matrix.
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
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.
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.