Point Cloud Library (PCL)  1.14.1
range_image.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #pragma once
40 
41 #include <pcl/range_image/range_image.h>
42 
43 #include <pcl/pcl_macros.h>
44 #include <pcl/common/distances.h>
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 #include <pcl/common/vector_average.h> // for VectorAverage3f
47 #include <vector>
48 
49 namespace pcl
50 {
51 
52 /////////////////////////////////////////////////////////////////////////
53 inline float
55 {
56  return (asin_lookup_table[
57  static_cast<int> (
58  static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * value)) +
59  static_cast<float> (lookup_table_size-1) / 2.0f)]);
60 }
61 
62 /////////////////////////////////////////////////////////////////////////
63 inline float
64 RangeImage::atan2LookUp (float y, float x)
65 {
66  if (x==0 && y==0)
67  return 0;
68  float ret;
69  if (std::abs (x) < std::abs (y))
70  {
71  ret = atan_lookup_table[
72  static_cast<int> (
73  static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (x / y))) +
74  static_cast<float> (lookup_table_size-1) / 2.0f)];
75  ret = static_cast<float> (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret);
76  }
77  else
78  ret = atan_lookup_table[
79  static_cast<int> (
80  static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (y / x))) +
81  static_cast<float> (lookup_table_size-1)/2.0f)];
82  if (x < 0)
83  ret = static_cast<float> (y < 0 ? ret-M_PI : ret+M_PI);
84 
85  return (ret);
86 }
87 
88 /////////////////////////////////////////////////////////////////////////
89 inline float
90 RangeImage::cosLookUp (float value)
91 {
92  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  return (cos_lookup_table[cell_idx]);
94 }
95 
96 /////////////////////////////////////////////////////////////////////////
97 template <typename PointCloudType> void
98 RangeImage::createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution,
99  float max_angle_width, float max_angle_height,
100  const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
101  float noise_level, float min_range, int border_size)
102 {
103  createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height,
104  sensor_pose, coordinate_frame, noise_level, min_range, border_size);
105 }
106 
107 /////////////////////////////////////////////////////////////////////////
108 template <typename PointCloudType> void
109 RangeImage::createFromPointCloud (const PointCloudType& point_cloud,
110  float angular_resolution_x, float angular_resolution_y,
111  float max_angle_width, float max_angle_height,
112  const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
113  float noise_level, float min_range, int border_size)
114 {
115  setAngularResolution (angular_resolution_x, angular_resolution_y);
116 
117  width = static_cast<std::uint32_t> (pcl_lrint (std::floor (max_angle_width*angular_resolution_x_reciprocal_)));
118  height = static_cast<std::uint32_t> (pcl_lrint (std::floor (max_angle_height*angular_resolution_y_reciprocal_)));
119 
120  int full_width = static_cast<int> (pcl_lrint (std::floor (pcl::deg2rad (360.0f)*angular_resolution_x_reciprocal_))),
121  full_height = static_cast<int> (pcl_lrint (std::floor (pcl::deg2rad (180.0f)*angular_resolution_y_reciprocal_)));
122  image_offset_x_ = (full_width -static_cast<int> (width) )/2;
123  image_offset_y_ = (full_height-static_cast<int> (height))/2;
124  is_dense = false;
125 
127  to_world_system_ = sensor_pose * to_world_system_;
128 
129  to_range_image_system_ = to_world_system_.inverse (Eigen::Isometry);
130  //std::cout << "to_world_system_ is\n"<<to_world_system_<<"\nand to_range_image_system_ is\n"<<to_range_image_system_<<"\n\n";
131 
132  unsigned int size = width*height;
133  points.clear ();
134  points.resize (size, unobserved_point);
135 
136  int top=height, right=-1, bottom=-1, left=width;
137  doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
138 
139  if (border_size != std::numeric_limits<int>::min()) {
140  cropImage (border_size, top, right, bottom, left);
141 
143  }
144 }
145 
146 /////////////////////////////////////////////////////////////////////////
147 template <typename PointCloudType> void
148 RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution,
149  const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
150  const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
151  float noise_level, float min_range, int border_size)
152 {
153  createFromPointCloudWithKnownSize (point_cloud, angular_resolution, angular_resolution, point_cloud_center, point_cloud_radius,
154  sensor_pose, coordinate_frame, noise_level, min_range, border_size);
155 }
156 
157 /////////////////////////////////////////////////////////////////////////
158 template <typename PointCloudType> void
159 RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud,
160  float angular_resolution_x, float angular_resolution_y,
161  const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
162  const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
163  float noise_level, float min_range, int border_size)
164 {
165  //MEASURE_FUNCTION_TIME;
166 
167  //std::cout << "Starting to create range image from "<<point_cloud.size ()<<" points.\n";
168 
169  // If the sensor pose is inside of the sphere we have to calculate the image the normal way
170  if ((point_cloud_center-sensor_pose.translation()).norm() <= point_cloud_radius) {
171  createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
172  pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
173  sensor_pose, coordinate_frame, noise_level, min_range, border_size);
174  return;
175  }
176 
177  setAngularResolution (angular_resolution_x, angular_resolution_y);
178 
180  to_world_system_ = sensor_pose * to_world_system_;
181  to_range_image_system_ = to_world_system_.inverse (Eigen::Isometry);
182 
183  float max_angle_size = getMaxAngleSize (sensor_pose, point_cloud_center, point_cloud_radius);
184  int pixel_radius_x = pcl_lrint (std::ceil (0.5f*max_angle_size*angular_resolution_x_reciprocal_)),
185  pixel_radius_y = pcl_lrint (std::ceil (0.5f*max_angle_size*angular_resolution_y_reciprocal_));
186  width = 2*pixel_radius_x;
187  height = 2*pixel_radius_y;
188  is_dense = false;
189 
190  image_offset_x_ = image_offset_y_ = 0; // temporary values for getImagePoint
191  int center_pixel_x, center_pixel_y;
192  getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y);
193  image_offset_x_ = (std::max) (0, center_pixel_x-pixel_radius_x);
194  image_offset_y_ = (std::max) (0, center_pixel_y-pixel_radius_y);
195 
196  points.clear ();
198 
199  int top=height, right=-1, bottom=-1, left=width;
200  doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
201 
202  if (border_size != std::numeric_limits<int>::min()) {
203  cropImage (border_size, top, right, bottom, left);
204 
206  }
207 }
208 
209 /////////////////////////////////////////////////////////////////////////
210 template <typename PointCloudTypeWithViewpoints> void
211 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
212  float angular_resolution,
213  float max_angle_width, float max_angle_height,
214  RangeImage::CoordinateFrame coordinate_frame,
215  float noise_level, float min_range, int border_size)
216 {
217  createFromPointCloudWithViewpoints (point_cloud, angular_resolution, angular_resolution,
218  max_angle_width, max_angle_height, coordinate_frame,
219  noise_level, min_range, border_size);
220 }
221 
222 /////////////////////////////////////////////////////////////////////////
223 template <typename PointCloudTypeWithViewpoints> void
224 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
225  float angular_resolution_x, float angular_resolution_y,
226  float max_angle_width, float max_angle_height,
227  RangeImage::CoordinateFrame coordinate_frame,
228  float noise_level, float min_range, int border_size)
229 {
230  Eigen::Vector3f average_viewpoint = getAverageViewPoint (point_cloud);
231  Eigen::Affine3f sensor_pose = static_cast<Eigen::Affine3f> (Eigen::Translation3f (average_viewpoint));
232  createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height,
233  sensor_pose, coordinate_frame, noise_level, min_range, border_size);
234 }
235 
236 /////////////////////////////////////////////////////////////////////////
237 template <typename PointCloudType> void
238 RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, float min_range, int& top, int& right, int& bottom, int& left)
239 {
240  using PointType2 = typename PointCloudType::PointType;
241  const typename pcl::PointCloud<PointType2>::VectorType &points2 = point_cloud.points;
242 
243  unsigned int size = width*height;
244  std::vector<int> counters(size, 0);
245 
246  top=height; right=-1; bottom=-1; left=width;
247 
248  float x_real, y_real, range_of_current_point;
249  int x, y;
250  for (const auto& point: points2)
251  {
252  if (!isFinite (point)) // Check for NAN etc
253  continue;
254  Vector3fMapConst current_point = point.getVector3fMap ();
255 
256  this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
257  this->real2DToInt2D (x_real, y_real, x, y);
258 
259  if (range_of_current_point < min_range|| !isInImage (x, y))
260  continue;
261  //std::cout << " ("<<current_point[0]<<", "<<current_point[1]<<", "<<current_point[2]<<") falls into pixel "<<x<<","<<y<<".\n";
262 
263  // Do some minor interpolation by checking the three closest neighbors to the point, that are not filled yet.
264  int floor_x = pcl_lrint (std::floor (x_real)), floor_y = pcl_lrint (std::floor (y_real)),
265  ceil_x = pcl_lrint (std::ceil (x_real)), ceil_y = pcl_lrint (std::ceil (y_real));
266 
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;
272  //std::cout << x_real<<","<<y_real<<": ";
273 
274  for (int i=0; i<4; ++i)
275  {
276  int n_x=neighbor_x[i], n_y=neighbor_y[i];
277  //std::cout << n_x<<","<<n_y<<" ";
278  if (n_x==x && n_y==y)
279  continue;
280  if (isInImage (n_x, n_y))
281  {
282  int neighbor_array_pos = n_y*width + n_x;
283  if (counters[neighbor_array_pos] == 0)
284  {
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);
288  }
289  }
290  }
291  //std::cout <<std::endl;
292 
293  // The point itself
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;
298 
299  if (counter==0)
300  {
301  replace_with_current_point = true;
302  }
303  else
304  {
305  if (range_of_current_point < range_at_image_point-noise_level)
306  {
307  replace_with_current_point = true;
308  }
309  else if (std::fabs (range_of_current_point-range_at_image_point)<=noise_level)
310  {
311  addCurrentPoint = true;
312  }
313  }
314 
315  if (replace_with_current_point)
316  {
317  counter = 1;
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);
320  //std::cout << "Adding point "<<x<<","<<y<<"\n";
321  }
322  else if (addCurrentPoint)
323  {
324  ++counter;
325  range_at_image_point += (range_of_current_point-range_at_image_point)/counter;
326  }
327  }
328 }
329 
330 /////////////////////////////////////////////////////////////////////////
331 void
332 RangeImage::getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const
333 {
334  Eigen::Vector3f point (x, y, z);
335  getImagePoint (point, image_x, image_y, range);
336 }
337 
338 /////////////////////////////////////////////////////////////////////////
339 void
340 RangeImage::getImagePoint (float x, float y, float z, float& image_x, float& image_y) const
341 {
342  float range;
343  getImagePoint (x, y, z, image_x, image_y, range);
344 }
345 
346 /////////////////////////////////////////////////////////////////////////
347 void
348 RangeImage::getImagePoint (float x, float y, float z, int& image_x, int& image_y) const
349 {
350  float image_x_float, image_y_float;
351  getImagePoint (x, y, z, image_x_float, image_y_float);
352  real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
353 }
354 
355 /////////////////////////////////////////////////////////////////////////
356 void
357 RangeImage::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const
358 {
359  Eigen::Vector3f transformedPoint = to_range_image_system_ * point;
360  range = transformedPoint.norm ();
361  if (range < std::numeric_limits<float>::epsilon()) {
362  PCL_DEBUG ("[pcl::RangeImage::getImagePoint] Transformed point is (0,0,0), cannot project it.\n");
363  image_x = image_y = 0.0f;
364  return;
365  }
366  float angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]),
367  angle_y = asinLookUp (transformedPoint[1]/range);
368  getImagePointFromAngles (angle_x, angle_y, image_x, image_y);
369  //std::cout << " ("<<point[0]<<","<<point[1]<<","<<point[2]<<")"
370  //<< " => ("<<transformedPoint[0]<<","<<transformedPoint[1]<<","<<transformedPoint[2]<<")"
371  //<< " => "<<angle_x<<","<<angle_y<<" => "<<image_x<<","<<image_y<<"\n";
372 }
373 
374 /////////////////////////////////////////////////////////////////////////
375 void
376 RangeImage::getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const {
377  float image_x_float, image_y_float;
378  getImagePoint (point, image_x_float, image_y_float, range);
379  real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
380 }
381 
382 /////////////////////////////////////////////////////////////////////////
383 void
384 RangeImage::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const
385 {
386  float range;
387  getImagePoint (point, image_x, image_y, range);
388 }
389 
390 /////////////////////////////////////////////////////////////////////////
391 void
392 RangeImage::getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const
393 {
394  float image_x_float, image_y_float;
395  getImagePoint (point, image_x_float, image_y_float);
396  real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
397 }
398 
399 /////////////////////////////////////////////////////////////////////////
400 float
401 RangeImage::checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const
402 {
403  int image_x, image_y;
404  float range;
405  getImagePoint (point, image_x, image_y, range);
406  if (!isInImage (image_x, image_y))
407  point_in_image = unobserved_point;
408  else
409  point_in_image = getPoint (image_x, image_y);
410  return range;
411 }
412 
413 /////////////////////////////////////////////////////////////////////////
414 float
415 RangeImage::getRangeDifference (const Eigen::Vector3f& point) const
416 {
417  int image_x, image_y;
418  float range;
419  getImagePoint (point, image_x, image_y, range);
420  if (!isInImage (image_x, image_y))
421  return -std::numeric_limits<float>::infinity ();
422  float image_point_range = getPoint (image_x, image_y).range;
423  if (std::isinf (image_point_range))
424  {
425  if (image_point_range > 0.0f)
426  return std::numeric_limits<float>::infinity ();
427  return -std::numeric_limits<float>::infinity ();
428  }
429  return image_point_range - range;
430 }
431 
432 /////////////////////////////////////////////////////////////////////////
433 void
434 RangeImage::getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const
435 {
436  image_x = (angle_x*cosLookUp (angle_y) + static_cast<float> (M_PI))*angular_resolution_x_reciprocal_ - static_cast<float> (image_offset_x_);
437  image_y = (angle_y + 0.5f*static_cast<float> (M_PI))*angular_resolution_y_reciprocal_ - static_cast<float> (image_offset_y_);
438 }
439 
440 /////////////////////////////////////////////////////////////////////////
441 void
442 RangeImage::real2DToInt2D (float x, float y, int& xInt, int& yInt) const
443 {
444  xInt = static_cast<int> (pcl_lrintf (x));
445  yInt = static_cast<int> (pcl_lrintf (y));
446 }
447 
448 /////////////////////////////////////////////////////////////////////////
449 bool
450 RangeImage::isInImage (int x, int y) const
451 {
452  return (x >= 0 && x < static_cast<int> (width) && y >= 0 && y < static_cast<int> (height));
453 }
454 
455 /////////////////////////////////////////////////////////////////////////
456 bool
457 RangeImage::isValid (int x, int y) const
458 {
459  return isInImage (x,y) && std::isfinite (getPoint (x,y).range);
460 }
461 
462 /////////////////////////////////////////////////////////////////////////
463 bool
464 RangeImage::isValid (int index) const
465 {
466  return std::isfinite (getPoint (index).range);
467 }
468 
469 /////////////////////////////////////////////////////////////////////////
470 bool
471 RangeImage::isObserved (int x, int y) const
472 {
473  return !(!isInImage (x,y) || (std::isinf (getPoint (x,y).range) && getPoint (x,y).range < 0.0f));
474 }
475 
476 /////////////////////////////////////////////////////////////////////////
477 bool
478 RangeImage::isMaxRange (int x, int y) const
479 {
480  float range = getPoint (x,y).range;
481  return std::isinf (range) && range>0.0f;
482 }
483 
484 /////////////////////////////////////////////////////////////////////////
485 const PointWithRange&
486 RangeImage::getPoint (int image_x, int image_y) const
487 {
488  if (!isInImage (image_x, image_y))
489  return unobserved_point;
490  return points[image_y*width + image_x];
491 }
492 
493 /////////////////////////////////////////////////////////////////////////
494 const PointWithRange&
495 RangeImage::getPointNoCheck (int image_x, int image_y) const
496 {
497  return points[image_y*width + image_x];
498 }
499 
500 /////////////////////////////////////////////////////////////////////////
502 RangeImage::getPointNoCheck (int image_x, int image_y)
503 {
504  return points[image_y*width + image_x];
505 }
506 
507 /////////////////////////////////////////////////////////////////////////
509 RangeImage::getPoint (int image_x, int image_y)
510 {
511  return points[image_y*width + image_x];
512 }
513 
514 
515 /////////////////////////////////////////////////////////////////////////
516 const PointWithRange&
517 RangeImage::getPoint (int index) const
518 {
519  return points[index];
520 }
521 
522 /////////////////////////////////////////////////////////////////////////
523 const PointWithRange&
524 RangeImage::getPoint (float image_x, float image_y) const
525 {
526  int x, y;
527  real2DToInt2D (image_x, image_y, x, y);
528  return getPoint (x, y);
529 }
530 
531 /////////////////////////////////////////////////////////////////////////
533 RangeImage::getPoint (float image_x, float image_y)
534 {
535  int x, y;
536  real2DToInt2D (image_x, image_y, x, y);
537  return getPoint (x, y);
538 }
539 
540 /////////////////////////////////////////////////////////////////////////
541 void
542 RangeImage::getPoint (int image_x, int image_y, Eigen::Vector3f& point) const
543 {
544  //std::cout << getPoint (image_x, image_y)<< " - " << getPoint (image_x, image_y).getVector3fMap ()<<"\n";
545  point = getPoint (image_x, image_y).getVector3fMap ();
546 }
547 
548 /////////////////////////////////////////////////////////////////////////
549 void
550 RangeImage::getPoint (int index, Eigen::Vector3f& point) const
551 {
552  point = getPoint (index).getVector3fMap ();
553 }
554 
555 /////////////////////////////////////////////////////////////////////////
556 const Eigen::Map<const Eigen::Vector3f>
557 RangeImage::getEigenVector3f (int x, int y) const
558 {
559  return getPoint (x, y).getVector3fMap ();
560 }
561 
562 /////////////////////////////////////////////////////////////////////////
563 const Eigen::Map<const Eigen::Vector3f>
565 {
566  return getPoint (index).getVector3fMap ();
567 }
568 
569 /////////////////////////////////////////////////////////////////////////
570 void
571 RangeImage::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const
572 {
573  float angle_x, angle_y;
574  //std::cout << image_x<<","<<image_y<<","<<range;
575  getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y);
576 
577  float cosY = std::cos (angle_y);
578  point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * std::cos (angle_x)*cosY);
579  point = to_world_system_ * point;
580 }
581 
582 /////////////////////////////////////////////////////////////////////////
583 void
584 RangeImage::calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const
585 {
586  const PointWithRange& point_in_image = getPoint (image_x, image_y);
587  calculate3DPoint (image_x, image_y, point_in_image.range, point);
588 }
589 
590 /////////////////////////////////////////////////////////////////////////
591 void
592 RangeImage::calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const {
593  point.range = range;
594  Eigen::Vector3f tmp_point;
595  calculate3DPoint (image_x, image_y, range, tmp_point);
596  point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
597 }
598 
599 /////////////////////////////////////////////////////////////////////////
600 void
601 RangeImage::calculate3DPoint (float image_x, float image_y, PointWithRange& point) const
602 {
603  const PointWithRange& point_in_image = getPoint (image_x, image_y);
604  calculate3DPoint (image_x, image_y, point_in_image.range, point);
605 }
606 
607 /////////////////////////////////////////////////////////////////////////
608 void
609 RangeImage::getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const
610 {
611  angle_y = (image_y+static_cast<float> (image_offset_y_))*angular_resolution_y_ - 0.5f*static_cast<float> (M_PI);
612  float cos_angle_y = std::cos (angle_y);
613  angle_x = (cos_angle_y==0.0f ? 0.0f : ( (image_x+ static_cast<float> (image_offset_x_))*angular_resolution_x_ - static_cast<float> (M_PI))/cos_angle_y);
614 }
615 
616 /////////////////////////////////////////////////////////////////////////
617 float
618 RangeImage::getImpactAngle (int x1, int y1, int x2, int y2) const
619 {
620  if (!isInImage (x1, y1) || !isInImage (x2,y2))
621  return -std::numeric_limits<float>::infinity ();
622  return getImpactAngle (getPoint (x1,y1),getPoint (x2,y2));
623 }
624 
625 /////////////////////////////////////////////////////////////////////////
626 float
627 RangeImage::getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const {
628  if ( (std::isinf (point1.range)&&point1.range<0) || (std::isinf (point2.range)&&point2.range<0))
629  return -std::numeric_limits<float>::infinity ();
630 
631  float r1 = (std::min) (point1.range, point2.range),
632  r2 = (std::max) (point1.range, point2.range);
633  float impact_angle = static_cast<float> (0.5f * M_PI);
634 
635  if (std::isinf (r2))
636  {
637  if (r2 > 0.0f && !std::isinf (r1))
638  impact_angle = 0.0f;
639  }
640  else if (!std::isinf (r1))
641  {
642  float r1Sqr = r1*r1,
643  r2Sqr = r2*r2,
644  dSqr = squaredEuclideanDistance (point1, point2),
645  d = std::sqrt (dSqr);
646  float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
647  cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
648  impact_angle = std::acos (cos_impact_angle); // Using the cosine rule
649  }
650 
651  if (point1.range > point2.range)
652  impact_angle = -impact_angle;
653 
654  return impact_angle;
655 }
656 
657 /////////////////////////////////////////////////////////////////////////
658 float
659 RangeImage::getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const
660 {
661  float impact_angle = getImpactAngle (point1, point2);
662  if (std::isinf (impact_angle))
663  return -std::numeric_limits<float>::infinity ();
664  float ret = 1.0f - static_cast<float>(std::fabs (impact_angle)/ (0.5f*M_PI));
665  if (impact_angle < 0.0f)
666  ret = -ret;
667  //if (std::abs (ret)>1)
668  //std::cout << PVARAC (impact_angle)<<PVARN (ret);
669  return ret;
670 }
671 
672 /////////////////////////////////////////////////////////////////////////
673 float
674 RangeImage::getAcutenessValue (int x1, int y1, int x2, int y2) const
675 {
676  if (!isInImage (x1, y1) || !isInImage (x2,y2))
677  return -std::numeric_limits<float>::infinity ();
678  return getAcutenessValue (getPoint (x1,y1), getPoint (x2,y2));
679 }
680 
681 /////////////////////////////////////////////////////////////////////////
682 const Eigen::Vector3f
684 {
685  return {to_world_system_ (0,3), to_world_system_ (1,3), to_world_system_ (2,3)};
686 }
687 
688 /////////////////////////////////////////////////////////////////////////
689 void
690 RangeImage::getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const
691 {
692  angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
693  if (!isValid (x,y))
694  return;
695  Eigen::Vector3f point;
696  getPoint (x, y, point);
697  Eigen::Affine3f transformation = getTransformationToViewerCoordinateFrame (point);
698 
699  if (isObserved (x-radius, y) && isObserved (x+radius, y))
700  {
701  Eigen::Vector3f transformed_left;
702  if (isMaxRange (x-radius, y))
703  transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
704  else
705  {
706  Eigen::Vector3f left;
707  getPoint (x-radius, y, left);
708  transformed_left = - (transformation * left);
709  //std::cout << PVARN (transformed_left[1]);
710  transformed_left[1] = 0.0f;
711  transformed_left.normalize ();
712  }
713 
714  Eigen::Vector3f transformed_right;
715  if (isMaxRange (x+radius, y))
716  transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
717  else
718  {
719  Eigen::Vector3f right;
720  getPoint (x+radius, y, right);
721  transformed_right = transformation * right;
722  //std::cout << PVARN (transformed_right[1]);
723  transformed_right[1] = 0.0f;
724  transformed_right.normalize ();
725  }
726  angle_change_x = transformed_left.dot (transformed_right);
727  angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
728  angle_change_x = std::acos (angle_change_x);
729  }
730 
731  if (isObserved (x, y-radius) && isObserved (x, y+radius))
732  {
733  Eigen::Vector3f transformed_top;
734  if (isMaxRange (x, y-radius))
735  transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
736  else
737  {
738  Eigen::Vector3f top;
739  getPoint (x, y-radius, top);
740  transformed_top = - (transformation * top);
741  //std::cout << PVARN (transformed_top[0]);
742  transformed_top[0] = 0.0f;
743  transformed_top.normalize ();
744  }
745 
746  Eigen::Vector3f transformed_bottom;
747  if (isMaxRange (x, y+radius))
748  transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
749  else
750  {
751  Eigen::Vector3f bottom;
752  getPoint (x, y+radius, bottom);
753  transformed_bottom = transformation * bottom;
754  //std::cout << PVARN (transformed_bottom[0]);
755  transformed_bottom[0] = 0.0f;
756  transformed_bottom.normalize ();
757  }
758  angle_change_y = transformed_top.dot (transformed_bottom);
759  angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
760  angle_change_y = std::acos (angle_change_y);
761  }
762 }
763 
764 
765 //inline float RangeImage::getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, const PointWithRange& neighbor2) const
766 //{
767  //if (!std::isfinite (point.range) || (!std::isfinite (neighbor1.range)&&neighbor1.range<0) || (!std::isfinite (neighbor2.range)&&neighbor2.range<0))
768  //return -std::numeric_limits<float>::infinity ();
769  //if (std::isinf (neighbor1.range))
770  //{
771  //if (std::isinf (neighbor2.range))
772  //return 0.0f;
773  //else
774  //return std::acos ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor2.x, neighbor2.y, neighbor2.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ()));
775  //}
776  //if (std::isinf (neighbor2.range))
777  //return std::acos ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor1.x, neighbor1.y, neighbor1.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ()));
778 
779  //float d1_squared = squaredEuclideanDistance (point, neighbor1),
780  //d1 = std::sqrt (d1_squared),
781  //d2_squared = squaredEuclideanDistance (point, neighbor2),
782  //d2 = std::sqrt (d2_squared),
783  //d3_squared = squaredEuclideanDistance (neighbor1, neighbor2);
784  //float cos_surface_change = (d1_squared + d2_squared - d3_squared)/ (2.0f*d1*d2),
785  //surface_change = std::acos (cos_surface_change);
786  //if (std::isnan (surface_change))
787  //surface_change = static_cast<float> (M_PI);
788  ////std::cout << PVARN (point)<<PVARN (neighbor1)<<PVARN (neighbor2)<<PVARN (cos_surface_change)<<PVARN (surface_change)<<PVARN (d1)<<PVARN (d2)<<PVARN (d1_squared)<<PVARN (d2_squared)<<PVARN (d3_squared);
789 
790  //return surface_change;
791 //}
792 
793 /////////////////////////////////////////////////////////////////////////
794 float
795 RangeImage::getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, float radius)
796 {
797  return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
798 }
799 
800 /////////////////////////////////////////////////////////////////////////
801 Eigen::Vector3f
803 {
804  return {point.x, point.y, point.z};
805 }
806 
807 /////////////////////////////////////////////////////////////////////////
808 void
809 RangeImage::get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange& average_point) const
810 {
811  //std::cout << __PRETTY_FUNCTION__<<" called.\n";
812  //MEASURE_FUNCTION_TIME;
813  float weight_sum = 1.0f;
814  average_point = getPoint (x,y);
815  if (std::isinf (average_point.range))
816  {
817  if (average_point.range>0.0f) // The first point is max range -> return a max range point
818  return;
819  weight_sum = 0.0f;
820  average_point.x = average_point.y = average_point.z = average_point.range = 0.0f;
821  }
822 
823  int x2=x, y2=y;
824  Vector4fMap average_point_eigen = average_point.getVector4fMap ();
825  //std::cout << PVARN (no_of_points);
826  for (int step=1; step<no_of_points; ++step)
827  {
828  //std::cout << PVARC (step);
829  x2+=delta_x; y2+=delta_y;
830  if (!isValid (x2, y2))
831  continue;
832  const PointWithRange& p = getPointNoCheck (x2, y2);
833  average_point_eigen+=p.getVector4fMap (); average_point.range+=p.range;
834  weight_sum += 1.0f;
835  }
836  if (weight_sum<= 0.0f)
837  {
838  average_point = unobserved_point;
839  return;
840  }
841  float normalization_factor = 1.0f/weight_sum;
842  average_point_eigen *= normalization_factor;
843  average_point.range *= normalization_factor;
844  //std::cout << PVARN (average_point);
845 }
846 
847 /////////////////////////////////////////////////////////////////////////
848 float
849 RangeImage::getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const
850 {
851  if (!isObserved (x1,y1)||!isObserved (x2,y2))
852  return -std::numeric_limits<float>::infinity ();
853  const PointWithRange& point1 = getPoint (x1,y1),
854  & point2 = getPoint (x2,y2);
855  if (std::isinf (point1.range) && std::isinf (point2.range))
856  return 0.0f;
857  if (std::isinf (point1.range) || std::isinf (point2.range))
858  return std::numeric_limits<float>::infinity ();
859  return squaredEuclideanDistance (point1, point2);
860 }
861 
862 /////////////////////////////////////////////////////////////////////////
863 float
864 RangeImage::getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const
865 {
866  float average_pixel_distance = 0.0f;
867  float weight=0.0f;
868  for (int i=0; i<max_steps; ++i)
869  {
870  int x1=x+i*offset_x, y1=y+i*offset_y;
871  int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y;
872  float pixel_distance = getEuclideanDistanceSquared (x1,y1,x2,y2);
873  if (!std::isfinite (pixel_distance))
874  {
875  //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<pixel_distance<<"\n";
876  if (i==0)
877  return pixel_distance;
878  break;
879  }
880  //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<std::sqrt (pixel_distance)<<"m\n";
881  weight += 1.0f;
882  average_pixel_distance += std::sqrt (pixel_distance);
883  }
884  average_pixel_distance /= weight;
885  //std::cout << x<<","<<y<<","<<offset_x<<","<<offset_y<<" => "<<average_pixel_distance<<"\n";
886  return average_pixel_distance;
887 }
888 
889 /////////////////////////////////////////////////////////////////////////
890 float
891 RangeImage::getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const
892 {
893  if (!isValid (x,y))
894  return -std::numeric_limits<float>::infinity ();
895  const PointWithRange& point = getPoint (x, y);
896  int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> ( (radius + 1.0)), 2.0));
897  Eigen::Vector3f normal;
898  if (!getNormalForClosestNeighbors (x, y, radius, point, no_of_nearest_neighbors, normal, 1))
899  return -std::numeric_limits<float>::infinity ();
900  return deg2rad (90.0f) - std::acos (normal.dot ( (getSensorPos ()-getEigenVector3f (point)).normalized ()));
901 }
902 
903 
904 /////////////////////////////////////////////////////////////////////////
905 bool
906 RangeImage::getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size) const
907 {
908  VectorAverage3f vector_average;
909  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
910  {
911  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
912  {
913  if (!isInImage (x2, y2))
914  continue;
915  const PointWithRange& point = getPoint (x2, y2);
916  if (!std::isfinite (point.range))
917  continue;
918  vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
919  }
920  }
921  if (vector_average.getNoOfSamples () < 3)
922  return false;
923  Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
924  vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
925  if (normal.dot ( (getSensorPos ()-vector_average.getMean ()).normalized ()) < 0.0f)
926  normal *= -1.0f;
927  return true;
928 }
929 
930 /////////////////////////////////////////////////////////////////////////
931 float
932 RangeImage::getNormalBasedAcutenessValue (int x, int y, int radius) const
933 {
934  float impact_angle = getImpactAngleBasedOnLocalNormal (x, y, radius);
935  if (std::isinf (impact_angle))
936  return -std::numeric_limits<float>::infinity ();
937  float ret = 1.0f - static_cast<float> ( (impact_angle / (0.5f * M_PI)));
938  //std::cout << PVARAC (impact_angle)<<PVARN (ret);
939  return ret;
940 }
941 
942 /////////////////////////////////////////////////////////////////////////
943 bool
944 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
945  int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size) const
946 {
947  return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal, nullptr, step_size);
948 }
949 
950 /////////////////////////////////////////////////////////////////////////
951 bool
952 RangeImage::getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius) const
953 {
954  if (!isValid (x,y))
955  return false;
956  int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> (radius + 1.0), 2.0));
957  return getNormalForClosestNeighbors (x, y, radius, getPoint (x,y).getVector3fMap (), no_of_nearest_neighbors, normal);
958 }
959 
960 namespace
961 { // Anonymous namespace, so that this is only accessible in this file
962  struct NeighborWithDistance
963  { // local struct to help us with sorting
964  float distance;
965  const PointWithRange* neighbor;
966  bool operator < (const NeighborWithDistance& other) const { return distance<other.distance;}
967  };
968 }
969 
970 /////////////////////////////////////////////////////////////////////////
971 bool
972 RangeImage::getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_closest_neighbors, int step_size,
973  float& max_closest_neighbor_distance_squared,
974  Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
975  Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
976  Eigen::Vector3f* eigen_values_all_neighbors) const
977 {
978  max_closest_neighbor_distance_squared=0.0f;
979  normal.setZero (); mean.setZero (); eigen_values.setZero ();
980  if (normal_all_neighbors!=nullptr)
981  normal_all_neighbors->setZero ();
982  if (mean_all_neighbors!=nullptr)
983  mean_all_neighbors->setZero ();
984  if (eigen_values_all_neighbors!=nullptr)
985  eigen_values_all_neighbors->setZero ();
986 
987  const auto sqrt_blocksize = 2 * radius + 1;
988  const auto blocksize = sqrt_blocksize * sqrt_blocksize;
989 
990  PointWithRange given_point;
991  given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
992 
993  std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
994  int neighbor_counter = 0;
995  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
996  {
997  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
998  {
999  if (!isValid (x2, y2))
1000  continue;
1001  NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
1002  neighbor_with_distance.neighbor = &getPoint (x2, y2);
1003  neighbor_with_distance.distance = squaredEuclideanDistance (given_point, *neighbor_with_distance.neighbor);
1004  ++neighbor_counter;
1005  }
1006  }
1007  no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
1008 
1009  std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter); // Normal sort seems to be the fastest method (faster than partial_sort)
1010  //std::stable_sort (ordered_neighbors, ordered_neighbors+neighbor_counter);
1011  //std::partial_sort (ordered_neighbors, ordered_neighbors+no_of_closest_neighbors, ordered_neighbors+neighbor_counter);
1012 
1013  max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1014  //float max_distance_squared = max_closest_neighbor_distance_squared;
1015  float max_distance_squared = max_closest_neighbor_distance_squared*4.0f; // Double the allowed distance value
1016  //max_closest_neighbor_distance_squared = max_distance_squared;
1017 
1018  VectorAverage3f vector_average;
1019  //for (int neighbor_idx=0; neighbor_idx<no_of_closest_neighbors; ++neighbor_idx)
1020  int neighbor_idx;
1021  for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1022  {
1023  if (ordered_neighbors[neighbor_idx].distance > max_distance_squared)
1024  break;
1025  //std::cout << ordered_neighbors[neighbor_idx].distance<<"\n";
1026  vector_average.add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1027  }
1028 
1029  if (vector_average.getNoOfSamples () < 3)
1030  return false;
1031  //std::cout << PVARN (vector_average.getNoOfSamples ());
1032  Eigen::Vector3f eigen_vector2, eigen_vector3;
1033  vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1034  Eigen::Vector3f viewing_direction = (getSensorPos ()-point).normalized ();
1035  if (normal.dot (viewing_direction) < 0.0f)
1036  normal *= -1.0f;
1037  mean = vector_average.getMean ();
1038 
1039  if (normal_all_neighbors==nullptr)
1040  return true;
1041 
1042  // Add remaining neighbors
1043  for (int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1044  vector_average.add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1045 
1046  vector_average.doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1047  //std::cout << PVARN (vector_average.getNoOfSamples ())<<".\n";
1048  if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1049  *normal_all_neighbors *= -1.0f;
1050  *mean_all_neighbors = vector_average.getMean ();
1051 
1052  //std::cout << viewing_direction[0]<<","<<viewing_direction[1]<<","<<viewing_direction[2]<<"\n";
1053 
1054  return true;
1055 }
1056 
1057 /////////////////////////////////////////////////////////////////////////
1058 float
1059 RangeImage::getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const
1060 {
1061  const PointWithRange& point = getPoint (x, y);
1062  if (!std::isfinite (point.range))
1063  return -std::numeric_limits<float>::infinity ();
1064 
1065  const auto sqrt_blocksize = 2 * radius + 1;
1066  const auto blocksize = sqrt_blocksize * sqrt_blocksize;
1067  std::vector<float> neighbor_distances (blocksize);
1068  int neighbor_counter = 0;
1069  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1070  {
1071  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1072  {
1073  if (!isValid (x2, y2) || (x2==x&&y2==y))
1074  continue;
1075  const PointWithRange& neighbor = getPointNoCheck (x2,y2);
1076  float& neighbor_distance = neighbor_distances[neighbor_counter++];
1077  neighbor_distance = squaredEuclideanDistance (point, neighbor);
1078  }
1079  }
1080  std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter); // Normal sort seems to be
1081  // the fastest method (faster than partial_sort)
1082  n = (std::min) (neighbor_counter, n);
1083  return neighbor_distances[n-1];
1084 }
1085 
1086 
1087 /////////////////////////////////////////////////////////////////////////
1088 bool
1089 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_nearest_neighbors,
1090  Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane, int step_size) const
1091 {
1092  Eigen::Vector3f mean, eigen_values;
1093  float used_squared_max_distance;
1094  bool ret = getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1095  normal, mean, eigen_values);
1096 
1097  if (ret)
1098  {
1099  if (point_on_plane != nullptr)
1100  *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1101  }
1102  return ret;
1103 }
1104 
1105 
1106 /////////////////////////////////////////////////////////////////////////
1107 float
1108 RangeImage::getCurvature (int x, int y, int radius, int step_size) const
1109 {
1110  VectorAverage3f vector_average;
1111  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1112  {
1113  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1114  {
1115  if (!isInImage (x2, y2))
1116  continue;
1117  const PointWithRange& point = getPoint (x2, y2);
1118  if (!std::isfinite (point.range))
1119  continue;
1120  vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
1121  }
1122  }
1123  if (vector_average.getNoOfSamples () < 3)
1124  return false;
1125  Eigen::Vector3f eigen_values;
1126  vector_average.doPCA (eigen_values);
1127  return eigen_values[0]/eigen_values.sum ();
1128 }
1129 
1130 
1131 /////////////////////////////////////////////////////////////////////////
1132 template <typename PointCloudTypeWithViewpoints> Eigen::Vector3f
1133 RangeImage::getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud)
1134 {
1135  Eigen::Vector3f average_viewpoint (0,0,0);
1136  int point_counter = 0;
1137  for (const auto& point: point_cloud.points)
1138  {
1139  if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1140  continue;
1141  average_viewpoint[0] += point.vp_x;
1142  average_viewpoint[1] += point.vp_y;
1143  average_viewpoint[2] += point.vp_z;
1144  ++point_counter;
1145  }
1146  average_viewpoint /= point_counter;
1147 
1148  return average_viewpoint;
1149 }
1150 
1151 /////////////////////////////////////////////////////////////////////////
1152 bool
1153 RangeImage::getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const
1154 {
1155  if (!isValid (x, y))
1156  return false;
1157  viewing_direction = (getPoint (x,y).getVector3fMap ()-getSensorPos ()).normalized ();
1158  return true;
1159 }
1160 
1161 /////////////////////////////////////////////////////////////////////////
1162 void
1163 RangeImage::getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const
1164 {
1165  viewing_direction = (point-getSensorPos ()).normalized ();
1166 }
1167 
1168 /////////////////////////////////////////////////////////////////////////
1169 Eigen::Affine3f
1170 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const
1171 {
1172  Eigen::Affine3f transformation;
1173  getTransformationToViewerCoordinateFrame (point, transformation);
1174  return transformation;
1175 }
1176 
1177 /////////////////////////////////////////////////////////////////////////
1178 void
1179 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1180 {
1181  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1182  getTransformationFromTwoUnitVectorsAndOrigin (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, point, transformation);
1183 }
1184 
1185 /////////////////////////////////////////////////////////////////////////
1186 void
1187 RangeImage::getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1188 {
1189  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1190  getTransformationFromTwoUnitVectors (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, transformation);
1191 }
1192 
1193 /////////////////////////////////////////////////////////////////////////
1194 inline void
1195 RangeImage::setAngularResolution (float angular_resolution)
1196 {
1197  angular_resolution_x_ = angular_resolution_y_ = angular_resolution;
1199 }
1200 
1201 /////////////////////////////////////////////////////////////////////////
1202 inline void
1203 RangeImage::setAngularResolution (float angular_resolution_x, float angular_resolution_y)
1204 {
1205  angular_resolution_x_ = angular_resolution_x;
1207  angular_resolution_y_ = angular_resolution_y;
1209 }
1210 
1211 /////////////////////////////////////////////////////////////////////////
1212 inline void
1213 RangeImage::setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system)
1214 {
1215  to_range_image_system_ = to_range_image_system;
1217 }
1218 
1219 /////////////////////////////////////////////////////////////////////////
1220 inline void
1221 RangeImage::getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const
1222 {
1223  angular_resolution_x = angular_resolution_x_;
1224  angular_resolution_y = angular_resolution_y_;
1225 }
1226 
1227 /////////////////////////////////////////////////////////////////////////
1228 template <typename PointCloudType> void
1229 RangeImage::integrateFarRanges (const PointCloudType& far_ranges)
1230 {
1231  float x_real, y_real, range_of_current_point;
1232  for (const auto& point: far_ranges.points)
1233  {
1234  //if (!isFinite (point)) // Check for NAN etc
1235  //continue;
1236  Vector3fMapConst current_point = point.getVector3fMap ();
1237 
1238  this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
1239 
1240  int floor_x = static_cast<int> (pcl_lrint (std::floor (x_real))),
1241  floor_y = static_cast<int> (pcl_lrint (std::floor (y_real))),
1242  ceil_x = static_cast<int> (pcl_lrint (std::ceil (x_real))),
1243  ceil_y = static_cast<int> (pcl_lrint (std::ceil (y_real)));
1244 
1245  int neighbor_x[4], neighbor_y[4];
1246  neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1247  neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1248  neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1249  neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1250 
1251  for (int i=0; i<4; ++i)
1252  {
1253  int x=neighbor_x[i], y=neighbor_y[i];
1254  if (!isInImage (x, y))
1255  continue;
1256  PointWithRange& image_point = getPoint (x, y);
1257  if (!std::isfinite (image_point.range))
1258  image_point.range = std::numeric_limits<float>::infinity ();
1259  }
1260  }
1261 }
1262 
1263 } // namespace pcl
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...
Definition: point_tests.h:55
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
Definition: range_image.h:770
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
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.
Definition: point_cloud.h:395
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...
Definition: range_image.h:776
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...
Definition: eigen.hpp:583
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=...
Definition: eigen.hpp:564
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.
Definition: distances.h:182
static const int lookup_table_size
Definition: range_image.h:785
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
Definition: point_cloud.h:411
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).
Definition: point_cloud.h:398
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 &center, 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
Definition: range_image.h:788
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.
Definition: range_image.hpp:64
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
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.
Definition: range_image.hpp:98
static std::vector< float > asin_lookup_table
Definition: range_image.h:786
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 ...
Definition: range_image.h:774
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 ...
Definition: range_image.h:772
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
Definition: range_image.h:768
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
Definition: range_image.h:769
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:403
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.
Definition: range_image.hpp:90
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
static float asinLookUp(float value)
Query the asin lookup table.
Definition: range_image.hpp:54
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
Definition: range_image.h:771
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
Definition: range_image.h:352
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
Definition: range_image.h:787
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
Definition: range_image.h:778
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.