Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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 ();
197 points.resize (static_cast<std::size_t>(width)*static_cast<std::size_t>(height), unobserved_point);
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 - static_cast<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 {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 {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).
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
std::vector< PointWithRange, Eigen::aligned_allocator< PointWithRange > > points
The point data.
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...
static float atan2LookUp(float y, float x)
Query the std::atan2 lookup table.
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.
static Eigen::Vector3f getEigenVector3f(const PointWithRange &point)
Get Eigen::Vector3f from PointWithRange.
Eigen::Affine3f to_world_system_
Inverse of to_range_image_system_.
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.
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.
float getAngularResolution() const
Getter for the angular resolution of the range image in x direction in radians per pixel.
static PCL_EXPORTS std::vector< float > cos_lookup_table
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.
float angular_resolution_y_reciprocal_
1.0/angular_resolution_y_ - provided for better performance of multiplication compared to division
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.
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.
static PCL_EXPORTS std::vector< float > atan_lookup_table
static PCL_EXPORTS std::vector< float > asin_lookup_table
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...
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.
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.
static PCL_EXPORTS const int lookup_table_size
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_.
float angular_resolution_x_reciprocal_
1.0/angular_resolution_x_ - provided for better performance of multiplication compared to division
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.
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:583
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:564
VectorAverage< float, 3 > VectorAverage3f
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:254
#define pcl_lrintf(x)
Definition pcl_macros.h:255
#define M_PI
Definition pcl_macros.h:203
A point structure representing Euclidean xyz coordinates, padded with an extra range float.