39 #ifndef PCL_RANGE_IMAGE_IMPL_HPP_ 40 #define PCL_RANGE_IMAGE_IMPL_HPP_ 42 #include <pcl/pcl_macros.h> 54 static_cast<float> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1) / 2.0f) * value)) +
65 if (fabsf (x) < fabsf (y))
69 static_cast<float> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1) / 2.0f) * (x / y))) +
71 ret =
static_cast<float> (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret);
76 static_cast<float> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1) / 2.0f) * (y / x))) +
79 ret =
static_cast<float> (y < 0 ? ret-M_PI : ret+M_PI);
88 int cell_idx =
static_cast<int> (pcl_lrintf ( (static_cast<float> (
lookup_table_size-1)) * fabsf (value) / (2.0f * static_cast<float> (M_PI))));
93 template <
typename Po
intCloudType>
void 95 float max_angle_width,
float max_angle_height,
97 float noise_level,
float min_range,
int border_size)
99 createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height,
100 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
104 template <
typename Po
intCloudType>
void 106 float angular_resolution_x,
float angular_resolution_y,
107 float max_angle_width,
float max_angle_height,
109 float noise_level,
float min_range,
int border_size)
116 int full_width =
static_cast<int> (pcl_lrint (floor (
pcl::deg2rad (360.0f)*angular_resolution_x_reciprocal_))),
117 full_height = static_cast<int> (pcl_lrint (floor (
pcl::deg2rad (180.0f)*angular_resolution_y_reciprocal_)));
133 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
135 cropImage (border_size, top, right, bottom, left);
141 template <
typename Po
intCloudType>
void 143 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
145 float noise_level,
float min_range,
int border_size)
148 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
152 template <
typename Po
intCloudType>
void 154 float angular_resolution_x,
float angular_resolution_y,
155 const Eigen::Vector3f& point_cloud_center,
float point_cloud_radius,
157 float noise_level,
float min_range,
int border_size)
164 if ((point_cloud_center-sensor_pose.translation()).norm() <= point_cloud_radius) {
167 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
177 float max_angle_size =
getMaxAngleSize (sensor_pose, point_cloud_center, point_cloud_radius);
180 width = 2*pixel_radius_x;
181 height = 2*pixel_radius_y;
185 int center_pixel_x, center_pixel_y;
186 getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y);
194 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
196 cropImage (border_size, top, right, bottom, left);
202 template <
typename Po
intCloudTypeWithViewpo
ints>
void 204 float angular_resolution,
205 float max_angle_width,
float max_angle_height,
207 float noise_level,
float min_range,
int border_size)
210 max_angle_width, max_angle_height, coordinate_frame,
211 noise_level, min_range, border_size);
215 template <
typename Po
intCloudTypeWithViewpo
ints>
void 217 float angular_resolution_x,
float angular_resolution_y,
218 float max_angle_width,
float max_angle_height,
220 float noise_level,
float min_range,
int border_size)
223 Eigen::Affine3f sensor_pose =
static_cast<Eigen::Affine3f
> (Eigen::Translation3f (average_viewpoint));
224 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height,
225 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
229 template <
typename Po
intCloudType>
void 230 RangeImage::doZBuffer (
const PointCloudType& point_cloud,
float noise_level,
float min_range,
int& top,
int& right,
int& bottom,
int& left)
232 typedef typename PointCloudType::PointType PointType2;
236 int* counters =
new int[
size];
237 ERASE_ARRAY (counters, size);
241 float x_real, y_real, range_of_current_point;
249 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
252 if (range_of_current_point < min_range|| !
isInImage (x, y))
257 int floor_x = pcl_lrint (floor (x_real)), floor_y = pcl_lrint (floor (y_real)),
258 ceil_x = pcl_lrint (ceil (x_real)), ceil_y = pcl_lrint (ceil (y_real));
260 int neighbor_x[4], neighbor_y[4];
261 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
262 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
263 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
264 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
267 for (
int i=0; i<4; ++i)
269 int n_x=neighbor_x[i], n_y=neighbor_y[i];
271 if (n_x==x && n_y==y)
275 int neighbor_array_pos = n_y*
width + n_x;
276 if (counters[neighbor_array_pos]==0)
278 float& neighbor_range =
points[neighbor_array_pos].range;
279 neighbor_range = (pcl_isinf (neighbor_range) ? range_of_current_point : (std::min) (neighbor_range, range_of_current_point));
280 top= (std::min) (top, n_y); right= (std::max) (right, n_x); bottom= (std::max) (bottom, n_y); left= (std::min) (left, n_x);
287 int arrayPos = y*
width + x;
288 float& range_at_image_point =
points[arrayPos].range;
289 int& counter = counters[arrayPos];
290 bool addCurrentPoint=
false, replace_with_current_point=
false;
294 replace_with_current_point =
true;
298 if (range_of_current_point < range_at_image_point-noise_level)
300 replace_with_current_point =
true;
302 else if (fabs (range_of_current_point-range_at_image_point)<=noise_level)
304 addCurrentPoint =
true;
308 if (replace_with_current_point)
311 range_at_image_point = range_of_current_point;
312 top= (std::min) (top, y); right= (std::max) (right, x); bottom= (std::max) (bottom, y); left= (std::min) (left, x);
315 else if (addCurrentPoint)
318 range_at_image_point += (range_of_current_point-range_at_image_point)/counter;
329 Eigen::Vector3f point (x, y, z);
345 float image_x_float, image_y_float;
347 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
355 range = transformedPoint.norm ();
356 float angle_x =
atan2LookUp (transformedPoint[0], transformedPoint[2]),
357 angle_y =
asinLookUp (transformedPoint[1]/range);
367 float image_x_float, image_y_float;
369 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
384 float image_x_float, image_y_float;
386 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
393 int image_x, image_y;
399 point_in_image =
getPoint (image_x, image_y);
407 int image_x, image_y;
411 return -std::numeric_limits<float>::infinity ();
413 if (pcl_isinf (image_point_range))
415 if (image_point_range > 0.0f)
416 return std::numeric_limits<float>::infinity ();
418 return -std::numeric_limits<float>::infinity ();
420 return image_point_range - range;
435 xInt =
static_cast<int> (pcl_lrintf (x));
436 yInt =
static_cast<int> (pcl_lrintf (y));
443 return (x >= 0 && x < static_cast<int> (
width) && y >= 0 && y < static_cast<int> (
height));
457 return pcl_isfinite (
getPoint (index).range);
474 return pcl_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 = cosf (angle_y);
571 point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * cosf (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 = cosf (angle_y);
614 return -std::numeric_limits<float>::infinity ();
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 && !pcl_isinf (r1))
633 else if (!pcl_isinf (r1))
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 = acosf (cos_impact_angle);
645 impact_angle = -impact_angle;
655 if (pcl_isinf (impact_angle))
656 return -std::numeric_limits<float>::infinity ();
657 float ret = 1.0f - float (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 = acosf (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 = acosf (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 (pcl_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 (pcl_isinf (point1.
range) && pcl_isinf (point2.range))
850 if (pcl_isinf (point1.
range) || pcl_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 (!pcl_isfinite (pixel_distance))
870 return pixel_distance;
876 average_pixel_distance += sqrtf (pixel_distance);
878 average_pixel_distance /= weight;
880 return average_pixel_distance;
888 return -std::numeric_limits<float>::infinity ();
890 int no_of_nearest_neighbors =
static_cast<int> (pow (static_cast<double> ( (radius + 1.0)), 2.0));
891 Eigen::Vector3f normal;
893 return -std::numeric_limits<float>::infinity ();
903 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
905 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
910 if (!pcl_isfinite (point.
range))
912 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
917 Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
918 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
929 if (pcl_isinf (impact_angle))
930 return -std::numeric_limits<float>::infinity ();
931 float ret = 1.0f -
static_cast<float> ( (impact_angle / (0.5f * M_PI)));
939 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size)
const 941 return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal, NULL, step_size);
950 int no_of_nearest_neighbors =
static_cast<int> (pow (static_cast<double> (radius + 1.0), 2.0));
956 struct NeighborWithDistance
960 bool operator < (
const NeighborWithDistance& other)
const {
return distance<other.distance;}
967 float& max_closest_neighbor_distance_squared,
968 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
969 Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
970 Eigen::Vector3f* eigen_values_all_neighbors)
const 972 max_closest_neighbor_distance_squared=0.0f;
973 normal.setZero (); mean.setZero (); eigen_values.setZero ();
974 if (normal_all_neighbors!=NULL)
975 normal_all_neighbors->setZero ();
976 if (mean_all_neighbors!=NULL)
977 mean_all_neighbors->setZero ();
978 if (eigen_values_all_neighbors!=NULL)
979 eigen_values_all_neighbors->setZero ();
981 int blocksize =
static_cast<int> (pow (static_cast<double> ( (2.0 * radius + 1.0)), 2.0));
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==NULL)
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 (!pcl_isfinite (point.
range))
1056 return -std::numeric_limits<float>::infinity ();
1058 int blocksize =
static_cast<int> (pow (static_cast<double> (2.0 * radius + 1.0), 2.0));
1059 std::vector<float> neighbor_distances (blocksize);
1060 int neighbor_counter = 0;
1061 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1063 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1065 if (!
isValid (x2, y2) || (x2==x&&y2==y))
1068 float& neighbor_distance = neighbor_distances[neighbor_counter++];
1072 std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter);
1074 n = (std::min) (neighbor_counter, n);
1075 return neighbor_distances[n-1];
1082 Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane,
int step_size)
const 1084 Eigen::Vector3f mean, eigen_values;
1085 float used_squared_max_distance;
1086 bool ret =
getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1087 normal, mean, eigen_values);
1091 if (point_on_plane != NULL)
1092 *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1103 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1105 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1110 if (!pcl_isfinite (point.
range))
1112 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
1117 Eigen::Vector3f eigen_values;
1118 vector_average.
doPCA (eigen_values);
1119 return eigen_values[0]/eigen_values.sum ();
1124 template <
typename Po
intCloudTypeWithViewpo
ints> Eigen::Vector3f
1127 Eigen::Vector3f average_viewpoint (0,0,0);
1128 int point_counter = 0;
1129 for (
unsigned int point_idx=0; point_idx<point_cloud.points.size (); ++point_idx)
1131 const typename PointCloudTypeWithViewpoints::PointType& point = point_cloud.points[point_idx];
1132 if (!pcl_isfinite (point.vp_x) || !pcl_isfinite (point.vp_y) || !pcl_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 (
typename PointCloudType::const_iterator it = far_ranges.points.begin (); it != far_ranges.points.end (); ++it)
1231 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
1233 int floor_x =
static_cast<int> (pcl_lrint (floor (x_real))),
1234 floor_y = static_cast<int> (pcl_lrint (floor (y_real))),
1235 ceil_x = static_cast<int> (pcl_lrint (ceil (x_real))),
1236 ceil_y = static_cast<int> (pcl_lrint (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 (!pcl_isfinite (image_point.
range))
1251 image_point.
range = std::numeric_limits<float>::infinity ();
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
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...
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=NULL, Eigen::Vector3f *mean_all_neighbors=NULL, Eigen::Vector3f *eigen_values_all_neighbors=NULL) const
Same as above but extracts some more data and can also return the extracted information for all neigh...
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 getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
A point structure representing Euclidean xyz coordinates, padded with an extra range float...
std::vector< PointWithRange, Eigen::aligned_allocator< PointWithRange > > points
The point data.
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 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...
int image_offset_y_
Position of the top left corner of the range image compared to an image of full size (360x180 degrees...
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 getTransformationFromTwoUnitVectorsAndOrigin(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, const Eigen::Vector3f &origin, Eigen::Affine3f &transformation)
Get the transformation that will translate orign to (0,0,0) and rotate z_axis into (0...
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.
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
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...
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=...
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 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 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.
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.
bool isInImage(int x, int y) const
Check if a point is inside of the image.
Define standard C methods to do distance calculations.
void add(const Eigen::Matrix< real, dimension, 1 > &sample, real weight=1.0)
Add a new sample.
uint32_t height
The point cloud height (if organized as an image-structure).
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
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...
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.
uint32_t width
The point cloud width (if organized as an image-structure).
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.
unsigned int getNoOfSamples()
Get the number of added vectors.
const Eigen::Matrix< real, dimension, 1 > & getMean() const
Get the mean of the added vectors.
void doPCA(Eigen::Matrix< real, dimension, 1 > &eigen_values, Eigen::Matrix< real, dimension, 1 > &eigen_vector1, Eigen::Matrix< real, dimension, 1 > &eigen_vector2, Eigen::Matrix< real, dimension, 1 > &eigen_vector3) const
Do Principal component analysis.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
static std::vector< float > cos_lookup_table
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
float distance(const PointT &p1, const PointT &p2)
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.
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
static float atan2LookUp(float y, float x)
Query the atan2 lookup table.
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.
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.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performace of multiplication compared to division ...
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performace of multiplication compared to division ...
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 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...
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! ...
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
void getAnglesFromImagePoint(float image_x, float image_y, float &angle_x, float &angle_y) const
Get the angles corresponding to the given image point.
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).
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
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.
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...
static float cosLookUp(float value)
Query the cos lookup table.
static float asinLookUp(float value)
Query the asin lookup table.
float getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
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 angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
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...
Calculates the weighted average and the covariance matrix.
static std::vector< float > atan_lookup_table
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
PCL_EXPORTS void 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...
Eigen::Affine3f getTransformationToViewerCoordinateFrame(const Eigen::Vector3f &point) const
Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction.
void integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.
VectorType::const_iterator const_iterator