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))
74 ret =
static_cast<float> (x*y > 0 ?
M_PI/2-ret : -
M_PI/2-ret);
82 ret =
static_cast<float> (y < 0 ? ret-
M_PI : ret+
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)
136 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
138 if (border_size != std::numeric_limits<int>::min()) {
139 cropImage (border_size, top, right, bottom, left);
146 template <
typename Po
intCloudType>
void
148 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
150 float noise_level,
float min_range,
int border_size)
153 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
157 template <
typename Po
intCloudType>
void
159 float angular_resolution_x,
float angular_resolution_y,
160 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
162 float noise_level,
float min_range,
int border_size)
169 if ((point_cloud_center-sensor_pose.translation()).norm() <= point_cloud_radius) {
172 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
182 float max_angle_size =
getMaxAngleSize (sensor_pose, point_cloud_center, point_cloud_radius);
185 width = 2*pixel_radius_x;
186 height = 2*pixel_radius_y;
190 int center_pixel_x, center_pixel_y;
191 getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y);
199 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
201 if (border_size != std::numeric_limits<int>::min()) {
202 cropImage (border_size, top, right, bottom, left);
209 template <
typename Po
intCloudTypeWithViewpo
ints>
void
211 float angular_resolution,
212 float max_angle_width,
float max_angle_height,
214 float noise_level,
float min_range,
int border_size)
217 max_angle_width, max_angle_height, coordinate_frame,
218 noise_level, min_range, border_size);
222 template <
typename Po
intCloudTypeWithViewpo
ints>
void
224 float angular_resolution_x,
float angular_resolution_y,
225 float max_angle_width,
float max_angle_height,
227 float noise_level,
float min_range,
int border_size)
230 Eigen::Affine3f sensor_pose =
static_cast<Eigen::Affine3f
> (Eigen::Translation3f (average_viewpoint));
231 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height,
232 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
236 template <
typename Po
intCloudType>
void
237 RangeImage::doZBuffer (
const PointCloudType& point_cloud,
float noise_level,
float min_range,
int& top,
int& right,
int& bottom,
int& left)
239 using PointType2 =
typename PointCloudType::PointType;
243 int* counters =
new int[
size];
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;
336 Eigen::Vector3f point (x, y, z);
352 float image_x_float, image_y_float;
354 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
362 range = transformedPoint.norm ();
363 if (range < std::numeric_limits<float>::epsilon()) {
364 PCL_DEBUG (
"[pcl::RangeImage::getImagePoint] Transformed point is (0,0,0), cannot project it.\n");
365 image_x = image_y = 0.0f;
368 float angle_x =
atan2LookUp (transformedPoint[0], transformedPoint[2]),
369 angle_y =
asinLookUp (transformedPoint[1]/range);
379 float image_x_float, image_y_float;
381 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
396 float image_x_float, image_y_float;
398 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
405 int image_x, image_y;
411 point_in_image =
getPoint (image_x, image_y);
419 int image_x, image_y;
423 return -std::numeric_limits<float>::infinity ();
425 if (std::isinf (image_point_range))
427 if (image_point_range > 0.0f)
428 return std::numeric_limits<float>::infinity ();
429 return -std::numeric_limits<float>::infinity ();
431 return image_point_range - range;
454 return (x >= 0 && x <
static_cast<int> (
width) && y >= 0 && y <
static_cast<int> (
height));
468 return std::isfinite (
getPoint (index).range);
483 return std::isinf (range) && range>0.0f;
547 point =
getPoint (image_x, image_y).getVector3fMap ();
554 point =
getPoint (index).getVector3fMap ();
558 const Eigen::Map<const Eigen::Vector3f>
561 return getPoint (x, y).getVector3fMap ();
565 const Eigen::Map<const Eigen::Vector3f>
568 return getPoint (index).getVector3fMap ();
575 float angle_x, angle_y;
579 float cosY = std::cos (angle_y);
580 point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * std::cos (angle_x)*cosY);
596 Eigen::Vector3f tmp_point;
598 point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
614 float cos_angle_y = std::cos (angle_y);
623 return -std::numeric_limits<float>::infinity ();
630 if ( (std::isinf (point1.
range)&&point1.
range<0) || (std::isinf (point2.
range)&&point2.
range<0))
631 return -std::numeric_limits<float>::infinity ();
633 float r1 = (std::min) (point1.
range, point2.
range),
635 float impact_angle =
static_cast<float> (0.5f *
M_PI);
639 if (r2 > 0.0f && !std::isinf (r1))
642 else if (!std::isinf (r1))
647 d = std::sqrt (dSqr);
648 float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
649 cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
650 impact_angle = std::acos (cos_impact_angle);
654 impact_angle = -impact_angle;
664 if (std::isinf (impact_angle))
665 return -std::numeric_limits<float>::infinity ();
666 float ret = 1.0f - float (std::fabs (impact_angle)/ (0.5f*
M_PI));
667 if (impact_angle < 0.0f)
679 return -std::numeric_limits<float>::infinity ();
684 const Eigen::Vector3f
694 angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
697 Eigen::Vector3f point;
703 Eigen::Vector3f transformed_left;
705 transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
708 Eigen::Vector3f left;
710 transformed_left = - (transformation * left);
712 transformed_left[1] = 0.0f;
713 transformed_left.normalize ();
716 Eigen::Vector3f transformed_right;
718 transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
721 Eigen::Vector3f right;
723 transformed_right = transformation * right;
725 transformed_right[1] = 0.0f;
726 transformed_right.normalize ();
728 angle_change_x = transformed_left.dot (transformed_right);
729 angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
730 angle_change_x = std::acos (angle_change_x);
735 Eigen::Vector3f transformed_top;
737 transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
742 transformed_top = - (transformation * top);
744 transformed_top[0] = 0.0f;
745 transformed_top.normalize ();
748 Eigen::Vector3f transformed_bottom;
750 transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
753 Eigen::Vector3f bottom;
755 transformed_bottom = transformation * bottom;
757 transformed_bottom[0] = 0.0f;
758 transformed_bottom.normalize ();
760 angle_change_y = transformed_top.dot (transformed_bottom);
761 angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
762 angle_change_y = std::acos (angle_change_y);
799 return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
806 return Eigen::Vector3f (point.x, point.y, point.z);
815 float weight_sum = 1.0f;
817 if (std::isinf (average_point.
range))
819 if (average_point.
range>0.0f)
822 average_point.x = average_point.y = average_point.z = average_point.
range = 0.0f;
826 Vector4fMap average_point_eigen = average_point.getVector4fMap ();
828 for (
int step=1; step<no_of_points; ++step)
831 x2+=delta_x; y2+=delta_y;
835 average_point_eigen+=p.getVector4fMap (); average_point.
range+=p.
range;
838 if (weight_sum<= 0.0f)
843 float normalization_factor = 1.0f/weight_sum;
844 average_point_eigen *= normalization_factor;
845 average_point.
range *= normalization_factor;
854 return -std::numeric_limits<float>::infinity ();
857 if (std::isinf (point1.
range) && std::isinf (point2.range))
859 if (std::isinf (point1.
range) || std::isinf (point2.range))
860 return std::numeric_limits<float>::infinity ();
868 float average_pixel_distance = 0.0f;
870 for (
int i=0; i<max_steps; ++i)
872 int x1=x+i*offset_x, y1=y+i*offset_y;
873 int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y;
875 if (!std::isfinite (pixel_distance))
879 return pixel_distance;
884 average_pixel_distance += std::sqrt (pixel_distance);
886 average_pixel_distance /= weight;
888 return average_pixel_distance;
896 return -std::numeric_limits<float>::infinity ();
898 int no_of_nearest_neighbors =
static_cast<int> (pow (
static_cast<double> ( (radius + 1.0)), 2.0));
899 Eigen::Vector3f normal;
901 return -std::numeric_limits<float>::infinity ();
911 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
913 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
918 if (!std::isfinite (point.
range))
920 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
925 Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
926 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
937 if (std::isinf (impact_angle))
938 return -std::numeric_limits<float>::infinity ();
939 float ret = 1.0f -
static_cast<float> ( (impact_angle / (0.5f *
M_PI)));
947 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size)
const
949 return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal,
nullptr, step_size);
958 int no_of_nearest_neighbors =
static_cast<int> (pow (
static_cast<double> (radius + 1.0), 2.0));
964 struct NeighborWithDistance
968 bool operator < (
const NeighborWithDistance& other)
const {
return distance<other.distance;}
975 float& max_closest_neighbor_distance_squared,
976 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
977 Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
978 Eigen::Vector3f* eigen_values_all_neighbors)
const
980 max_closest_neighbor_distance_squared=0.0f;
981 normal.setZero (); mean.setZero (); eigen_values.setZero ();
982 if (normal_all_neighbors!=
nullptr)
983 normal_all_neighbors->setZero ();
984 if (mean_all_neighbors!=
nullptr)
985 mean_all_neighbors->setZero ();
986 if (eigen_values_all_neighbors!=
nullptr)
987 eigen_values_all_neighbors->setZero ();
989 const auto sqrt_blocksize = 2 * radius + 1;
990 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
993 given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
995 std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
996 int neighbor_counter = 0;
997 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
999 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1003 NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
1004 neighbor_with_distance.neighbor = &
getPoint (x2, y2);
1009 no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
1011 std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter);
1015 max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1017 float max_distance_squared = max_closest_neighbor_distance_squared*4.0f;
1023 for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1025 if (ordered_neighbors[neighbor_idx].
distance > max_distance_squared)
1028 vector_average.
add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1034 Eigen::Vector3f eigen_vector2, eigen_vector3;
1035 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1036 Eigen::Vector3f viewing_direction = (
getSensorPos ()-point).normalized ();
1037 if (normal.dot (viewing_direction) < 0.0f)
1039 mean = vector_average.
getMean ();
1041 if (normal_all_neighbors==
nullptr)
1045 for (
int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1046 vector_average.
add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1048 vector_average.
doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1050 if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1051 *normal_all_neighbors *= -1.0f;
1052 *mean_all_neighbors = vector_average.
getMean ();
1064 if (!std::isfinite (point.
range))
1065 return -std::numeric_limits<float>::infinity ();
1067 const auto sqrt_blocksize = 2 * radius + 1;
1068 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
1069 std::vector<float> neighbor_distances (blocksize);
1070 int neighbor_counter = 0;
1071 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1073 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1075 if (!
isValid (x2, y2) || (x2==x&&y2==y))
1078 float& neighbor_distance = neighbor_distances[neighbor_counter++];
1082 std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter);
1084 n = (std::min) (neighbor_counter, n);
1085 return neighbor_distances[n-1];
1092 Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane,
int step_size)
const
1094 Eigen::Vector3f mean, eigen_values;
1095 float used_squared_max_distance;
1096 bool ret =
getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1097 normal, mean, eigen_values);
1101 if (point_on_plane !=
nullptr)
1102 *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1113 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1115 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1120 if (!std::isfinite (point.
range))
1122 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
1127 Eigen::Vector3f eigen_values;
1128 vector_average.
doPCA (eigen_values);
1129 return eigen_values[0]/eigen_values.sum ();
1134 template <
typename Po
intCloudTypeWithViewpo
ints> Eigen::Vector3f
1137 Eigen::Vector3f average_viewpoint (0,0,0);
1138 int point_counter = 0;
1139 for (
const auto& point: point_cloud.points)
1141 if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1143 average_viewpoint[0] += point.vp_x;
1144 average_viewpoint[1] += point.vp_y;
1145 average_viewpoint[2] += point.vp_z;
1148 average_viewpoint /= point_counter;
1150 return average_viewpoint;
1167 viewing_direction = (point-
getSensorPos ()).normalized ();
1174 Eigen::Affine3f transformation;
1176 return transformation;
1183 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1191 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1230 template <
typename Po
intCloudType>
void
1233 float x_real, y_real, range_of_current_point;
1234 for (
const auto& point: far_ranges.points)
1240 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
1242 int floor_x =
static_cast<int> (
pcl_lrint (std::floor (x_real))),
1243 floor_y =
static_cast<int> (
pcl_lrint (std::floor (y_real))),
1244 ceil_x =
static_cast<int> (
pcl_lrint (std::ceil (x_real))),
1245 ceil_y =
static_cast<int> (
pcl_lrint (std::ceil (y_real)));
1247 int neighbor_x[4], neighbor_y[4];
1248 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1249 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1250 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1251 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1253 for (
int i=0; i<4; ++i)
1255 int x=neighbor_x[i], y=neighbor_y[i];
1259 if (!std::isfinite (image_point.
range))
1260 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.
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.
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.
#define ERASE_ARRAY(var, size)
A point structure representing Euclidean xyz coordinates, padded with an extra range float.