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