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