39 #ifndef PCL_RANGE_IMAGE_IMPL_HPP_
40 #define PCL_RANGE_IMAGE_IMPL_HPP_
65 if (std::abs (x) < std::abs (y))
71 ret =
static_cast<float> (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret);
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)) * std::abs (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)
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 using PointType2 =
typename PointCloudType::PointType;
236 int* counters =
new int[
size];
241 float x_real, y_real, range_of_current_point;
243 for (
const auto& point: points2)
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 (std::floor (x_real)), floor_y =
pcl_lrint (std::floor (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 = (std::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 (std::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 (std::isinf (image_point_range))
415 if (image_point_range > 0.0f)
416 return std::numeric_limits<float>::infinity ();
417 return -std::numeric_limits<float>::infinity ();
419 return image_point_range - range;
442 return (x >= 0 && x <
static_cast<int> (
width) && y >= 0 && y <
static_cast<int> (
height));
456 return std::isfinite (
getPoint (index).range);
471 return std::isinf (range) && range>0.0f;
535 point =
getPoint (image_x, image_y).getVector3fMap ();
542 point =
getPoint (index).getVector3fMap ();
546 const Eigen::Map<const Eigen::Vector3f>
549 return getPoint (x, y).getVector3fMap ();
553 const Eigen::Map<const Eigen::Vector3f>
556 return getPoint (index).getVector3fMap ();
563 float angle_x, angle_y;
567 float cosY = std::cos (angle_y);
568 point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * std::cos (angle_x)*cosY);
584 Eigen::Vector3f tmp_point;
586 point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
602 float cos_angle_y = std::cos (angle_y);
611 return -std::numeric_limits<float>::infinity ();
618 if ( (std::isinf (point1.
range)&&point1.
range<0) || (std::isinf (point2.
range)&&point2.
range<0))
619 return -std::numeric_limits<float>::infinity ();
621 float r1 = (std::min) (point1.
range, point2.
range),
623 float impact_angle =
static_cast<float> (0.5f * M_PI);
627 if (r2 > 0.0f && !std::isinf (r1))
630 else if (!std::isinf (r1))
635 d = std::sqrt (dSqr);
636 float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
637 cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
638 impact_angle = std::acos (cos_impact_angle);
642 impact_angle = -impact_angle;
652 if (std::isinf (impact_angle))
653 return -std::numeric_limits<float>::infinity ();
654 float ret = 1.0f - float (std::fabs (impact_angle)/ (0.5f*M_PI));
655 if (impact_angle < 0.0f)
667 return -std::numeric_limits<float>::infinity ();
672 const Eigen::Vector3f
682 angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
685 Eigen::Vector3f point;
691 Eigen::Vector3f transformed_left;
693 transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
696 Eigen::Vector3f left;
698 transformed_left = - (transformation * left);
700 transformed_left[1] = 0.0f;
701 transformed_left.normalize ();
704 Eigen::Vector3f transformed_right;
706 transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
709 Eigen::Vector3f right;
711 transformed_right = transformation * right;
713 transformed_right[1] = 0.0f;
714 transformed_right.normalize ();
716 angle_change_x = transformed_left.dot (transformed_right);
717 angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
718 angle_change_x = std::acos (angle_change_x);
723 Eigen::Vector3f transformed_top;
725 transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
730 transformed_top = - (transformation * top);
732 transformed_top[0] = 0.0f;
733 transformed_top.normalize ();
736 Eigen::Vector3f transformed_bottom;
738 transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
741 Eigen::Vector3f bottom;
743 transformed_bottom = transformation * bottom;
745 transformed_bottom[0] = 0.0f;
746 transformed_bottom.normalize ();
748 angle_change_y = transformed_top.dot (transformed_bottom);
749 angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
750 angle_change_y = std::acos (angle_change_y);
787 return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
794 return Eigen::Vector3f (point.x, point.y, point.z);
803 float weight_sum = 1.0f;
805 if (std::isinf (average_point.
range))
807 if (average_point.
range>0.0f)
810 average_point.x = average_point.y = average_point.z = average_point.
range = 0.0f;
814 Vector4fMap average_point_eigen = average_point.getVector4fMap ();
816 for (
int step=1; step<no_of_points; ++step)
819 x2+=delta_x; y2+=delta_y;
823 average_point_eigen+=p.getVector4fMap (); average_point.
range+=p.
range;
826 if (weight_sum<= 0.0f)
831 float normalization_factor = 1.0f/weight_sum;
832 average_point_eigen *= normalization_factor;
833 average_point.
range *= normalization_factor;
842 return -std::numeric_limits<float>::infinity ();
845 if (std::isinf (point1.
range) && std::isinf (point2.range))
847 if (std::isinf (point1.
range) || std::isinf (point2.range))
848 return std::numeric_limits<float>::infinity ();
856 float average_pixel_distance = 0.0f;
858 for (
int i=0; i<max_steps; ++i)
860 int x1=x+i*offset_x, y1=y+i*offset_y;
861 int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y;
863 if (!std::isfinite (pixel_distance))
867 return pixel_distance;
872 average_pixel_distance += std::sqrt (pixel_distance);
874 average_pixel_distance /= weight;
876 return average_pixel_distance;
884 return -std::numeric_limits<float>::infinity ();
886 int no_of_nearest_neighbors =
static_cast<int> (pow (
static_cast<double> ( (radius + 1.0)), 2.0));
887 Eigen::Vector3f normal;
889 return -std::numeric_limits<float>::infinity ();
899 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
901 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
906 if (!std::isfinite (point.
range))
908 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
913 Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
914 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
925 if (std::isinf (impact_angle))
926 return -std::numeric_limits<float>::infinity ();
927 float ret = 1.0f -
static_cast<float> ( (impact_angle / (0.5f * M_PI)));
935 int no_of_nearest_neighbors, Eigen::Vector3f& normal,
int step_size)
const
937 return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal,
nullptr, step_size);
946 int no_of_nearest_neighbors =
static_cast<int> (pow (
static_cast<double> (radius + 1.0), 2.0));
952 struct NeighborWithDistance
956 bool operator < (
const NeighborWithDistance& other)
const {
return distance<other.distance;}
963 float& max_closest_neighbor_distance_squared,
964 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
965 Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
966 Eigen::Vector3f* eigen_values_all_neighbors)
const
968 max_closest_neighbor_distance_squared=0.0f;
969 normal.setZero (); mean.setZero (); eigen_values.setZero ();
970 if (normal_all_neighbors!=
nullptr)
971 normal_all_neighbors->setZero ();
972 if (mean_all_neighbors!=
nullptr)
973 mean_all_neighbors->setZero ();
974 if (eigen_values_all_neighbors!=
nullptr)
975 eigen_values_all_neighbors->setZero ();
977 const auto sqrt_blocksize = 2 * radius + 1;
978 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
981 given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
983 std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
984 int neighbor_counter = 0;
985 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
987 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
991 NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
992 neighbor_with_distance.neighbor = &
getPoint (x2, y2);
997 no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
999 std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter);
1003 max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1005 float max_distance_squared = max_closest_neighbor_distance_squared*4.0f;
1011 for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1013 if (ordered_neighbors[neighbor_idx].
distance > max_distance_squared)
1016 vector_average.
add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1022 Eigen::Vector3f eigen_vector2, eigen_vector3;
1023 vector_average.
doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1024 Eigen::Vector3f viewing_direction = (
getSensorPos ()-point).normalized ();
1025 if (normal.dot (viewing_direction) < 0.0f)
1027 mean = vector_average.
getMean ();
1029 if (normal_all_neighbors==
nullptr)
1033 for (
int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1034 vector_average.
add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1036 vector_average.
doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1038 if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1039 *normal_all_neighbors *= -1.0f;
1040 *mean_all_neighbors = vector_average.
getMean ();
1052 if (!std::isfinite (point.
range))
1053 return -std::numeric_limits<float>::infinity ();
1055 const auto sqrt_blocksize = 2 * radius + 1;
1056 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
1057 std::vector<float> neighbor_distances (blocksize);
1058 int neighbor_counter = 0;
1059 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1061 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1063 if (!
isValid (x2, y2) || (x2==x&&y2==y))
1066 float& neighbor_distance = neighbor_distances[neighbor_counter++];
1070 std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter);
1072 n = (std::min) (neighbor_counter, n);
1073 return neighbor_distances[n-1];
1080 Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane,
int step_size)
const
1082 Eigen::Vector3f mean, eigen_values;
1083 float used_squared_max_distance;
1084 bool ret =
getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1085 normal, mean, eigen_values);
1089 if (point_on_plane !=
nullptr)
1090 *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1101 for (
int y2=y-radius; y2<=y+radius; y2+=step_size)
1103 for (
int x2=x-radius; x2<=x+radius; x2+=step_size)
1108 if (!std::isfinite (point.
range))
1110 vector_average.
add (Eigen::Vector3f (point.x, point.y, point.z));
1115 Eigen::Vector3f eigen_values;
1116 vector_average.
doPCA (eigen_values);
1117 return eigen_values[0]/eigen_values.sum ();
1122 template <
typename Po
intCloudTypeWithViewpo
ints> Eigen::Vector3f
1125 Eigen::Vector3f average_viewpoint (0,0,0);
1126 int point_counter = 0;
1127 for (
const auto& point: point_cloud.points)
1129 if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1131 average_viewpoint[0] += point.vp_x;
1132 average_viewpoint[1] += point.vp_y;
1133 average_viewpoint[2] += point.vp_z;
1136 average_viewpoint /= point_counter;
1138 return average_viewpoint;
1155 viewing_direction = (point-
getSensorPos ()).normalized ();
1162 Eigen::Affine3f transformation;
1164 return transformation;
1171 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1179 Eigen::Vector3f viewing_direction = (point-
getSensorPos ()).normalized ();
1218 template <
typename Po
intCloudType>
void
1221 float x_real, y_real, range_of_current_point;
1222 for (
const auto& point: far_ranges.points)
1228 this->
getImagePoint (current_point, x_real, y_real, range_of_current_point);
1230 int floor_x =
static_cast<int> (
pcl_lrint (std::floor (x_real))),
1231 floor_y =
static_cast<int> (
pcl_lrint (std::floor (y_real))),
1232 ceil_x =
static_cast<int> (
pcl_lrint (std::ceil (x_real))),
1233 ceil_y =
static_cast<int> (
pcl_lrint (std::ceil (y_real)));
1235 int neighbor_x[4], neighbor_y[4];
1236 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1237 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1238 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1239 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1241 for (
int i=0; i<4; ++i)
1243 int x=neighbor_x[i], y=neighbor_y[i];
1247 if (!std::isfinite (image_point.
range))
1248 image_point.
range = std::numeric_limits<float>::infinity ();