Point Cloud Library (PCL)  1.9.0-dev
range_image.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_RANGE_IMAGE_IMPL_HPP_
40 #define PCL_RANGE_IMAGE_IMPL_HPP_
41 
42 #include <pcl/pcl_macros.h>
43 #include <pcl/common/distances.h>
44 
45 namespace pcl
46 {
47 
48 /////////////////////////////////////////////////////////////////////////
49 inline float
51 {
52  return (asin_lookup_table[
53  static_cast<int> (
54  static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * value)) +
55  static_cast<float> (lookup_table_size-1) / 2.0f)]);
56 }
57 
58 /////////////////////////////////////////////////////////////////////////
59 inline float
60 RangeImage::atan2LookUp (float y, float x)
61 {
62  if (x==0 && y==0)
63  return 0;
64  float ret;
65  if (fabsf (x) < fabsf (y))
66  {
67  ret = atan_lookup_table[
68  static_cast<int> (
69  static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (x / y))) +
70  static_cast<float> (lookup_table_size-1) / 2.0f)];
71  ret = static_cast<float> (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret);
72  }
73  else
74  ret = atan_lookup_table[
75  static_cast<int> (
76  static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (y / x))) +
77  static_cast<float> (lookup_table_size-1)/2.0f)];
78  if (x < 0)
79  ret = static_cast<float> (y < 0 ? ret-M_PI : ret+M_PI);
80 
81  return (ret);
82 }
83 
84 /////////////////////////////////////////////////////////////////////////
85 inline float
86 RangeImage::cosLookUp (float value)
87 {
88  int cell_idx = static_cast<int> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1)) * fabsf (value) / (2.0f * static_cast<float> (M_PI))));
89  return (cos_lookup_table[cell_idx]);
90 }
91 
92 /////////////////////////////////////////////////////////////////////////
93 template <typename PointCloudType> void
94 RangeImage::createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution,
95  float max_angle_width, float max_angle_height,
96  const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
97  float noise_level, float min_range, int border_size)
98 {
99  createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height,
100  sensor_pose, coordinate_frame, noise_level, min_range, border_size);
101 }
102 
103 /////////////////////////////////////////////////////////////////////////
104 template <typename PointCloudType> void
105 RangeImage::createFromPointCloud (const PointCloudType& point_cloud,
106  float angular_resolution_x, float angular_resolution_y,
107  float max_angle_width, float max_angle_height,
108  const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
109  float noise_level, float min_range, int border_size)
110 {
111  setAngularResolution (angular_resolution_x, angular_resolution_y);
112 
113  width = static_cast<uint32_t> (pcl_lrint (floor (max_angle_width*angular_resolution_x_reciprocal_)));
114  height = static_cast<uint32_t> (pcl_lrint (floor (max_angle_height*angular_resolution_y_reciprocal_)));
115 
116  int full_width = static_cast<int> (pcl_lrint (floor (pcl::deg2rad (360.0f)*angular_resolution_x_reciprocal_))),
117  full_height = static_cast<int> (pcl_lrint (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 (ceil (0.5f*max_angle_size*angular_resolution_x_reciprocal_)),
179  pixel_radius_y = pcl_lrint (ceil (0.5f*max_angle_size*angular_resolution_y_reciprocal_));
180  width = 2*pixel_radius_x;
181  height = 2*pixel_radius_y;
182  is_dense = false;
183 
184  image_offset_x_ = image_offset_y_ = 0; // temporary values for getImagePoint
185  int center_pixel_x, center_pixel_y;
186  getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y);
187  image_offset_x_ = (std::max) (0, center_pixel_x-pixel_radius_x);
188  image_offset_y_ = (std::max) (0, center_pixel_y-pixel_radius_y);
189 
190  points.clear ();
192 
193  int top=height, right=-1, bottom=-1, left=width;
194  doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
195 
196  cropImage (border_size, top, right, bottom, left);
197 
199 }
200 
201 /////////////////////////////////////////////////////////////////////////
202 template <typename PointCloudTypeWithViewpoints> void
203 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
204  float angular_resolution,
205  float max_angle_width, float max_angle_height,
206  RangeImage::CoordinateFrame coordinate_frame,
207  float noise_level, float min_range, int border_size)
208 {
209  createFromPointCloudWithViewpoints (point_cloud, angular_resolution, angular_resolution,
210  max_angle_width, max_angle_height, coordinate_frame,
211  noise_level, min_range, border_size);
212 }
213 
214 /////////////////////////////////////////////////////////////////////////
215 template <typename PointCloudTypeWithViewpoints> void
216 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
217  float angular_resolution_x, float angular_resolution_y,
218  float max_angle_width, float max_angle_height,
219  RangeImage::CoordinateFrame coordinate_frame,
220  float noise_level, float min_range, int border_size)
221 {
222  Eigen::Vector3f average_viewpoint = getAverageViewPoint (point_cloud);
223  Eigen::Affine3f sensor_pose = static_cast<Eigen::Affine3f> (Eigen::Translation3f (average_viewpoint));
224  createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height,
225  sensor_pose, coordinate_frame, noise_level, min_range, border_size);
226 }
227 
228 /////////////////////////////////////////////////////////////////////////
229 template <typename PointCloudType> void
230 RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, float min_range, int& top, int& right, int& bottom, int& left)
231 {
232  typedef typename PointCloudType::PointType PointType2;
233  const typename pcl::PointCloud<PointType2>::VectorType &points2 = point_cloud.points;
234 
235  unsigned int size = width*height;
236  int* counters = new int[size];
237  ERASE_ARRAY (counters, size);
238 
239  top=height; right=-1; bottom=-1; left=width;
240 
241  float x_real, y_real, range_of_current_point;
242  int x, y;
243  for (typename pcl::PointCloud<PointType2>::VectorType::const_iterator it=points2.begin (); it!=points2.end (); ++it)
244  {
245  if (!isFinite (*it)) // Check for NAN etc
246  continue;
247  Vector3fMapConst current_point = it->getVector3fMap ();
248 
249  this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
250  this->real2DToInt2D (x_real, y_real, x, y);
251 
252  if (range_of_current_point < min_range|| !isInImage (x, y))
253  continue;
254  //std::cout << " ("<<current_point[0]<<", "<<current_point[1]<<", "<<current_point[2]<<") falls into pixel "<<x<<","<<y<<".\n";
255 
256  // Do some minor interpolation by checking the three closest neighbors to the point, that are not filled yet.
257  int floor_x = pcl_lrint (floor (x_real)), floor_y = pcl_lrint (floor (y_real)),
258  ceil_x = pcl_lrint (ceil (x_real)), ceil_y = pcl_lrint (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 = (pcl_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 (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 (pcl_isinf (image_point_range))
414  {
415  if (image_point_range > 0.0f)
416  return std::numeric_limits<float>::infinity ();
417  else
418  return -std::numeric_limits<float>::infinity ();
419  }
420  return image_point_range - range;
421 }
422 
423 /////////////////////////////////////////////////////////////////////////
424 void
425 RangeImage::getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const
426 {
427  image_x = (angle_x*cosLookUp (angle_y) + static_cast<float> (M_PI))*angular_resolution_x_reciprocal_ - static_cast<float> (image_offset_x_);
428  image_y = (angle_y + 0.5f*static_cast<float> (M_PI))*angular_resolution_y_reciprocal_ - static_cast<float> (image_offset_y_);
429 }
430 
431 /////////////////////////////////////////////////////////////////////////
432 void
433 RangeImage::real2DToInt2D (float x, float y, int& xInt, int& yInt) const
434 {
435  xInt = static_cast<int> (pcl_lrintf (x));
436  yInt = static_cast<int> (pcl_lrintf (y));
437 }
438 
439 /////////////////////////////////////////////////////////////////////////
440 bool
441 RangeImage::isInImage (int x, int y) const
442 {
443  return (x >= 0 && x < static_cast<int> (width) && y >= 0 && y < static_cast<int> (height));
444 }
445 
446 /////////////////////////////////////////////////////////////////////////
447 bool
448 RangeImage::isValid (int x, int y) const
449 {
450  return isInImage (x,y) && pcl_isfinite (getPoint (x,y).range);
451 }
452 
453 /////////////////////////////////////////////////////////////////////////
454 bool
455 RangeImage::isValid (int index) const
456 {
457  return pcl_isfinite (getPoint (index).range);
458 }
459 
460 /////////////////////////////////////////////////////////////////////////
461 bool
462 RangeImage::isObserved (int x, int y) const
463 {
464  if (!isInImage (x,y) || (pcl_isinf (getPoint (x,y).range)&&getPoint (x,y).range<0.0f))
465  return false;
466  return true;
467 }
468 
469 /////////////////////////////////////////////////////////////////////////
470 bool
471 RangeImage::isMaxRange (int x, int y) const
472 {
473  float range = getPoint (x,y).range;
474  return pcl_isinf (range) && range>0.0f;
475 }
476 
477 /////////////////////////////////////////////////////////////////////////
478 const PointWithRange&
479 RangeImage::getPoint (int image_x, int image_y) const
480 {
481  if (!isInImage (image_x, image_y))
482  return unobserved_point;
483  return points[image_y*width + image_x];
484 }
485 
486 /////////////////////////////////////////////////////////////////////////
487 const PointWithRange&
488 RangeImage::getPointNoCheck (int image_x, int image_y) const
489 {
490  return points[image_y*width + image_x];
491 }
492 
493 /////////////////////////////////////////////////////////////////////////
495 RangeImage::getPointNoCheck (int image_x, int image_y)
496 {
497  return points[image_y*width + image_x];
498 }
499 
500 /////////////////////////////////////////////////////////////////////////
502 RangeImage::getPoint (int image_x, int image_y)
503 {
504  return points[image_y*width + image_x];
505 }
506 
507 
508 /////////////////////////////////////////////////////////////////////////
509 const PointWithRange&
510 RangeImage::getPoint (int index) const
511 {
512  return points[index];
513 }
514 
515 /////////////////////////////////////////////////////////////////////////
516 const PointWithRange&
517 RangeImage::getPoint (float image_x, float image_y) const
518 {
519  int x, y;
520  real2DToInt2D (image_x, image_y, x, y);
521  return getPoint (x, y);
522 }
523 
524 /////////////////////////////////////////////////////////////////////////
526 RangeImage::getPoint (float image_x, float image_y)
527 {
528  int x, y;
529  real2DToInt2D (image_x, image_y, x, y);
530  return getPoint (x, y);
531 }
532 
533 /////////////////////////////////////////////////////////////////////////
534 void
535 RangeImage::getPoint (int image_x, int image_y, Eigen::Vector3f& point) const
536 {
537  //std::cout << getPoint (image_x, image_y)<< " - " << getPoint (image_x, image_y).getVector3fMap ()<<"\n";
538  point = getPoint (image_x, image_y).getVector3fMap ();
539 }
540 
541 /////////////////////////////////////////////////////////////////////////
542 void
543 RangeImage::getPoint (int index, Eigen::Vector3f& point) const
544 {
545  point = getPoint (index).getVector3fMap ();
546 }
547 
548 /////////////////////////////////////////////////////////////////////////
549 const Eigen::Map<const Eigen::Vector3f>
550 RangeImage::getEigenVector3f (int x, int y) const
551 {
552  return getPoint (x, y).getVector3fMap ();
553 }
554 
555 /////////////////////////////////////////////////////////////////////////
556 const Eigen::Map<const Eigen::Vector3f>
558 {
559  return getPoint (index).getVector3fMap ();
560 }
561 
562 /////////////////////////////////////////////////////////////////////////
563 void
564 RangeImage::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const
565 {
566  float angle_x, angle_y;
567  //std::cout << image_x<<","<<image_y<<","<<range;
568  getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y);
569 
570  float cosY = cosf (angle_y);
571  point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * cosf (angle_x)*cosY);
572  point = to_world_system_ * point;
573 }
574 
575 /////////////////////////////////////////////////////////////////////////
576 void
577 RangeImage::calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const
578 {
579  const PointWithRange& point_in_image = getPoint (image_x, image_y);
580  calculate3DPoint (image_x, image_y, point_in_image.range, point);
581 }
582 
583 /////////////////////////////////////////////////////////////////////////
584 void
585 RangeImage::calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const {
586  point.range = range;
587  Eigen::Vector3f tmp_point;
588  calculate3DPoint (image_x, image_y, range, tmp_point);
589  point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
590 }
591 
592 /////////////////////////////////////////////////////////////////////////
593 void
594 RangeImage::calculate3DPoint (float image_x, float image_y, PointWithRange& point) const
595 {
596  const PointWithRange& point_in_image = getPoint (image_x, image_y);
597  calculate3DPoint (image_x, image_y, point_in_image.range, point);
598 }
599 
600 /////////////////////////////////////////////////////////////////////////
601 void
602 RangeImage::getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const
603 {
604  angle_y = (image_y+static_cast<float> (image_offset_y_))*angular_resolution_y_ - 0.5f*static_cast<float> (M_PI);
605  float cos_angle_y = cosf (angle_y);
606  angle_x = (cos_angle_y==0.0f ? 0.0f : ( (image_x+ static_cast<float> (image_offset_x_))*angular_resolution_x_ - static_cast<float> (M_PI))/cos_angle_y);
607 }
608 
609 /////////////////////////////////////////////////////////////////////////
610 float
611 RangeImage::getImpactAngle (int x1, int y1, int x2, int y2) const
612 {
613  if (!isInImage (x1, y1) || !isInImage (x2,y2))
614  return -std::numeric_limits<float>::infinity ();
615  return getImpactAngle (getPoint (x1,y1),getPoint (x2,y2));
616 }
617 
618 /////////////////////////////////////////////////////////////////////////
619 float
620 RangeImage::getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const {
621  if ( (pcl_isinf (point1.range)&&point1.range<0) || (pcl_isinf (point2.range)&&point2.range<0))
622  return -std::numeric_limits<float>::infinity ();
623 
624  float r1 = (std::min) (point1.range, point2.range),
625  r2 = (std::max) (point1.range, point2.range);
626  float impact_angle = static_cast<float> (0.5f * M_PI);
627 
628  if (pcl_isinf (r2))
629  {
630  if (r2 > 0.0f && !pcl_isinf (r1))
631  impact_angle = 0.0f;
632  }
633  else if (!pcl_isinf (r1))
634  {
635  float r1Sqr = r1*r1,
636  r2Sqr = r2*r2,
637  dSqr = squaredEuclideanDistance (point1, point2),
638  d = std::sqrt (dSqr);
639  float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
640  cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
641  impact_angle = acosf (cos_impact_angle); // Using the cosine rule
642  }
643 
644  if (point1.range > point2.range)
645  impact_angle = -impact_angle;
646 
647  return impact_angle;
648 }
649 
650 /////////////////////////////////////////////////////////////////////////
651 float
652 RangeImage::getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const
653 {
654  float impact_angle = getImpactAngle (point1, point2);
655  if (pcl_isinf (impact_angle))
656  return -std::numeric_limits<float>::infinity ();
657  float ret = 1.0f - float (fabs (impact_angle)/ (0.5f*M_PI));
658  if (impact_angle < 0.0f)
659  ret = -ret;
660  //if (fabs (ret)>1)
661  //std::cout << PVARAC (impact_angle)<<PVARN (ret);
662  return ret;
663 }
664 
665 /////////////////////////////////////////////////////////////////////////
666 float
667 RangeImage::getAcutenessValue (int x1, int y1, int x2, int y2) const
668 {
669  if (!isInImage (x1, y1) || !isInImage (x2,y2))
670  return -std::numeric_limits<float>::infinity ();
671  return getAcutenessValue (getPoint (x1,y1), getPoint (x2,y2));
672 }
673 
674 /////////////////////////////////////////////////////////////////////////
675 const Eigen::Vector3f
677 {
678  return Eigen::Vector3f (to_world_system_ (0,3), to_world_system_ (1,3), to_world_system_ (2,3));
679 }
680 
681 /////////////////////////////////////////////////////////////////////////
682 void
683 RangeImage::getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const
684 {
685  angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
686  if (!isValid (x,y))
687  return;
688  Eigen::Vector3f point;
689  getPoint (x, y, point);
690  Eigen::Affine3f transformation = getTransformationToViewerCoordinateFrame (point);
691 
692  if (isObserved (x-radius, y) && isObserved (x+radius, y))
693  {
694  Eigen::Vector3f transformed_left;
695  if (isMaxRange (x-radius, y))
696  transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
697  else
698  {
699  Eigen::Vector3f left;
700  getPoint (x-radius, y, left);
701  transformed_left = - (transformation * left);
702  //std::cout << PVARN (transformed_left[1]);
703  transformed_left[1] = 0.0f;
704  transformed_left.normalize ();
705  }
706 
707  Eigen::Vector3f transformed_right;
708  if (isMaxRange (x+radius, y))
709  transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
710  else
711  {
712  Eigen::Vector3f right;
713  getPoint (x+radius, y, right);
714  transformed_right = transformation * right;
715  //std::cout << PVARN (transformed_right[1]);
716  transformed_right[1] = 0.0f;
717  transformed_right.normalize ();
718  }
719  angle_change_x = transformed_left.dot (transformed_right);
720  angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
721  angle_change_x = acosf (angle_change_x);
722  }
723 
724  if (isObserved (x, y-radius) && isObserved (x, y+radius))
725  {
726  Eigen::Vector3f transformed_top;
727  if (isMaxRange (x, y-radius))
728  transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
729  else
730  {
731  Eigen::Vector3f top;
732  getPoint (x, y-radius, top);
733  transformed_top = - (transformation * top);
734  //std::cout << PVARN (transformed_top[0]);
735  transformed_top[0] = 0.0f;
736  transformed_top.normalize ();
737  }
738 
739  Eigen::Vector3f transformed_bottom;
740  if (isMaxRange (x, y+radius))
741  transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
742  else
743  {
744  Eigen::Vector3f bottom;
745  getPoint (x, y+radius, bottom);
746  transformed_bottom = transformation * bottom;
747  //std::cout << PVARN (transformed_bottom[0]);
748  transformed_bottom[0] = 0.0f;
749  transformed_bottom.normalize ();
750  }
751  angle_change_y = transformed_top.dot (transformed_bottom);
752  angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
753  angle_change_y = acosf (angle_change_y);
754  }
755 }
756 
757 
758 //inline float RangeImage::getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, const PointWithRange& neighbor2) const
759 //{
760  //if (!pcl_isfinite (point.range) || (!pcl_isfinite (neighbor1.range)&&neighbor1.range<0) || (!pcl_isfinite (neighbor2.range)&&neighbor2.range<0))
761  //return -std::numeric_limits<float>::infinity ();
762  //if (pcl_isinf (neighbor1.range))
763  //{
764  //if (pcl_isinf (neighbor2.range))
765  //return 0.0f;
766  //else
767  //return acosf ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor2.x, neighbor2.y, neighbor2.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ()));
768  //}
769  //if (pcl_isinf (neighbor2.range))
770  //return acosf ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor1.x, neighbor1.y, neighbor1.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ()));
771 
772  //float d1_squared = squaredEuclideanDistance (point, neighbor1),
773  //d1 = std::sqrt (d1_squared),
774  //d2_squared = squaredEuclideanDistance (point, neighbor2),
775  //d2 = std::sqrt (d2_squared),
776  //d3_squared = squaredEuclideanDistance (neighbor1, neighbor2);
777  //float cos_surface_change = (d1_squared + d2_squared - d3_squared)/ (2.0f*d1*d2),
778  //surface_change = acosf (cos_surface_change);
779  //if (pcl_isnan (surface_change))
780  //surface_change = static_cast<float> (M_PI);
781  ////std::cout << PVARN (point)<<PVARN (neighbor1)<<PVARN (neighbor2)<<PVARN (cos_surface_change)<<PVARN (surface_change)<<PVARN (d1)<<PVARN (d2)<<PVARN (d1_squared)<<PVARN (d2_squared)<<PVARN (d3_squared);
782 
783  //return surface_change;
784 //}
785 
786 /////////////////////////////////////////////////////////////////////////
787 float
788 RangeImage::getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, float radius)
789 {
790  return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
791 }
792 
793 /////////////////////////////////////////////////////////////////////////
794 Eigen::Vector3f
796 {
797  return Eigen::Vector3f (point.x, point.y, point.z);
798 }
799 
800 /////////////////////////////////////////////////////////////////////////
801 void
802 RangeImage::get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange& average_point) const
803 {
804  //std::cout << __PRETTY_FUNCTION__<<" called.\n";
805  //MEASURE_FUNCTION_TIME;
806  float weight_sum = 1.0f;
807  average_point = getPoint (x,y);
808  if (pcl_isinf (average_point.range))
809  {
810  if (average_point.range>0.0f) // The first point is max range -> return a max range point
811  return;
812  weight_sum = 0.0f;
813  average_point.x = average_point.y = average_point.z = average_point.range = 0.0f;
814  }
815 
816  int x2=x, y2=y;
817  Vector4fMap average_point_eigen = average_point.getVector4fMap ();
818  //std::cout << PVARN (no_of_points);
819  for (int step=1; step<no_of_points; ++step)
820  {
821  //std::cout << PVARC (step);
822  x2+=delta_x; y2+=delta_y;
823  if (!isValid (x2, y2))
824  continue;
825  const PointWithRange& p = getPointNoCheck (x2, y2);
826  average_point_eigen+=p.getVector4fMap (); average_point.range+=p.range;
827  weight_sum += 1.0f;
828  }
829  if (weight_sum<= 0.0f)
830  {
831  average_point = unobserved_point;
832  return;
833  }
834  float normalization_factor = 1.0f/weight_sum;
835  average_point_eigen *= normalization_factor;
836  average_point.range *= normalization_factor;
837  //std::cout << PVARN (average_point);
838 }
839 
840 /////////////////////////////////////////////////////////////////////////
841 float
842 RangeImage::getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const
843 {
844  if (!isObserved (x1,y1)||!isObserved (x2,y2))
845  return -std::numeric_limits<float>::infinity ();
846  const PointWithRange& point1 = getPoint (x1,y1),
847  & point2 = getPoint (x2,y2);
848  if (pcl_isinf (point1.range) && pcl_isinf (point2.range))
849  return 0.0f;
850  if (pcl_isinf (point1.range) || pcl_isinf (point2.range))
851  return std::numeric_limits<float>::infinity ();
852  return squaredEuclideanDistance (point1, point2);
853 }
854 
855 /////////////////////////////////////////////////////////////////////////
856 float
857 RangeImage::getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const
858 {
859  float average_pixel_distance = 0.0f;
860  float weight=0.0f;
861  for (int i=0; i<max_steps; ++i)
862  {
863  int x1=x+i*offset_x, y1=y+i*offset_y;
864  int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y;
865  float pixel_distance = getEuclideanDistanceSquared (x1,y1,x2,y2);
866  if (!pcl_isfinite (pixel_distance))
867  {
868  //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<pixel_distance<<"\n";
869  if (i==0)
870  return pixel_distance;
871  else
872  break;
873  }
874  //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<std::sqrt (pixel_distance)<<"m\n";
875  weight += 1.0f;
876  average_pixel_distance += std::sqrt (pixel_distance);
877  }
878  average_pixel_distance /= weight;
879  //std::cout << x<<","<<y<<","<<offset_x<<","<<offset_y<<" => "<<average_pixel_distance<<"\n";
880  return average_pixel_distance;
881 }
882 
883 /////////////////////////////////////////////////////////////////////////
884 float
885 RangeImage::getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const
886 {
887  if (!isValid (x,y))
888  return -std::numeric_limits<float>::infinity ();
889  const PointWithRange& point = getPoint (x, y);
890  int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> ( (radius + 1.0)), 2.0));
891  Eigen::Vector3f normal;
892  if (!getNormalForClosestNeighbors (x, y, radius, point, no_of_nearest_neighbors, normal, 1))
893  return -std::numeric_limits<float>::infinity ();
894  return deg2rad (90.0f) - acosf (normal.dot ( (getSensorPos ()-getEigenVector3f (point)).normalized ()));
895 }
896 
897 
898 /////////////////////////////////////////////////////////////////////////
899 bool
900 RangeImage::getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size) const
901 {
902  VectorAverage3f vector_average;
903  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
904  {
905  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
906  {
907  if (!isInImage (x2, y2))
908  continue;
909  const PointWithRange& point = getPoint (x2, y2);
910  if (!pcl_isfinite (point.range))
911  continue;
912  vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
913  }
914  }
915  if (vector_average.getNoOfSamples () < 3)
916  return false;
917  Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
918  vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
919  if (normal.dot ( (getSensorPos ()-vector_average.getMean ()).normalized ()) < 0.0f)
920  normal *= -1.0f;
921  return true;
922 }
923 
924 /////////////////////////////////////////////////////////////////////////
925 float
926 RangeImage::getNormalBasedAcutenessValue (int x, int y, int radius) const
927 {
928  float impact_angle = getImpactAngleBasedOnLocalNormal (x, y, radius);
929  if (pcl_isinf (impact_angle))
930  return -std::numeric_limits<float>::infinity ();
931  float ret = 1.0f - static_cast<float> ( (impact_angle / (0.5f * M_PI)));
932  //std::cout << PVARAC (impact_angle)<<PVARN (ret);
933  return ret;
934 }
935 
936 /////////////////////////////////////////////////////////////////////////
937 bool
938 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
939  int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size) const
940 {
941  return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal, NULL, step_size);
942 }
943 
944 /////////////////////////////////////////////////////////////////////////
945 bool
946 RangeImage::getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius) const
947 {
948  if (!isValid (x,y))
949  return false;
950  int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> (radius + 1.0), 2.0));
951  return getNormalForClosestNeighbors (x, y, radius, getPoint (x,y).getVector3fMap (), no_of_nearest_neighbors, normal);
952 }
953 
954 namespace
955 { // Anonymous namespace, so that this is only accessible in this file
956  struct NeighborWithDistance
957  { // local struct to help us with sorting
958  float distance;
959  const PointWithRange* neighbor;
960  bool operator < (const NeighborWithDistance& other) const { return distance<other.distance;}
961  };
962 }
963 
964 /////////////////////////////////////////////////////////////////////////
965 bool
966 RangeImage::getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_closest_neighbors, int step_size,
967  float& max_closest_neighbor_distance_squared,
968  Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
969  Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
970  Eigen::Vector3f* eigen_values_all_neighbors) const
971 {
972  max_closest_neighbor_distance_squared=0.0f;
973  normal.setZero (); mean.setZero (); eigen_values.setZero ();
974  if (normal_all_neighbors!=NULL)
975  normal_all_neighbors->setZero ();
976  if (mean_all_neighbors!=NULL)
977  mean_all_neighbors->setZero ();
978  if (eigen_values_all_neighbors!=NULL)
979  eigen_values_all_neighbors->setZero ();
980 
981  int blocksize = static_cast<int> (pow (static_cast<double> ( (2.0 * radius + 1.0)), 2.0));
982 
983  PointWithRange given_point;
984  given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
985 
986  std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
987  int neighbor_counter = 0;
988  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
989  {
990  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
991  {
992  if (!isValid (x2, y2))
993  continue;
994  NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
995  neighbor_with_distance.neighbor = &getPoint (x2, y2);
996  neighbor_with_distance.distance = squaredEuclideanDistance (given_point, *neighbor_with_distance.neighbor);
997  ++neighbor_counter;
998  }
999  }
1000  no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
1001 
1002  std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter); // Normal sort seems to be the fastest method (faster than partial_sort)
1003  //std::stable_sort (ordered_neighbors, ordered_neighbors+neighbor_counter);
1004  //std::partial_sort (ordered_neighbors, ordered_neighbors+no_of_closest_neighbors, ordered_neighbors+neighbor_counter);
1005 
1006  max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1007  //float max_distance_squared = max_closest_neighbor_distance_squared;
1008  float max_distance_squared = max_closest_neighbor_distance_squared*4.0f; // Double the allowed distance value
1009  //max_closest_neighbor_distance_squared = max_distance_squared;
1010 
1011  VectorAverage3f vector_average;
1012  //for (int neighbor_idx=0; neighbor_idx<no_of_closest_neighbors; ++neighbor_idx)
1013  int neighbor_idx;
1014  for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1015  {
1016  if (ordered_neighbors[neighbor_idx].distance > max_distance_squared)
1017  break;
1018  //std::cout << ordered_neighbors[neighbor_idx].distance<<"\n";
1019  vector_average.add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1020  }
1021 
1022  if (vector_average.getNoOfSamples () < 3)
1023  return false;
1024  //std::cout << PVARN (vector_average.getNoOfSamples ());
1025  Eigen::Vector3f eigen_vector2, eigen_vector3;
1026  vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1027  Eigen::Vector3f viewing_direction = (getSensorPos ()-point).normalized ();
1028  if (normal.dot (viewing_direction) < 0.0f)
1029  normal *= -1.0f;
1030  mean = vector_average.getMean ();
1031 
1032  if (normal_all_neighbors==NULL)
1033  return true;
1034 
1035  // Add remaining neighbors
1036  for (int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1037  vector_average.add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1038 
1039  vector_average.doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1040  //std::cout << PVARN (vector_average.getNoOfSamples ())<<".\n";
1041  if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1042  *normal_all_neighbors *= -1.0f;
1043  *mean_all_neighbors = vector_average.getMean ();
1044 
1045  //std::cout << viewing_direction[0]<<","<<viewing_direction[1]<<","<<viewing_direction[2]<<"\n";
1046 
1047  return true;
1048 }
1049 
1050 /////////////////////////////////////////////////////////////////////////
1051 float
1052 RangeImage::getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const
1053 {
1054  const PointWithRange& point = getPoint (x, y);
1055  if (!pcl_isfinite (point.range))
1056  return -std::numeric_limits<float>::infinity ();
1057 
1058  int blocksize = static_cast<int> (pow (static_cast<double> (2.0 * radius + 1.0), 2.0));
1059  std::vector<float> neighbor_distances (blocksize);
1060  int neighbor_counter = 0;
1061  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1062  {
1063  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1064  {
1065  if (!isValid (x2, y2) || (x2==x&&y2==y))
1066  continue;
1067  const PointWithRange& neighbor = getPointNoCheck (x2,y2);
1068  float& neighbor_distance = neighbor_distances[neighbor_counter++];
1069  neighbor_distance = squaredEuclideanDistance (point, neighbor);
1070  }
1071  }
1072  std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter); // Normal sort seems to be
1073  // the fastest method (faster than partial_sort)
1074  n = (std::min) (neighbor_counter, n);
1075  return neighbor_distances[n-1];
1076 }
1077 
1078 
1079 /////////////////////////////////////////////////////////////////////////
1080 bool
1081 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_nearest_neighbors,
1082  Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane, int step_size) const
1083 {
1084  Eigen::Vector3f mean, eigen_values;
1085  float used_squared_max_distance;
1086  bool ret = getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1087  normal, mean, eigen_values);
1088 
1089  if (ret)
1090  {
1091  if (point_on_plane != NULL)
1092  *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1093  }
1094  return ret;
1095 }
1096 
1097 
1098 /////////////////////////////////////////////////////////////////////////
1099 float
1100 RangeImage::getCurvature (int x, int y, int radius, int step_size) const
1101 {
1102  VectorAverage3f vector_average;
1103  for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1104  {
1105  for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1106  {
1107  if (!isInImage (x2, y2))
1108  continue;
1109  const PointWithRange& point = getPoint (x2, y2);
1110  if (!pcl_isfinite (point.range))
1111  continue;
1112  vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
1113  }
1114  }
1115  if (vector_average.getNoOfSamples () < 3)
1116  return false;
1117  Eigen::Vector3f eigen_values;
1118  vector_average.doPCA (eigen_values);
1119  return eigen_values[0]/eigen_values.sum ();
1120 }
1121 
1122 
1123 /////////////////////////////////////////////////////////////////////////
1124 template <typename PointCloudTypeWithViewpoints> Eigen::Vector3f
1125 RangeImage::getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud)
1126 {
1127  Eigen::Vector3f average_viewpoint (0,0,0);
1128  int point_counter = 0;
1129  for (unsigned int point_idx=0; point_idx<point_cloud.points.size (); ++point_idx)
1130  {
1131  const typename PointCloudTypeWithViewpoints::PointType& point = point_cloud.points[point_idx];
1132  if (!pcl_isfinite (point.vp_x) || !pcl_isfinite (point.vp_y) || !pcl_isfinite (point.vp_z))
1133  continue;
1134  average_viewpoint[0] += point.vp_x;
1135  average_viewpoint[1] += point.vp_y;
1136  average_viewpoint[2] += point.vp_z;
1137  ++point_counter;
1138  }
1139  average_viewpoint /= point_counter;
1140 
1141  return average_viewpoint;
1142 }
1143 
1144 /////////////////////////////////////////////////////////////////////////
1145 bool
1146 RangeImage::getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const
1147 {
1148  if (!isValid (x, y))
1149  return false;
1150  viewing_direction = (getPoint (x,y).getVector3fMap ()-getSensorPos ()).normalized ();
1151  return true;
1152 }
1153 
1154 /////////////////////////////////////////////////////////////////////////
1155 void
1156 RangeImage::getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const
1157 {
1158  viewing_direction = (point-getSensorPos ()).normalized ();
1159 }
1160 
1161 /////////////////////////////////////////////////////////////////////////
1162 Eigen::Affine3f
1163 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const
1164 {
1165  Eigen::Affine3f transformation;
1166  getTransformationToViewerCoordinateFrame (point, transformation);
1167  return transformation;
1168 }
1169 
1170 /////////////////////////////////////////////////////////////////////////
1171 void
1172 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1173 {
1174  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1175  getTransformationFromTwoUnitVectorsAndOrigin (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, point, transformation);
1176 }
1177 
1178 /////////////////////////////////////////////////////////////////////////
1179 void
1180 RangeImage::getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1181 {
1182  Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1183  getTransformationFromTwoUnitVectors (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, transformation);
1184 }
1185 
1186 /////////////////////////////////////////////////////////////////////////
1187 inline void
1188 RangeImage::setAngularResolution (float angular_resolution)
1189 {
1190  angular_resolution_x_ = angular_resolution_y_ = angular_resolution;
1192 }
1193 
1194 /////////////////////////////////////////////////////////////////////////
1195 inline void
1196 RangeImage::setAngularResolution (float angular_resolution_x, float angular_resolution_y)
1197 {
1198  angular_resolution_x_ = angular_resolution_x;
1200  angular_resolution_y_ = angular_resolution_y;
1202 }
1203 
1204 /////////////////////////////////////////////////////////////////////////
1205 inline void
1206 RangeImage::setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system)
1207 {
1208  to_range_image_system_ = to_range_image_system;
1210 }
1211 
1212 /////////////////////////////////////////////////////////////////////////
1213 inline void
1214 RangeImage::getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const
1215 {
1216  angular_resolution_x = angular_resolution_x_;
1217  angular_resolution_y = angular_resolution_y_;
1218 }
1219 
1220 /////////////////////////////////////////////////////////////////////////
1221 template <typename PointCloudType> void
1222 RangeImage::integrateFarRanges (const PointCloudType& far_ranges)
1223 {
1224  float x_real, y_real, range_of_current_point;
1225  for (typename PointCloudType::const_iterator it = far_ranges.points.begin (); it != far_ranges.points.end (); ++it)
1226  {
1227  //if (!isFinite (*it)) // Check for NAN etc
1228  //continue;
1229  Vector3fMapConst current_point = it->getVector3fMap ();
1230 
1231  this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
1232 
1233  int floor_x = static_cast<int> (pcl_lrint (floor (x_real))),
1234  floor_y = static_cast<int> (pcl_lrint (floor (y_real))),
1235  ceil_x = static_cast<int> (pcl_lrint (ceil (x_real))),
1236  ceil_y = static_cast<int> (pcl_lrint (ceil (y_real)));
1237 
1238  int neighbor_x[4], neighbor_y[4];
1239  neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1240  neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1241  neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1242  neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1243 
1244  for (int i=0; i<4; ++i)
1245  {
1246  int x=neighbor_x[i], y=neighbor_y[i];
1247  if (!isInImage (x, y))
1248  continue;
1249  PointWithRange& image_point = getPoint (x, y);
1250  if (!pcl_isfinite (image_point.range))
1251  image_point.range = std::numeric_limits<float>::infinity ();
1252  }
1253  }
1254 }
1255 
1256 } // namespace end
1257 #endif
1258 
float getImpactAngleBasedOnLocalNormal(int x, int y, int radius) const
Extract a local normal (with a heuristic not to include background points) and calculate the impact a...
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
void createFromPointCloudWithViewpoints(const PointCloudTypeWithViewpoints &point_cloud, float angular_resolution, float max_angle_width, float max_angle_height, CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud, using the average viewpoint of the points (vp_x...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:54
float angular_resolution_x_
Angular resolution of the range image in x direction in radians per pixel.
Definition: range_image.h:772
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
void getSurfaceAngleChange(int x, int y, int radius, float &angle_change_x, float &angle_change_y) const
Calculates, how much the surface changes at a point.
A point structure representing Euclidean xyz coordinates, padded with an extra range float...
std::vector< PointWithRange, Eigen::aligned_allocator< PointWithRange > > points
The point data.
Definition: point_cloud.h:410
float getEuclideanDistanceSquared(int x1, int y1, int x2, int y2) const
Get the squared euclidean distance between the two image points.
Eigen::Affine3f getTransformationToViewerCoordinateFrame(const Eigen::Vector3f &point) const
Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction.
float getAverageEuclideanDistance(int x, int y, int offset_x, int offset_y, int max_steps) const
Doing the above for some steps in the given direction and averaging.
void doPCA(Eigen::Matrix< real, dimension, 1 > &eigen_values, Eigen::Matrix< real, dimension, 1 > &eigen_vector1, Eigen::Matrix< real, dimension, 1 > &eigen_vector2, Eigen::Matrix< real, dimension, 1 > &eigen_vector3) const
Do Principal component analysis.
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h: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:778
bool getViewingDirection(int x, int y, Eigen::Vector3f &viewing_direction) const
Get the viewing direction for the given point.
void getTransformationFromTwoUnitVectorsAndOrigin(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, const Eigen::Vector3f &origin, Eigen::Affine3f &transformation)
Get the transformation that will translate origin to (0,0,0) and rotate z_axis into (0...
Definition: eigen.hpp:652
iterator end()
Definition: point_cloud.h:443
bool getNormalForClosestNeighbors(int x, int y, int radius, const PointWithRange &point, int no_of_nearest_neighbors, Eigen::Vector3f &normal, int step_size=1) const
Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered...
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:426
void getTransformationFromTwoUnitVectors(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate z_axis into (0,0,1) and y_direction into a vector with x=...
Definition: eigen.hpp:634
const Eigen::Matrix< real, dimension, 1 > & getMean() const
Get the mean of the added vectors.
float getNormalBasedAcutenessValue(int x, int y, int radius) const
Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) This u...
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
virtual void getImagePoint(const Eigen::Vector3f &point, float &image_x, float &image_y, float &range) const
Get imagePoint from 3D point in world coordinates.
bool getNormal(int x, int y, int radius, Eigen::Vector3f &normal, int step_size=1) const
Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius...
void createFromPointCloudWithKnownSize(const PointCloudType &point_cloud, float angular_resolution, const Eigen::Vector3f &point_cloud_center, float point_cloud_radius, const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud, getting a hint about the size of the scene for faster calc...
static PCL_EXPORTS void getCoordinateFrameTransformation(RangeImage::CoordinateFrame coordinate_frame, Eigen::Affine3f &transformation)
Get the transformation that transforms the given coordinate frame into CAMERA_FRAME.
void real2DToInt2D(float x, float y, int &xInt, int &yInt) const
Transforms an image point in float values to an image point in int values.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:174
static const int lookup_table_size
Definition: range_image.h:787
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
void add(const Eigen::Matrix< real, dimension, 1 > &sample, real weight=1.0)
Add a new sample.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void getAnglesFromImagePoint(float image_x, float image_y, float &angle_x, float &angle_y) const
Get the angles corresponding to the given image point.
void calculate3DPoint(float image_x, float image_y, float range, PointWithRange &point) const
Calculate the 3D point according to the given image point and range.
void setTransformationToRangeImageSystem(const Eigen::Affine3f &to_range_image_system)
Setter for the transformation from the range image system (the sensor coordinate frame) into the worl...
float getImpactAngle(const PointWithRange &point1, const PointWithRange &point2) const
Calculate the impact angle based on the sensor position and the two given points - will return -INFIN...
static float getMaxAngleSize(const Eigen::Affine3f &viewer_pose, const Eigen::Vector3f &center, float radius)
Get the size of a certain area when seen from the given pose.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
bool isInImage(int x, int y) const
Check if a point is inside of the image.
unsigned int getNoOfSamples()
Get the number of added vectors.
float getCurvature(int x, int y, int radius, int step_size) const
Calculates the curvature in a point using pca.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
static std::vector< float > cos_lookup_table
Definition: range_image.h:790
void get1dPointAverage(int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange &average_point) const
Calculates the average 3D position of the no_of_points points described by the start point x...
void doZBuffer(const PointCloudType &point_cloud, float noise_level, float min_range, int &top, int &right, int &bottom, int &left)
Integrate the given point cloud into the current range image using a z-buffer.
void getRotationToViewerCoordinateFrame(const Eigen::Vector3f &point, Eigen::Affine3f &transformation) const
Same as above, but only returning the rotation.
static float atan2LookUp(float y, float x)
Query the atan2 lookup table.
Definition: range_image.hpp:60
void createFromPointCloud(const PointCloudType &point_cloud, float angular_resolution=pcl::deg2rad(0.5f), float max_angle_width=pcl::deg2rad(360.0f), float max_angle_height=pcl::deg2rad(180.0f), const Eigen::Affine3f &sensor_pose=Eigen::Affine3f::Identity(), CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, float min_range=0.0f, int border_size=0)
Create the depth image from a point cloud.
Definition: range_image.hpp:94
static std::vector< float > asin_lookup_table
Definition: range_image.h:788
PCL_EXPORTS void recalculate3DPointPositions()
Recalculate all 3D point positions according to their pixel position and range.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performance of multiplication compared to division ...
Definition: range_image.h:776
void getImagePointFromAngles(float angle_x, float angle_y, float &image_x, float &image_y) const
Get the image point corresponding to the given angles.
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performance of multiplication compared to division ...
Definition: range_image.h:774
Eigen::Affine3f to_range_image_system_
Inverse of to_world_system_.
Definition: range_image.h:770
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=NULL, Eigen::Vector3f *mean_all_neighbors=NULL, Eigen::Vector3f *eigen_values_all_neighbors=NULL) const
Same as above but extracts some more data and can also return the extracted information for all neigh...
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
Definition: range_image.h:771
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
static Eigen::Vector3f getAverageViewPoint(const PointCloudTypeWithViewpoints &point_cloud)
Get the average viewpoint of a point cloud where each point carries viewpoint information as vp_x...
void setAngularResolution(float angular_resolution)
Set the angular resolution of the range image.
const PointWithRange & getPointNoCheck(int image_x, int image_y) const
Return the 3D point with range at the given image position.
bool isObserved(int x, int y) const
Check if a point is inside of the image and has either a finite range or a max reading (range=INFINIT...
iterator begin()
Definition: point_cloud.h:442
float getRangeDifference(const Eigen::Vector3f &point) const
Returns the difference in range between the given point and the range of the point in the image at th...
static float cosLookUp(float value)
Query the cos lookup table.
Definition: range_image.hpp:86
static float asinLookUp(float value)
Query the asin lookup table.
Definition: range_image.hpp:50
float angular_resolution_y_
Angular resolution of the range image in y direction in radians per pixel.
Definition: range_image.h:773
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel...
Definition: range_image.h:354
float getSquaredDistanceOfNthNeighbor(int x, int y, int radius, int n, int step_size) const
Calculates the weighted average and the covariance matrix.
static std::vector< float > atan_lookup_table
Definition: range_image.h:789
PointWithRange unobserved_point
This point is used to be able to return a reference to a non-existing point.
Definition: range_image.h:780
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! ...
PCL_EXPORTS void cropImage(int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1)
Cut the range image to the minimal size so that it still contains all actual range readings...
float checkPoint(const Eigen::Vector3f &point, PointWithRange &point_in_image) const
point_in_image will be the point in the image at the position the given point would be...
float getAcutenessValue(const PointWithRange &point1, const PointWithRange &point2) const
Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) will r...
void integrateFarRanges(const PointCloudType &far_ranges)
Integrates the given far range measurements into the range image.
VectorType::const_iterator const_iterator
Definition: point_cloud.h:441