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 (fabsf (x) < fabsf (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)) * fabsf (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  typedef typename PointCloudType::PointType PointType2;
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  else
418  return -std::numeric_limits<float>::infinity ();
419  }
420  return image_point_range - range;
421 }
422 
423 /////////////////////////////////////////////////////////////////////////
424 void
425 RangeImage::getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const
426 {
427  image_x = (angle_x*cosLookUp (angle_y) + static_cast<float> (M_PI))*angular_resolution_x_reciprocal_ - static_cast<float> (image_offset_x_);
428  image_y = (angle_y + 0.5f*static_cast<float> (M_PI))*angular_resolution_y_reciprocal_ - static_cast<float> (image_offset_y_);
429 }
430 
431 /////////////////////////////////////////////////////////////////////////
432 void
433 RangeImage::real2DToInt2D (float x, float y, int& xInt, int& yInt) const
434 {
435  xInt = static_cast<int> (pcl_lrintf (x));
436  yInt = static_cast<int> (pcl_lrintf (y));
437 }
438 
439 /////////////////////////////////////////////////////////////////////////
440 bool
441 RangeImage::isInImage (int x, int y) const
442 {
443  return (x >= 0 && x < static_cast<int> (width) && y >= 0 && y < static_cast<int> (height));
444 }
445 
446 /////////////////////////////////////////////////////////////////////////
447 bool
448 RangeImage::isValid (int x, int y) const
449 {
450  return isInImage (x,y) && std::isfinite (getPoint (x,y).range);
451 }
452 
453 /////////////////////////////////////////////////////////////////////////
454 bool
455 RangeImage::isValid (int index) const
456 {
457  return std::isfinite (getPoint (index).range);
458 }
459 
460 /////////////////////////////////////////////////////////////////////////
461 bool
462 RangeImage::isObserved (int x, int y) const
463 {
464  return !(!isInImage (x,y) || (std::isinf (getPoint (x,y).range) && getPoint (x,y).range < 0.0f));
465 }
466 
467 /////////////////////////////////////////////////////////////////////////
468 bool
469 RangeImage::isMaxRange (int x, int y) const
470 {
471  float range = getPoint (x,y).range;
472  return std::isinf (range) && range>0.0f;
473 }
474 
475 /////////////////////////////////////////////////////////////////////////
476 const PointWithRange&
477 RangeImage::getPoint (int image_x, int image_y) const
478 {
479  if (!isInImage (image_x, image_y))
480  return unobserved_point;
481  return points[image_y*width + image_x];
482 }
483 
484 /////////////////////////////////////////////////////////////////////////
485 const PointWithRange&
486 RangeImage::getPointNoCheck (int image_x, int image_y) const
487 {
488  return points[image_y*width + image_x];
489 }
490 
491 /////////////////////////////////////////////////////////////////////////
493 RangeImage::getPointNoCheck (int image_x, int image_y)
494 {
495  return points[image_y*width + image_x];
496 }
497 
498 /////////////////////////////////////////////////////////////////////////
500 RangeImage::getPoint (int image_x, int image_y)
501 {
502  return points[image_y*width + image_x];
503 }
504 
505 
506 /////////////////////////////////////////////////////////////////////////
507 const PointWithRange&
508 RangeImage::getPoint (int index) const
509 {
510  return points[index];
511 }
512 
513 /////////////////////////////////////////////////////////////////////////
514 const PointWithRange&
515 RangeImage::getPoint (float image_x, float image_y) const
516 {
517  int x, y;
518  real2DToInt2D (image_x, image_y, x, y);
519  return getPoint (x, y);
520 }
521 
522 /////////////////////////////////////////////////////////////////////////
524 RangeImage::getPoint (float image_x, float image_y)
525 {
526  int x, y;
527  real2DToInt2D (image_x, image_y, x, y);
528  return getPoint (x, y);
529 }
530 
531 /////////////////////////////////////////////////////////////////////////
532 void
533 RangeImage::getPoint (int image_x, int image_y, Eigen::Vector3f& point) const
534 {
535  //std::cout << getPoint (image_x, image_y)<< " - " << getPoint (image_x, image_y).getVector3fMap ()<<"\n";
536  point = getPoint (image_x, image_y).getVector3fMap ();
537 }
538 
539 /////////////////////////////////////////////////////////////////////////
540 void
541 RangeImage::getPoint (int index, Eigen::Vector3f& point) const
542 {
543  point = getPoint (index).getVector3fMap ();
544 }
545 
546 /////////////////////////////////////////////////////////////////////////
547 const Eigen::Map<const Eigen::Vector3f>
548 RangeImage::getEigenVector3f (int x, int y) const
549 {
550  return getPoint (x, y).getVector3fMap ();
551 }
552 
553 /////////////////////////////////////////////////////////////////////////
554 const Eigen::Map<const Eigen::Vector3f>
556 {
557  return getPoint (index).getVector3fMap ();
558 }
559 
560 /////////////////////////////////////////////////////////////////////////
561 void
562 RangeImage::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const
563 {
564  float angle_x, angle_y;
565  //std::cout << image_x<<","<<image_y<<","<<range;
566  getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y);
567 
568  float cosY = cosf (angle_y);
569  point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * cosf (angle_x)*cosY);
570  point = to_world_system_ * point;
571 }
572 
573 /////////////////////////////////////////////////////////////////////////
574 void
575 RangeImage::calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const
576 {
577  const PointWithRange& point_in_image = getPoint (image_x, image_y);
578  calculate3DPoint (image_x, image_y, point_in_image.range, point);
579 }
580 
581 /////////////////////////////////////////////////////////////////////////
582 void
583 RangeImage::calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const {
584  point.range = range;
585  Eigen::Vector3f tmp_point;
586  calculate3DPoint (image_x, image_y, range, tmp_point);
587  point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
588 }
589 
590 /////////////////////////////////////////////////////////////////////////
591 void
592 RangeImage::calculate3DPoint (float image_x, float image_y, PointWithRange& point) const
593 {
594  const PointWithRange& point_in_image = getPoint (image_x, image_y);
595  calculate3DPoint (image_x, image_y, point_in_image.range, point);
596 }
597 
598 /////////////////////////////////////////////////////////////////////////
599 void
600 RangeImage::getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const
601 {
602  angle_y = (image_y+static_cast<float> (image_offset_y_))*angular_resolution_y_ - 0.5f*static_cast<float> (M_PI);
603  float cos_angle_y = cosf (angle_y);
604  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);
605 }
606 
607 /////////////////////////////////////////////////////////////////////////
608 float
609 RangeImage::getImpactAngle (int x1, int y1, int x2, int y2) const
610 {
611  if (!isInImage (x1, y1) || !isInImage (x2,y2))
612  return -std::numeric_limits<float>::infinity ();
613  return getImpactAngle (getPoint (x1,y1),getPoint (x2,y2));
614 }
615 
616 /////////////////////////////////////////////////////////////////////////
617 float
618 RangeImage::getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const {
619  if ( (std::isinf (point1.range)&&point1.range<0) || (std::isinf (point2.range)&&point2.range<0))
620  return -std::numeric_limits<float>::infinity ();
621 
622  float r1 = (std::min) (point1.range, point2.range),
623  r2 = (std::max) (point1.range, point2.range);
624  float impact_angle = static_cast<float> (0.5f * M_PI);
625 
626  if (std::isinf (r2))
627  {
628  if (r2 > 0.0f && !std::isinf (r1))
629  impact_angle = 0.0f;
630  }
631  else if (!std::isinf (r1))
632  {
633  float r1Sqr = r1*r1,
634  r2Sqr = r2*r2,
635  dSqr = squaredEuclideanDistance (point1, point2),
636  d = std::sqrt (dSqr);
637  float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
638  cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
639  impact_angle = acosf (cos_impact_angle); // Using the cosine rule
640  }
641 
642  if (point1.range > point2.range)
643  impact_angle = -impact_angle;
644 
645  return impact_angle;
646 }
647 
648 /////////////////////////////////////////////////////////////////////////
649 float
650 RangeImage::getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const
651 {
652  float impact_angle = getImpactAngle (point1, point2);
653  if (std::isinf (impact_angle))
654  return -std::numeric_limits<float>::infinity ();
655  float ret = 1.0f - float (std::fabs (impact_angle)/ (0.5f*M_PI));
656  if (impact_angle < 0.0f)
657  ret = -ret;
658  //if (fabs (ret)>1)
659  //std::cout << PVARAC (impact_angle)<<PVARN (ret);
660  return ret;
661 }
662 
663 /////////////////////////////////////////////////////////////////////////
664 float
665 RangeImage::getAcutenessValue (int x1, int y1, int x2, int y2) const
666 {
667  if (!isInImage (x1, y1) || !isInImage (x2,y2))
668  return -std::numeric_limits<float>::infinity ();
669  return getAcutenessValue (getPoint (x1,y1), getPoint (x2,y2));
670 }
671 
672 /////////////////////////////////////////////////////////////////////////
673 const Eigen::Vector3f
675 {
676  return Eigen::Vector3f (to_world_system_ (0,3), to_world_system_ (1,3), to_world_system_ (2,3));
677 }
678 
679 /////////////////////////////////////////////////////////////////////////
680 void
681 RangeImage::getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const
682 {
683  angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
684  if (!isValid (x,y))
685  return;
686  Eigen::Vector3f point;
687  getPoint (x, y, point);
688  Eigen::Affine3f transformation = getTransformationToViewerCoordinateFrame (point);
689 
690  if (isObserved (x-radius, y) && isObserved (x+radius, y))
691  {
692  Eigen::Vector3f transformed_left;
693  if (isMaxRange (x-radius, y))
694  transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
695  else
696  {
697  Eigen::Vector3f left;
698  getPoint (x-radius, y, left);
699  transformed_left = - (transformation * left);
700  //std::cout << PVARN (transformed_left[1]);
701  transformed_left[1] = 0.0f;
702  transformed_left.normalize ();
703  }
704 
705  Eigen::Vector3f transformed_right;
706  if (isMaxRange (x+radius, y))
707  transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
708  else
709  {
710  Eigen::Vector3f right;
711  getPoint (x+radius, y, right);
712  transformed_right = transformation * right;
713  //std::cout << PVARN (transformed_right[1]);
714  transformed_right[1] = 0.0f;
715  transformed_right.normalize ();
716  }
717  angle_change_x = transformed_left.dot (transformed_right);
718  angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
719  angle_change_x = acosf (angle_change_x);
720  }
721 
722  if (isObserved (x, y-radius) && isObserved (x, y+radius))
723  {
724  Eigen::Vector3f transformed_top;
725  if (isMaxRange (x, y-radius))
726  transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
727  else
728  {
729  Eigen::Vector3f top;
730  getPoint (x, y-radius, top);
731  transformed_top = - (transformation * top);
732  //std::cout << PVARN (transformed_top[0]);
733  transformed_top[0] = 0.0f;
734  transformed_top.normalize ();
735  }
736 
737  Eigen::Vector3f transformed_bottom;
738  if (isMaxRange (x, y+radius))
739  transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
740  else
741  {
742  Eigen::Vector3f bottom;
743  getPoint (x, y+radius, bottom);
744  transformed_bottom = transformation * bottom;
745  //std::cout << PVARN (transformed_bottom[0]);
746  transformed_bottom[0] = 0.0f;
747  transformed_bottom.normalize ();
748  }
749  angle_change_y = transformed_top.dot (transformed_bottom);
750  angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
751  angle_change_y = acosf (angle_change_y);
752  }
753 }
754 
755 
756 //inline float RangeImage::getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, const PointWithRange& neighbor2) const
757 //{
758  //if (!std::isfinite (point.range) || (!std::isfinite (neighbor1.range)&&neighbor1.range<0) || (!std::isfinite (neighbor2.range)&&neighbor2.range<0))
759  //return -std::numeric_limits<float>::infinity ();
760  //if (std::isinf (neighbor1.range))
761  //{
762  //if (std::isinf (neighbor2.range))
763  //return 0.0f;
764  //else
765  //return acosf ( (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 ()));
766  //}
767  //if (std::isinf (neighbor2.range))
768  //return acosf ( (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 ()));
769 
770  //float d1_squared = squaredEuclideanDistance (point, neighbor1),
771  //d1 = std::sqrt (d1_squared),
772  //d2_squared = squaredEuclideanDistance (point, neighbor2),
773  //d2 = std::sqrt (d2_squared),
774  //d3_squared = squaredEuclideanDistance (neighbor1, neighbor2);
775  //float cos_surface_change = (d1_squared + d2_squared - d3_squared)/ (2.0f*d1*d2),
776  //surface_change = acosf (cos_surface_change);
777  //if (std::isnan (surface_change))
778  //surface_change = static_cast<float> (M_PI);
779  ////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);
780 
781  //return surface_change;
782 //}
783 
784 /////////////////////////////////////////////////////////////////////////
785 float
786 RangeImage::getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, float radius)
787 {
788  return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
789 }
790 
791 /////////////////////////////////////////////////////////////////////////
792 Eigen::Vector3f
794 {
795  return Eigen::Vector3f (point.x, point.y, point.z);
796 }
797 
798 /////////////////////////////////////////////////////////////////////////
799 void
800 RangeImage::get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange& average_point) const
801 {
802  //std::cout << __PRETTY_FUNCTION__<<" called.\n";
803  //MEASURE_FUNCTION_TIME;
804  float weight_sum = 1.0f;
805  average_point = getPoint (x,y);
806  if (std::isinf (average_point.range))
807  {
808  if (average_point.range>0.0f) // The first point is max range -> return a max range point
809  return;
810  weight_sum = 0.0f;
811  average_point.x = average_point.y = average_point.z = average_point.range = 0.0f;
812  }
813 
814  int x2=x, y2=y;
815  Vector4fMap average_point_eigen = average_point.getVector4fMap ();
816  //std::cout << PVARN (no_of_points);
817  for (int step=1; step<no_of_points; ++step)
818  {
819  //std::cout << PVARC (step);
820  x2+=delta_x; y2+=delta_y;
821  if (!isValid (x2, y2))
822  continue;
823  const PointWithRange& p = getPointNoCheck (x2, y2);
824  average_point_eigen+=p.getVector4fMap (); average_point.range+=p.range;
825  weight_sum += 1.0f;
826  }
827  if (weight_sum<= 0.0f)
828  {
829  average_point = unobserved_point;
830  return;
831  }
832  float normalization_factor = 1.0f/weight_sum;
833  average_point_eigen *= normalization_factor;
834  average_point.range *= normalization_factor;
835  //std::cout << PVARN (average_point);
836 }
837 
838 /////////////////////////////////////////////////////////////////////////
839 float
840 RangeImage::getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const
841 {
842  if (!isObserved (x1,y1)||!isObserved (x2,y2))
843  return -std::numeric_limits<float>::infinity ();
844  const PointWithRange& point1 = getPoint (x1,y1),
845  & point2 = getPoint (x2,y2);
846  if (std::isinf (point1.range) && std::isinf (point2.range))
847  return 0.0f;
848  if (std::isinf (point1.range) || std::isinf (point2.range))
849  return std::numeric_limits<float>::infinity ();
850  return squaredEuclideanDistance (point1, point2);
851 }
852 
853 /////////////////////////////////////////////////////////////////////////
854 float
855 RangeImage::getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const
856 {
857  float average_pixel_distance = 0.0f;
858  float weight=0.0f;
859  for (int i=0; i<max_steps; ++i)
860  {
861  int x1=x+i*offset_x, y1=y+i*offset_y;
862  int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y;
863  float pixel_distance = getEuclideanDistanceSquared (x1,y1,x2,y2);
864  if (!std::isfinite (pixel_distance))
865  {
866  //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<pixel_distance<<"\n";
867  if (i==0)
868  return pixel_distance;
869  else
870  break;
871  }
872  //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<std::sqrt (pixel_distance)<<"m\n";
873  weight += 1.0f;
874  average_pixel_distance += std::sqrt (pixel_distance);
875  }
876  average_pixel_distance /= weight;
877  //std::cout << x<<","<<y<<","<<offset_x<<","<<offset_y<<" => "<<average_pixel_distance<<"\n";
878  return average_pixel_distance;
879 }
880 
881 /////////////////////////////////////////////////////////////////////////
882 float
883 RangeImage::getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const
884 {
885  if (!isValid (x,y))
886  return -std::numeric_limits<float>::infinity ();
887  const PointWithRange& point = getPoint (x, y);
888  int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> ( (radius + 1.0)), 2.0));
889  Eigen::Vector3f normal;
890  if (!getNormalForClosestNeighbors (x, y, radius, point, no_of_nearest_neighbors, normal, 1))
891  return -std::numeric_limits<float>::infinity ();
892  return deg2rad (90.0f) - acosf (normal.dot ( (getSensorPos ()-getEigenVector3f (point)).normalized ()));
893 }
894 
895 
896 /////////////////////////////////////////////////////////////////////////
897 bool
898 RangeImage::getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size) const
899 {
900  VectorAverage3f vector_average;
901  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
902  {
903  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
904  {
905  if (!isInImage (x2, y2))
906  continue;
907  const PointWithRange& point = getPoint (x2, y2);
908  if (!std::isfinite (point.range))
909  continue;
910  vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
911  }
912  }
913  if (vector_average.getNoOfSamples () < 3)
914  return false;
915  Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
916  vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
917  if (normal.dot ( (getSensorPos ()-vector_average.getMean ()).normalized ()) < 0.0f)
918  normal *= -1.0f;
919  return true;
920 }
921 
922 /////////////////////////////////////////////////////////////////////////
923 float
924 RangeImage::getNormalBasedAcutenessValue (int x, int y, int radius) const
925 {
926  float impact_angle = getImpactAngleBasedOnLocalNormal (x, y, radius);
927  if (std::isinf (impact_angle))
928  return -std::numeric_limits<float>::infinity ();
929  float ret = 1.0f - static_cast<float> ( (impact_angle / (0.5f * M_PI)));
930  //std::cout << PVARAC (impact_angle)<<PVARN (ret);
931  return ret;
932 }
933 
934 /////////////////////////////////////////////////////////////////////////
935 bool
936 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
937  int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size) const
938 {
939  return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal, nullptr, step_size);
940 }
941 
942 /////////////////////////////////////////////////////////////////////////
943 bool
944 RangeImage::getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius) const
945 {
946  if (!isValid (x,y))
947  return false;
948  int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> (radius + 1.0), 2.0));
949  return getNormalForClosestNeighbors (x, y, radius, getPoint (x,y).getVector3fMap (), no_of_nearest_neighbors, normal);
950 }
951 
952 namespace
953 { // Anonymous namespace, so that this is only accessible in this file
954  struct NeighborWithDistance
955  { // local struct to help us with sorting
956  float distance;
957  const PointWithRange* neighbor;
958  bool operator < (const NeighborWithDistance& other) const { return distance<other.distance;}
959  };
960 }
961 
962 /////////////////////////////////////////////////////////////////////////
963 bool
964 RangeImage::getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_closest_neighbors, int step_size,
965  float& max_closest_neighbor_distance_squared,
966  Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
967  Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
968  Eigen::Vector3f* eigen_values_all_neighbors) const
969 {
970  max_closest_neighbor_distance_squared=0.0f;
971  normal.setZero (); mean.setZero (); eigen_values.setZero ();
972  if (normal_all_neighbors!=nullptr)
973  normal_all_neighbors->setZero ();
974  if (mean_all_neighbors!=nullptr)
975  mean_all_neighbors->setZero ();
976  if (eigen_values_all_neighbors!=nullptr)
977  eigen_values_all_neighbors->setZero ();
978 
979  int blocksize = static_cast<int> (pow (static_cast<double> ( (2.0 * radius + 1.0)), 2.0));
980 
981  PointWithRange given_point;
982  given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
983 
984  std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
985  int neighbor_counter = 0;
986  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
987  {
988  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
989  {
990  if (!isValid (x2, y2))
991  continue;
992  NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
993  neighbor_with_distance.neighbor = &getPoint (x2, y2);
994  neighbor_with_distance.distance = squaredEuclideanDistance (given_point, *neighbor_with_distance.neighbor);
995  ++neighbor_counter;
996  }
997  }
998  no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
999 
1000  std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter); // Normal sort seems to be the fastest method (faster than partial_sort)
1001  //std::stable_sort (ordered_neighbors, ordered_neighbors+neighbor_counter);
1002  //std::partial_sort (ordered_neighbors, ordered_neighbors+no_of_closest_neighbors, ordered_neighbors+neighbor_counter);
1003 
1004  max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1005  //float max_distance_squared = max_closest_neighbor_distance_squared;
1006  float max_distance_squared = max_closest_neighbor_distance_squared*4.0f; // Double the allowed distance value
1007  //max_closest_neighbor_distance_squared = max_distance_squared;
1008 
1009  VectorAverage3f vector_average;
1010  //for (int neighbor_idx=0; neighbor_idx<no_of_closest_neighbors; ++neighbor_idx)
1011  int neighbor_idx;
1012  for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1013  {
1014  if (ordered_neighbors[neighbor_idx].distance > max_distance_squared)
1015  break;
1016  //std::cout << ordered_neighbors[neighbor_idx].distance<<"\n";
1017  vector_average.add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1018  }
1019 
1020  if (vector_average.getNoOfSamples () < 3)
1021  return false;
1022  //std::cout << PVARN (vector_average.getNoOfSamples ());
1023  Eigen::Vector3f eigen_vector2, eigen_vector3;
1024  vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1025  Eigen::Vector3f viewing_direction = (getSensorPos ()-point).normalized ();
1026  if (normal.dot (viewing_direction) < 0.0f)
1027  normal *= -1.0f;
1028  mean = vector_average.getMean ();
1029 
1030  if (normal_all_neighbors==nullptr)
1031  return true;
1032 
1033  // Add remaining neighbors
1034  for (int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1035  vector_average.add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1036 
1037  vector_average.doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1038  //std::cout << PVARN (vector_average.getNoOfSamples ())<<".\n";
1039  if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1040  *normal_all_neighbors *= -1.0f;
1041  *mean_all_neighbors = vector_average.getMean ();
1042 
1043  //std::cout << viewing_direction[0]<<","<<viewing_direction[1]<<","<<viewing_direction[2]<<"\n";
1044 
1045  return true;
1046 }
1047 
1048 /////////////////////////////////////////////////////////////////////////
1049 float
1050 RangeImage::getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const
1051 {
1052  const PointWithRange& point = getPoint (x, y);
1053  if (!std::isfinite (point.range))
1054  return -std::numeric_limits<float>::infinity ();
1055 
1056  int blocksize = static_cast<int> (pow (static_cast<double> (2.0 * radius + 1.0), 2.0));
1057  std::vector<float> neighbor_distances (blocksize);
1058  int neighbor_counter = 0;
1059  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1060  {
1061  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1062  {
1063  if (!isValid (x2, y2) || (x2==x&&y2==y))
1064  continue;
1065  const PointWithRange& neighbor = getPointNoCheck (x2,y2);
1066  float& neighbor_distance = neighbor_distances[neighbor_counter++];
1067  neighbor_distance = squaredEuclideanDistance (point, neighbor);
1068  }
1069  }
1070  std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter); // Normal sort seems to be
1071  // the fastest method (faster than partial_sort)
1072  n = (std::min) (neighbor_counter, n);
1073  return neighbor_distances[n-1];
1074 }
1075 
1076 
1077 /////////////////////////////////////////////////////////////////////////
1078 bool
1079 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_nearest_neighbors,
1080  Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane, int step_size) const
1081 {
1082  Eigen::Vector3f mean, eigen_values;
1083  float used_squared_max_distance;
1084  bool ret = getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1085  normal, mean, eigen_values);
1086 
1087  if (ret)
1088  {
1089  if (point_on_plane != nullptr)
1090  *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1091  }
1092  return ret;
1093 }
1094 
1095 
1096 /////////////////////////////////////////////////////////////////////////
1097 float
1098 RangeImage::getCurvature (int x, int y, int radius, int step_size) const
1099 {
1100  VectorAverage3f vector_average;
1101  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1102  {
1103  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1104  {
1105  if (!isInImage (x2, y2))
1106  continue;
1107  const PointWithRange& point = getPoint (x2, y2);
1108  if (!std::isfinite (point.range))
1109  continue;
1110  vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
1111  }
1112  }
1113  if (vector_average.getNoOfSamples () < 3)
1114  return false;
1115  Eigen::Vector3f eigen_values;
1116  vector_average.doPCA (eigen_values);
1117  return eigen_values[0]/eigen_values.sum ();
1118 }
1119 
1120 
1121 /////////////////////////////////////////////////////////////////////////
1122 template <typename PointCloudTypeWithViewpoints> Eigen::Vector3f
1123 RangeImage::getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud)
1124 {
1125  Eigen::Vector3f average_viewpoint (0,0,0);
1126  int point_counter = 0;
1127  for (unsigned int point_idx=0; point_idx<point_cloud.points.size (); ++point_idx)
1128  {
1129  const typename PointCloudTypeWithViewpoints::PointType& point = point_cloud.points[point_idx];
1130  if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1131  continue;
1132  average_viewpoint[0] += point.vp_x;
1133  average_viewpoint[1] += point.vp_y;
1134  average_viewpoint[2] += point.vp_z;
1135  ++point_counter;
1136  }
1137  average_viewpoint /= point_counter;
1138 
1139  return average_viewpoint;
1140 }
1141 
1142 /////////////////////////////////////////////////////////////////////////
1143 bool
1144 RangeImage::getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const
1145 {
1146  if (!isValid (x, y))
1147  return false;
1148  viewing_direction = (getPoint (x,y).getVector3fMap ()-getSensorPos ()).normalized ();
1149  return true;
1150 }
1151 
1152 /////////////////////////////////////////////////////////////////////////
1153 void
1154 RangeImage::getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const
1155 {
1156  viewing_direction = (point-getSensorPos ()).normalized ();
1157 }
1158 
1159 /////////////////////////////////////////////////////////////////////////
1160 Eigen::Affine3f
1161 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const
1162 {
1163  Eigen::Affine3f transformation;
1164  getTransformationToViewerCoordinateFrame (point, transformation);
1165  return transformation;
1166 }
1167 
1168 /////////////////////////////////////////////////////////////////////////
1169 void
1170 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1171 {
1172  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1173  getTransformationFromTwoUnitVectorsAndOrigin (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, point, transformation);
1174 }
1175 
1176 /////////////////////////////////////////////////////////////////////////
1177 void
1178 RangeImage::getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1179 {
1180  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1181  getTransformationFromTwoUnitVectors (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, transformation);
1182 }
1183 
1184 /////////////////////////////////////////////////////////////////////////
1185 inline void
1186 RangeImage::setAngularResolution (float angular_resolution)
1187 {
1188  angular_resolution_x_ = angular_resolution_y_ = angular_resolution;
1190 }
1191 
1192 /////////////////////////////////////////////////////////////////////////
1193 inline void
1194 RangeImage::setAngularResolution (float angular_resolution_x, float angular_resolution_y)
1195 {
1196  angular_resolution_x_ = angular_resolution_x;
1198  angular_resolution_y_ = angular_resolution_y;
1200 }
1201 
1202 /////////////////////////////////////////////////////////////////////////
1203 inline void
1204 RangeImage::setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system)
1205 {
1206  to_range_image_system_ = to_range_image_system;
1208 }
1209 
1210 /////////////////////////////////////////////////////////////////////////
1211 inline void
1212 RangeImage::getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const
1213 {
1214  angular_resolution_x = angular_resolution_x_;
1215  angular_resolution_y = angular_resolution_y_;
1216 }
1217 
1218 /////////////////////////////////////////////////////////////////////////
1219 template <typename PointCloudType> void
1220 RangeImage::integrateFarRanges (const PointCloudType& far_ranges)
1221 {
1222  float x_real, y_real, range_of_current_point;
1223  for (typename PointCloudType::const_iterator it = far_ranges.points.begin (); it != far_ranges.points.end (); ++it)
1224  {
1225  //if (!isFinite (*it)) // Check for NAN etc
1226  //continue;
1227  Vector3fMapConst current_point = it->getVector3fMap ();
1228 
1229  this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
1230 
1231  int floor_x = static_cast<int> (pcl_lrint (std::floor (x_real))),
1232  floor_y = static_cast<int> (pcl_lrint (std::floor (y_real))),
1233  ceil_x = static_cast<int> (pcl_lrint (std::ceil (x_real))),
1234  ceil_y = static_cast<int> (pcl_lrint (std::ceil (y_real)));
1235 
1236  int neighbor_x[4], neighbor_y[4];
1237  neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1238  neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1239  neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1240  neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1241 
1242  for (int i=0; i<4; ++i)
1243  {
1244  int x=neighbor_x[i], y=neighbor_y[i];
1245  if (!isInImage (x, y))
1246  continue;
1247  PointWithRange& image_point = getPoint (x, y);
1248  if (!std::isfinite (image_point.range))
1249  image_point.range = std::numeric_limits<float>::infinity ();
1250  }
1251  }
1252 }
1253 
1254 } // namespace end
1255 #endif
1256 
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:409
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.
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:44
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:442
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:425
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.
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:414
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:412
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.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
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.
static float atan2LookUp(float y, float x)
Query the 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:417
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
static Eigen::Vector3f getAverageViewPoint(const PointCloudTypeWithViewpoints &point_cloud)
Get the average viewpoint of a point cloud where each point carries viewpoint information as vp_x...
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
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:441
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.
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.
VectorType::const_iterator const_iterator
Definition: point_cloud.h:440