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