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 (const auto& point: points2)
244  {
245  if (!isFinite (point)) // Check for NAN etc
246  continue;
247  Vector3fMapConst current_point = point.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  const auto sqrt_blocksize = 2 * radius + 1;
978  const auto blocksize = sqrt_blocksize * sqrt_blocksize;
979 
980  PointWithRange given_point;
981  given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
982 
983  std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
984  int neighbor_counter = 0;
985  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
986  {
987  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
988  {
989  if (!isValid (x2, y2))
990  continue;
991  NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
992  neighbor_with_distance.neighbor = &getPoint (x2, y2);
993  neighbor_with_distance.distance = squaredEuclideanDistance (given_point, *neighbor_with_distance.neighbor);
994  ++neighbor_counter;
995  }
996  }
997  no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
998 
999  std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter); // Normal sort seems to be the fastest method (faster than partial_sort)
1000  //std::stable_sort (ordered_neighbors, ordered_neighbors+neighbor_counter);
1001  //std::partial_sort (ordered_neighbors, ordered_neighbors+no_of_closest_neighbors, ordered_neighbors+neighbor_counter);
1002 
1003  max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1004  //float max_distance_squared = max_closest_neighbor_distance_squared;
1005  float max_distance_squared = max_closest_neighbor_distance_squared*4.0f; // Double the allowed distance value
1006  //max_closest_neighbor_distance_squared = max_distance_squared;
1007 
1008  VectorAverage3f vector_average;
1009  //for (int neighbor_idx=0; neighbor_idx<no_of_closest_neighbors; ++neighbor_idx)
1010  int neighbor_idx;
1011  for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1012  {
1013  if (ordered_neighbors[neighbor_idx].distance > max_distance_squared)
1014  break;
1015  //std::cout << ordered_neighbors[neighbor_idx].distance<<"\n";
1016  vector_average.add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1017  }
1018 
1019  if (vector_average.getNoOfSamples () < 3)
1020  return false;
1021  //std::cout << PVARN (vector_average.getNoOfSamples ());
1022  Eigen::Vector3f eigen_vector2, eigen_vector3;
1023  vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1024  Eigen::Vector3f viewing_direction = (getSensorPos ()-point).normalized ();
1025  if (normal.dot (viewing_direction) < 0.0f)
1026  normal *= -1.0f;
1027  mean = vector_average.getMean ();
1028 
1029  if (normal_all_neighbors==nullptr)
1030  return true;
1031 
1032  // Add remaining neighbors
1033  for (int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1034  vector_average.add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1035 
1036  vector_average.doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1037  //std::cout << PVARN (vector_average.getNoOfSamples ())<<".\n";
1038  if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1039  *normal_all_neighbors *= -1.0f;
1040  *mean_all_neighbors = vector_average.getMean ();
1041 
1042  //std::cout << viewing_direction[0]<<","<<viewing_direction[1]<<","<<viewing_direction[2]<<"\n";
1043 
1044  return true;
1045 }
1046 
1047 /////////////////////////////////////////////////////////////////////////
1048 float
1049 RangeImage::getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const
1050 {
1051  const PointWithRange& point = getPoint (x, y);
1052  if (!std::isfinite (point.range))
1053  return -std::numeric_limits<float>::infinity ();
1054 
1055  const auto sqrt_blocksize = 2 * radius + 1;
1056  const auto blocksize = sqrt_blocksize * sqrt_blocksize;
1057  std::vector<float> neighbor_distances (blocksize);
1058  int neighbor_counter = 0;
1059  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
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 (const auto& point: point_cloud.points)
1128  {
1129  if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1130  continue;
1131  average_viewpoint[0] += point.vp_x;
1132  average_viewpoint[1] += point.vp_y;
1133  average_viewpoint[2] += point.vp_z;
1134  ++point_counter;
1135  }
1136  average_viewpoint /= point_counter;
1137 
1138  return average_viewpoint;
1139 }
1140 
1141 /////////////////////////////////////////////////////////////////////////
1142 bool
1143 RangeImage::getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const
1144 {
1145  if (!isValid (x, y))
1146  return false;
1147  viewing_direction = (getPoint (x,y).getVector3fMap ()-getSensorPos ()).normalized ();
1148  return true;
1149 }
1150 
1151 /////////////////////////////////////////////////////////////////////////
1152 void
1153 RangeImage::getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const
1154 {
1155  viewing_direction = (point-getSensorPos ()).normalized ();
1156 }
1157 
1158 /////////////////////////////////////////////////////////////////////////
1159 Eigen::Affine3f
1160 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const
1161 {
1162  Eigen::Affine3f transformation;
1163  getTransformationToViewerCoordinateFrame (point, transformation);
1164  return transformation;
1165 }
1166 
1167 /////////////////////////////////////////////////////////////////////////
1168 void
1169 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1170 {
1171  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1172  getTransformationFromTwoUnitVectorsAndOrigin (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, point, transformation);
1173 }
1174 
1175 /////////////////////////////////////////////////////////////////////////
1176 void
1177 RangeImage::getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1178 {
1179  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1180  getTransformationFromTwoUnitVectors (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, transformation);
1181 }
1182 
1183 /////////////////////////////////////////////////////////////////////////
1184 inline void
1185 RangeImage::setAngularResolution (float angular_resolution)
1186 {
1187  angular_resolution_x_ = angular_resolution_y_ = angular_resolution;
1189 }
1190 
1191 /////////////////////////////////////////////////////////////////////////
1192 inline void
1193 RangeImage::setAngularResolution (float angular_resolution_x, float angular_resolution_y)
1194 {
1195  angular_resolution_x_ = angular_resolution_x;
1197  angular_resolution_y_ = angular_resolution_y;
1199 }
1200 
1201 /////////////////////////////////////////////////////////////////////////
1202 inline void
1203 RangeImage::setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system)
1204 {
1205  to_range_image_system_ = to_range_image_system;
1207 }
1208 
1209 /////////////////////////////////////////////////////////////////////////
1210 inline void
1211 RangeImage::getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const
1212 {
1213  angular_resolution_x = angular_resolution_x_;
1214  angular_resolution_y = angular_resolution_y_;
1215 }
1216 
1217 /////////////////////////////////////////////////////////////////////////
1218 template <typename PointCloudType> void
1219 RangeImage::integrateFarRanges (const PointCloudType& far_ranges)
1220 {
1221  float x_real, y_real, range_of_current_point;
1222  for (const auto& point: far_ranges.points)
1223  {
1224  //if (!isFinite (point)) // Check for NAN etc
1225  //continue;
1226  Vector3fMapConst current_point = point.getVector3fMap ();
1227 
1228  this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
1229 
1230  int floor_x = static_cast<int> (pcl_lrint (std::floor (x_real))),
1231  floor_y = static_cast<int> (pcl_lrint (std::floor (y_real))),
1232  ceil_x = static_cast<int> (pcl_lrint (std::ceil (x_real))),
1233  ceil_y = static_cast<int> (pcl_lrint (std::ceil (y_real)));
1234 
1235  int neighbor_x[4], neighbor_y[4];
1236  neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1237  neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1238  neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1239  neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1240 
1241  for (int i=0; i<4; ++i)
1242  {
1243  int x=neighbor_x[i], y=neighbor_y[i];
1244  if (!isInImage (x, y))
1245  continue;
1246  PointWithRange& image_point = getPoint (x, y);
1247  if (!std::isfinite (image_point.range))
1248  image_point.range = std::numeric_limits<float>::infinity ();
1249  }
1250  }
1251 }
1252 
1253 } // namespace end
1254 #endif
1255 
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
#define pcl_lrint(x)
Definition: pcl_macros.h:153
void createFromPointCloudWithViewpoints(const PointCloudTypeWithViewpoints &point_cloud, float angular_resolution, float max_angle_width, float max_angle_height, CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud, using the average viewpoint of the points (vp_x...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
Definition: range_image.h:771
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
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:426
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 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...
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
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...
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:654
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.
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
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...
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:636
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 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 real2DToInt2D(float x, float y, int &xInt, int &yInt) const
Transforms an image point in float values to an image point in int values.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:178
static const int lookup_table_size
Definition: range_image.h:786
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
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.
bool isInImage(int x, int y) const
Check if a point is inside of the image.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:442
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...
Define standard C methods to do distance calculations.
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:431
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
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...
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:429
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...
bool getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
unsigned int getNoOfSamples()
Get the number of added vectors.
const Eigen::Matrix< real, dimension, 1 > & getMean() const
Get the mean of the added vectors.
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.
static std::vector< float > cos_lookup_table
Definition: range_image.h:789
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
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.
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
Definition: range_image.h:353
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.
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.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performance of multiplication compared to division ...
Definition: range_image.h:775
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performance of multiplication compared to division ...
Definition: range_image.h:773
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...
#define pcl_lrintf(x)
Definition: pcl_macros.h:154
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...
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! ...
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
Definition: range_image.h:769
void getAnglesFromImagePoint(float image_x, float image_y, float &angle_x, float &angle_y) const
Get the angles corresponding to the given image point.
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:434
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.
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...
#define ERASE_ARRAY(var, size)
Definition: pcl_macros.h:204
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 getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
bool isObserved(int x, int y) const
Check if a point is inside of the image and has either a finite range or a max reading (range=INFINIT...
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
Definition: range_image.h:772
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
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...
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
Defines all the PCL and non-PCL macros used.
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...
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.
void integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.