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