1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 */
38
39 #pragma once
40
41 #include <pcl/range_image/range_image.h>
42
43 #include <pcl/pcl_macros.h>
44 #include <pcl/common/distances.h>
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 #include <pcl/common/vector_average.h> // for VectorAverage3f
47
48 namespace pcl
49 {
50
51 /////////////////////////////////////////////////////////////////////////
52 inline float
asinLookUp(float value)53 RangeImage::asinLookUp (float value)
54 {
55 return (asin_lookup_table[
56 static_cast<int> (
57 static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * value)) +
58 static_cast<float> (lookup_table_size-1) / 2.0f)]);
59 }
60
61 /////////////////////////////////////////////////////////////////////////
62 inline float
atan2LookUp(float y,float x)63 RangeImage::atan2LookUp (float y, float x)
64 {
65 if (x==0 && y==0)
66 return 0;
67 float ret;
68 if (std::abs (x) < std::abs (y))
69 {
70 ret = atan_lookup_table[
71 static_cast<int> (
72 static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (x / y))) +
73 static_cast<float> (lookup_table_size-1) / 2.0f)];
74 ret = static_cast<float> (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret);
75 }
76 else
77 ret = atan_lookup_table[
78 static_cast<int> (
79 static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (y / x))) +
80 static_cast<float> (lookup_table_size-1)/2.0f)];
81 if (x < 0)
82 ret = static_cast<float> (y < 0 ? ret-M_PI : ret+M_PI);
83
84 return (ret);
85 }
86
87 /////////////////////////////////////////////////////////////////////////
88 inline float
cosLookUp(float value)89 RangeImage::cosLookUp (float value)
90 {
91 int cell_idx = static_cast<int> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1)) * std::abs (value) / (2.0f * static_cast<float> (M_PI))));
92 return (cos_lookup_table[cell_idx]);
93 }
94
95 /////////////////////////////////////////////////////////////////////////
96 template <typename PointCloudType> void
createFromPointCloud(const PointCloudType & point_cloud,float angular_resolution,float max_angle_width,float max_angle_height,const Eigen::Affine3f & sensor_pose,RangeImage::CoordinateFrame coordinate_frame,float noise_level,float min_range,int border_size)97 RangeImage::createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution,
98 float max_angle_width, float max_angle_height,
99 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
100 float noise_level, float min_range, int border_size)
101 {
102 createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height,
103 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
104 }
105
106 /////////////////////////////////////////////////////////////////////////
107 template <typename PointCloudType> void
createFromPointCloud(const PointCloudType & point_cloud,float angular_resolution_x,float angular_resolution_y,float max_angle_width,float max_angle_height,const Eigen::Affine3f & sensor_pose,RangeImage::CoordinateFrame coordinate_frame,float noise_level,float min_range,int border_size)108 RangeImage::createFromPointCloud (const PointCloudType& point_cloud,
109 float angular_resolution_x, float angular_resolution_y,
110 float max_angle_width, float max_angle_height,
111 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
112 float noise_level, float min_range, int border_size)
113 {
114 setAngularResolution (angular_resolution_x, angular_resolution_y);
115
116 width = static_cast<std::uint32_t> (pcl_lrint (std::floor (max_angle_width*angular_resolution_x_reciprocal_)));
117 height = static_cast<std::uint32_t> (pcl_lrint (std::floor (max_angle_height*angular_resolution_y_reciprocal_)));
118
119 int full_width = static_cast<int> (pcl_lrint (std::floor (pcl::deg2rad (360.0f)*angular_resolution_x_reciprocal_))),
120 full_height = static_cast<int> (pcl_lrint (std::floor (pcl::deg2rad (180.0f)*angular_resolution_y_reciprocal_)));
121 image_offset_x_ = (full_width -static_cast<int> (width) )/2;
122 image_offset_y_ = (full_height-static_cast<int> (height))/2;
123 is_dense = false;
124
125 getCoordinateFrameTransformation (coordinate_frame, to_world_system_);
126 to_world_system_ = sensor_pose * to_world_system_;
127
128 to_range_image_system_ = to_world_system_.inverse (Eigen::Isometry);
129 //std::cout << "to_world_system_ is\n"<<to_world_system_<<"\nand to_range_image_system_ is\n"<<to_range_image_system_<<"\n\n";
130
131 unsigned int size = width*height;
132 points.clear ();
133 points.resize (size, unobserved_point);
134
135 int top=height, right=-1, bottom=-1, left=width;
136 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
137
138 cropImage (border_size, top, right, bottom, left);
139
140 recalculate3DPointPositions ();
141 }
142
143 /////////////////////////////////////////////////////////////////////////
144 template <typename PointCloudType> void
createFromPointCloudWithKnownSize(const PointCloudType & point_cloud,float angular_resolution,const Eigen::Vector3f & point_cloud_center,float point_cloud_radius,const Eigen::Affine3f & sensor_pose,RangeImage::CoordinateFrame coordinate_frame,float noise_level,float min_range,int border_size)145 RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution,
146 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
147 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
148 float noise_level, float min_range, int border_size)
149 {
150 createFromPointCloudWithKnownSize (point_cloud, angular_resolution, angular_resolution, point_cloud_center, point_cloud_radius,
151 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
152 }
153
154 /////////////////////////////////////////////////////////////////////////
155 template <typename PointCloudType> void
createFromPointCloudWithKnownSize(const PointCloudType & point_cloud,float angular_resolution_x,float angular_resolution_y,const Eigen::Vector3f & point_cloud_center,float point_cloud_radius,const Eigen::Affine3f & sensor_pose,RangeImage::CoordinateFrame coordinate_frame,float noise_level,float min_range,int border_size)156 RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud,
157 float angular_resolution_x, float angular_resolution_y,
158 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
159 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
160 float noise_level, float min_range, int border_size)
161 {
162 //MEASURE_FUNCTION_TIME;
163
164 //std::cout << "Starting to create range image from "<<point_cloud.size ()<<" points.\n";
165
166 // If the sensor pose is inside of the sphere we have to calculate the image the normal way
167 if ((point_cloud_center-sensor_pose.translation()).norm() <= point_cloud_radius) {
168 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
169 pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
170 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
171 return;
172 }
173
174 setAngularResolution (angular_resolution_x, angular_resolution_y);
175
176 getCoordinateFrameTransformation (coordinate_frame, to_world_system_);
177 to_world_system_ = sensor_pose * to_world_system_;
178 to_range_image_system_ = to_world_system_.inverse (Eigen::Isometry);
179
180 float max_angle_size = getMaxAngleSize (sensor_pose, point_cloud_center, point_cloud_radius);
181 int pixel_radius_x = pcl_lrint (std::ceil (0.5f*max_angle_size*angular_resolution_x_reciprocal_)),
182 pixel_radius_y = pcl_lrint (std::ceil (0.5f*max_angle_size*angular_resolution_y_reciprocal_));
183 width = 2*pixel_radius_x;
184 height = 2*pixel_radius_y;
185 is_dense = false;
186
187 image_offset_x_ = image_offset_y_ = 0; // temporary values for getImagePoint
188 int center_pixel_x, center_pixel_y;
189 getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y);
190 image_offset_x_ = (std::max) (0, center_pixel_x-pixel_radius_x);
191 image_offset_y_ = (std::max) (0, center_pixel_y-pixel_radius_y);
192
193 points.clear ();
194 points.resize (width*height, unobserved_point);
195
196 int top=height, right=-1, bottom=-1, left=width;
197 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left);
198
199 cropImage (border_size, top, right, bottom, left);
200
201 recalculate3DPointPositions ();
202 }
203
204 /////////////////////////////////////////////////////////////////////////
205 template <typename PointCloudTypeWithViewpoints> void
createFromPointCloudWithViewpoints(const PointCloudTypeWithViewpoints & point_cloud,float angular_resolution,float max_angle_width,float max_angle_height,RangeImage::CoordinateFrame coordinate_frame,float noise_level,float min_range,int border_size)206 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
207 float angular_resolution,
208 float max_angle_width, float max_angle_height,
209 RangeImage::CoordinateFrame coordinate_frame,
210 float noise_level, float min_range, int border_size)
211 {
212 createFromPointCloudWithViewpoints (point_cloud, angular_resolution, angular_resolution,
213 max_angle_width, max_angle_height, coordinate_frame,
214 noise_level, min_range, border_size);
215 }
216
217 /////////////////////////////////////////////////////////////////////////
218 template <typename PointCloudTypeWithViewpoints> void
createFromPointCloudWithViewpoints(const PointCloudTypeWithViewpoints & point_cloud,float angular_resolution_x,float angular_resolution_y,float max_angle_width,float max_angle_height,RangeImage::CoordinateFrame coordinate_frame,float noise_level,float min_range,int border_size)219 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
220 float angular_resolution_x, float angular_resolution_y,
221 float max_angle_width, float max_angle_height,
222 RangeImage::CoordinateFrame coordinate_frame,
223 float noise_level, float min_range, int border_size)
224 {
225 Eigen::Vector3f average_viewpoint = getAverageViewPoint (point_cloud);
226 Eigen::Affine3f sensor_pose = static_cast<Eigen::Affine3f> (Eigen::Translation3f (average_viewpoint));
227 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height,
228 sensor_pose, coordinate_frame, noise_level, min_range, border_size);
229 }
230
231 /////////////////////////////////////////////////////////////////////////
232 template <typename PointCloudType> void
doZBuffer(const PointCloudType & point_cloud,float noise_level,float min_range,int & top,int & right,int & bottom,int & left)233 RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, float min_range, int& top, int& right, int& bottom, int& left)
234 {
235 using PointType2 = typename PointCloudType::PointType;
236 const typename pcl::PointCloud<PointType2>::VectorType &points2 = point_cloud.points;
237
238 unsigned int size = width*height;
239 int* counters = new int[size];
240 ERASE_ARRAY (counters, size);
241
242 top=height; right=-1; bottom=-1; left=width;
243
244 float x_real, y_real, range_of_current_point;
245 int x, y;
246 for (const auto& point: points2)
247 {
248 if (!isFinite (point)) // Check for NAN etc
249 continue;
250 Vector3fMapConst current_point = point.getVector3fMap ();
251
252 this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
253 this->real2DToInt2D (x_real, y_real, x, y);
254
255 if (range_of_current_point < min_range|| !isInImage (x, y))
256 continue;
257 //std::cout << " ("<<current_point[0]<<", "<<current_point[1]<<", "<<current_point[2]<<") falls into pixel "<<x<<","<<y<<".\n";
258
259 // Do some minor interpolation by checking the three closest neighbors to the point, that are not filled yet.
260 int floor_x = pcl_lrint (std::floor (x_real)), floor_y = pcl_lrint (std::floor (y_real)),
261 ceil_x = pcl_lrint (std::ceil (x_real)), ceil_y = pcl_lrint (std::ceil (y_real));
262
263 int neighbor_x[4], neighbor_y[4];
264 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
265 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
266 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
267 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
268 //std::cout << x_real<<","<<y_real<<": ";
269
270 for (int i=0; i<4; ++i)
271 {
272 int n_x=neighbor_x[i], n_y=neighbor_y[i];
273 //std::cout << n_x<<","<<n_y<<" ";
274 if (n_x==x && n_y==y)
275 continue;
276 if (isInImage (n_x, n_y))
277 {
278 int neighbor_array_pos = n_y*width + n_x;
279 if (counters[neighbor_array_pos]==0)
280 {
281 float& neighbor_range = points[neighbor_array_pos].range;
282 neighbor_range = (std::isinf (neighbor_range) ? range_of_current_point : (std::min) (neighbor_range, range_of_current_point));
283 top= (std::min) (top, n_y); right= (std::max) (right, n_x); bottom= (std::max) (bottom, n_y); left= (std::min) (left, n_x);
284 }
285 }
286 }
287 //std::cout <<std::endl;
288
289 // The point itself
290 int arrayPos = y*width + x;
291 float& range_at_image_point = points[arrayPos].range;
292 int& counter = counters[arrayPos];
293 bool addCurrentPoint=false, replace_with_current_point=false;
294
295 if (counter==0)
296 {
297 replace_with_current_point = true;
298 }
299 else
300 {
301 if (range_of_current_point < range_at_image_point-noise_level)
302 {
303 replace_with_current_point = true;
304 }
305 else if (std::fabs (range_of_current_point-range_at_image_point)<=noise_level)
306 {
307 addCurrentPoint = true;
308 }
309 }
310
311 if (replace_with_current_point)
312 {
313 counter = 1;
314 range_at_image_point = range_of_current_point;
315 top= (std::min) (top, y); right= (std::max) (right, x); bottom= (std::max) (bottom, y); left= (std::min) (left, x);
316 //std::cout << "Adding point "<<x<<","<<y<<"\n";
317 }
318 else if (addCurrentPoint)
319 {
320 ++counter;
321 range_at_image_point += (range_of_current_point-range_at_image_point)/counter;
322 }
323 }
324
325 delete[] counters;
326 }
327
328 /////////////////////////////////////////////////////////////////////////
329 void
getImagePoint(float x,float y,float z,float & image_x,float & image_y,float & range) const330 RangeImage::getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const
331 {
332 Eigen::Vector3f point (x, y, z);
333 getImagePoint (point, image_x, image_y, range);
334 }
335
336 /////////////////////////////////////////////////////////////////////////
337 void
getImagePoint(float x,float y,float z,float & image_x,float & image_y) const338 RangeImage::getImagePoint (float x, float y, float z, float& image_x, float& image_y) const
339 {
340 float range;
341 getImagePoint (x, y, z, image_x, image_y, range);
342 }
343
344 /////////////////////////////////////////////////////////////////////////
345 void
getImagePoint(float x,float y,float z,int & image_x,int & image_y) const346 RangeImage::getImagePoint (float x, float y, float z, int& image_x, int& image_y) const
347 {
348 float image_x_float, image_y_float;
349 getImagePoint (x, y, z, image_x_float, image_y_float);
350 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
351 }
352
353 /////////////////////////////////////////////////////////////////////////
354 void
getImagePoint(const Eigen::Vector3f & point,float & image_x,float & image_y,float & range) const355 RangeImage::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const
356 {
357 Eigen::Vector3f transformedPoint = to_range_image_system_ * point;
358 range = transformedPoint.norm ();
359 float angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]),
360 angle_y = asinLookUp (transformedPoint[1]/range);
361 getImagePointFromAngles (angle_x, angle_y, image_x, image_y);
362 //std::cout << " ("<<point[0]<<","<<point[1]<<","<<point[2]<<")"
363 //<< " => ("<<transformedPoint[0]<<","<<transformedPoint[1]<<","<<transformedPoint[2]<<")"
364 //<< " => "<<angle_x<<","<<angle_y<<" => "<<image_x<<","<<image_y<<"\n";
365 }
366
367 /////////////////////////////////////////////////////////////////////////
368 void
getImagePoint(const Eigen::Vector3f & point,int & image_x,int & image_y,float & range) const369 RangeImage::getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const {
370 float image_x_float, image_y_float;
371 getImagePoint (point, image_x_float, image_y_float, range);
372 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
373 }
374
375 /////////////////////////////////////////////////////////////////////////
376 void
getImagePoint(const Eigen::Vector3f & point,float & image_x,float & image_y) const377 RangeImage::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const
378 {
379 float range;
380 getImagePoint (point, image_x, image_y, range);
381 }
382
383 /////////////////////////////////////////////////////////////////////////
384 void
getImagePoint(const Eigen::Vector3f & point,int & image_x,int & image_y) const385 RangeImage::getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const
386 {
387 float image_x_float, image_y_float;
388 getImagePoint (point, image_x_float, image_y_float);
389 real2DToInt2D (image_x_float, image_y_float, image_x, image_y);
390 }
391
392 /////////////////////////////////////////////////////////////////////////
393 float
checkPoint(const Eigen::Vector3f & point,PointWithRange & point_in_image) const394 RangeImage::checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const
395 {
396 int image_x, image_y;
397 float range;
398 getImagePoint (point, image_x, image_y, range);
399 if (!isInImage (image_x, image_y))
400 point_in_image = unobserved_point;
401 else
402 point_in_image = getPoint (image_x, image_y);
403 return range;
404 }
405
406 /////////////////////////////////////////////////////////////////////////
407 float
getRangeDifference(const Eigen::Vector3f & point) const408 RangeImage::getRangeDifference (const Eigen::Vector3f& point) const
409 {
410 int image_x, image_y;
411 float range;
412 getImagePoint (point, image_x, image_y, range);
413 if (!isInImage (image_x, image_y))
414 return -std::numeric_limits<float>::infinity ();
415 float image_point_range = getPoint (image_x, image_y).range;
416 if (std::isinf (image_point_range))
417 {
418 if (image_point_range > 0.0f)
419 return std::numeric_limits<float>::infinity ();
420 return -std::numeric_limits<float>::infinity ();
421 }
422 return image_point_range - range;
423 }
424
425 /////////////////////////////////////////////////////////////////////////
426 void
getImagePointFromAngles(float angle_x,float angle_y,float & image_x,float & image_y) const427 RangeImage::getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const
428 {
429 image_x = (angle_x*cosLookUp (angle_y) + static_cast<float> (M_PI))*angular_resolution_x_reciprocal_ - static_cast<float> (image_offset_x_);
430 image_y = (angle_y + 0.5f*static_cast<float> (M_PI))*angular_resolution_y_reciprocal_ - static_cast<float> (image_offset_y_);
431 }
432
433 /////////////////////////////////////////////////////////////////////////
434 void
real2DToInt2D(float x,float y,int & xInt,int & yInt) const435 RangeImage::real2DToInt2D (float x, float y, int& xInt, int& yInt) const
436 {
437 xInt = static_cast<int> (pcl_lrintf (x));
438 yInt = static_cast<int> (pcl_lrintf (y));
439 }
440
441 /////////////////////////////////////////////////////////////////////////
442 bool
isInImage(int x,int y) const443 RangeImage::isInImage (int x, int y) const
444 {
445 return (x >= 0 && x < static_cast<int> (width) && y >= 0 && y < static_cast<int> (height));
446 }
447
448 /////////////////////////////////////////////////////////////////////////
449 bool
isValid(int x,int y) const450 RangeImage::isValid (int x, int y) const
451 {
452 return isInImage (x,y) && std::isfinite (getPoint (x,y).range);
453 }
454
455 /////////////////////////////////////////////////////////////////////////
456 bool
isValid(int index) const457 RangeImage::isValid (int index) const
458 {
459 return std::isfinite (getPoint (index).range);
460 }
461
462 /////////////////////////////////////////////////////////////////////////
463 bool
isObserved(int x,int y) const464 RangeImage::isObserved (int x, int y) const
465 {
466 return !(!isInImage (x,y) || (std::isinf (getPoint (x,y).range) && getPoint (x,y).range < 0.0f));
467 }
468
469 /////////////////////////////////////////////////////////////////////////
470 bool
isMaxRange(int x,int y) const471 RangeImage::isMaxRange (int x, int y) const
472 {
473 float range = getPoint (x,y).range;
474 return std::isinf (range) && range>0.0f;
475 }
476
477 /////////////////////////////////////////////////////////////////////////
478 const PointWithRange&
getPoint(int image_x,int image_y) const479 RangeImage::getPoint (int image_x, int image_y) const
480 {
481 if (!isInImage (image_x, image_y))
482 return unobserved_point;
483 return points[image_y*width + image_x];
484 }
485
486 /////////////////////////////////////////////////////////////////////////
487 const PointWithRange&
getPointNoCheck(int image_x,int image_y) const488 RangeImage::getPointNoCheck (int image_x, int image_y) const
489 {
490 return points[image_y*width + image_x];
491 }
492
493 /////////////////////////////////////////////////////////////////////////
494 PointWithRange&
getPointNoCheck(int image_x,int image_y)495 RangeImage::getPointNoCheck (int image_x, int image_y)
496 {
497 return points[image_y*width + image_x];
498 }
499
500 /////////////////////////////////////////////////////////////////////////
501 PointWithRange&
getPoint(int image_x,int image_y)502 RangeImage::getPoint (int image_x, int image_y)
503 {
504 return points[image_y*width + image_x];
505 }
506
507
508 /////////////////////////////////////////////////////////////////////////
509 const PointWithRange&
getPoint(int index) const510 RangeImage::getPoint (int index) const
511 {
512 return points[index];
513 }
514
515 /////////////////////////////////////////////////////////////////////////
516 const PointWithRange&
getPoint(float image_x,float image_y) const517 RangeImage::getPoint (float image_x, float image_y) const
518 {
519 int x, y;
520 real2DToInt2D (image_x, image_y, x, y);
521 return getPoint (x, y);
522 }
523
524 /////////////////////////////////////////////////////////////////////////
525 PointWithRange&
getPoint(float image_x,float image_y)526 RangeImage::getPoint (float image_x, float image_y)
527 {
528 int x, y;
529 real2DToInt2D (image_x, image_y, x, y);
530 return getPoint (x, y);
531 }
532
533 /////////////////////////////////////////////////////////////////////////
534 void
getPoint(int image_x,int image_y,Eigen::Vector3f & point) const535 RangeImage::getPoint (int image_x, int image_y, Eigen::Vector3f& point) const
536 {
537 //std::cout << getPoint (image_x, image_y)<< " - " << getPoint (image_x, image_y).getVector3fMap ()<<"\n";
538 point = getPoint (image_x, image_y).getVector3fMap ();
539 }
540
541 /////////////////////////////////////////////////////////////////////////
542 void
getPoint(int index,Eigen::Vector3f & point) const543 RangeImage::getPoint (int index, Eigen::Vector3f& point) const
544 {
545 point = getPoint (index).getVector3fMap ();
546 }
547
548 /////////////////////////////////////////////////////////////////////////
549 const Eigen::Map<const Eigen::Vector3f>
getEigenVector3f(int x,int y) const550 RangeImage::getEigenVector3f (int x, int y) const
551 {
552 return getPoint (x, y).getVector3fMap ();
553 }
554
555 /////////////////////////////////////////////////////////////////////////
556 const Eigen::Map<const Eigen::Vector3f>
getEigenVector3f(int index) const557 RangeImage::getEigenVector3f (int index) const
558 {
559 return getPoint (index).getVector3fMap ();
560 }
561
562 /////////////////////////////////////////////////////////////////////////
563 void
calculate3DPoint(float image_x,float image_y,float range,Eigen::Vector3f & point) const564 RangeImage::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const
565 {
566 float angle_x, angle_y;
567 //std::cout << image_x<<","<<image_y<<","<<range;
568 getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y);
569
570 float cosY = std::cos (angle_y);
571 point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * std::cos (angle_x)*cosY);
572 point = to_world_system_ * point;
573 }
574
575 /////////////////////////////////////////////////////////////////////////
576 void
calculate3DPoint(float image_x,float image_y,Eigen::Vector3f & point) const577 RangeImage::calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const
578 {
579 const PointWithRange& point_in_image = getPoint (image_x, image_y);
580 calculate3DPoint (image_x, image_y, point_in_image.range, point);
581 }
582
583 /////////////////////////////////////////////////////////////////////////
584 void
calculate3DPoint(float image_x,float image_y,float range,PointWithRange & point) const585 RangeImage::calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const {
586 point.range = range;
587 Eigen::Vector3f tmp_point;
588 calculate3DPoint (image_x, image_y, range, tmp_point);
589 point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2];
590 }
591
592 /////////////////////////////////////////////////////////////////////////
593 void
calculate3DPoint(float image_x,float image_y,PointWithRange & point) const594 RangeImage::calculate3DPoint (float image_x, float image_y, PointWithRange& point) const
595 {
596 const PointWithRange& point_in_image = getPoint (image_x, image_y);
597 calculate3DPoint (image_x, image_y, point_in_image.range, point);
598 }
599
600 /////////////////////////////////////////////////////////////////////////
601 void
getAnglesFromImagePoint(float image_x,float image_y,float & angle_x,float & angle_y) const602 RangeImage::getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const
603 {
604 angle_y = (image_y+static_cast<float> (image_offset_y_))*angular_resolution_y_ - 0.5f*static_cast<float> (M_PI);
605 float cos_angle_y = std::cos (angle_y);
606 angle_x = (cos_angle_y==0.0f ? 0.0f : ( (image_x+ static_cast<float> (image_offset_x_))*angular_resolution_x_ - static_cast<float> (M_PI))/cos_angle_y);
607 }
608
609 /////////////////////////////////////////////////////////////////////////
610 float
getImpactAngle(int x1,int y1,int x2,int y2) const611 RangeImage::getImpactAngle (int x1, int y1, int x2, int y2) const
612 {
613 if (!isInImage (x1, y1) || !isInImage (x2,y2))
614 return -std::numeric_limits<float>::infinity ();
615 return getImpactAngle (getPoint (x1,y1),getPoint (x2,y2));
616 }
617
618 /////////////////////////////////////////////////////////////////////////
619 float
getImpactAngle(const PointWithRange & point1,const PointWithRange & point2) const620 RangeImage::getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const {
621 if ( (std::isinf (point1.range)&&point1.range<0) || (std::isinf (point2.range)&&point2.range<0))
622 return -std::numeric_limits<float>::infinity ();
623
624 float r1 = (std::min) (point1.range, point2.range),
625 r2 = (std::max) (point1.range, point2.range);
626 float impact_angle = static_cast<float> (0.5f * M_PI);
627
628 if (std::isinf (r2))
629 {
630 if (r2 > 0.0f && !std::isinf (r1))
631 impact_angle = 0.0f;
632 }
633 else if (!std::isinf (r1))
634 {
635 float r1Sqr = r1*r1,
636 r2Sqr = r2*r2,
637 dSqr = squaredEuclideanDistance (point1, point2),
638 d = ::sqrt (dSqr);
639 float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d);
640 cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle));
641 impact_angle = std::acos (cos_impact_angle); // Using the cosine rule
642 }
643
644 if (point1.range > point2.range)
645 impact_angle = -impact_angle;
646
647 return impact_angle;
648 }
649
650 /////////////////////////////////////////////////////////////////////////
651 float
getAcutenessValue(const PointWithRange & point1,const PointWithRange & point2) const652 RangeImage::getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const
653 {
654 float impact_angle = getImpactAngle (point1, point2);
655 if (std::isinf (impact_angle))
656 return -std::numeric_limits<float>::infinity ();
657 float ret = 1.0f - float (std::fabs (impact_angle)/ (0.5f*M_PI));
658 if (impact_angle < 0.0f)
659 ret = -ret;
660 //if (std::abs (ret)>1)
661 //std::cout << PVARAC (impact_angle)<<PVARN (ret);
662 return ret;
663 }
664
665 /////////////////////////////////////////////////////////////////////////
666 float
getAcutenessValue(int x1,int y1,int x2,int y2) const667 RangeImage::getAcutenessValue (int x1, int y1, int x2, int y2) const
668 {
669 if (!isInImage (x1, y1) || !isInImage (x2,y2))
670 return -std::numeric_limits<float>::infinity ();
671 return getAcutenessValue (getPoint (x1,y1), getPoint (x2,y2));
672 }
673
674 /////////////////////////////////////////////////////////////////////////
675 const Eigen::Vector3f
getSensorPos() const676 RangeImage::getSensorPos () const
677 {
678 return Eigen::Vector3f (to_world_system_ (0,3), to_world_system_ (1,3), to_world_system_ (2,3));
679 }
680
681 /////////////////////////////////////////////////////////////////////////
682 void
getSurfaceAngleChange(int x,int y,int radius,float & angle_change_x,float & angle_change_y) const683 RangeImage::getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const
684 {
685 angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity ();
686 if (!isValid (x,y))
687 return;
688 Eigen::Vector3f point;
689 getPoint (x, y, point);
690 Eigen::Affine3f transformation = getTransformationToViewerCoordinateFrame (point);
691
692 if (isObserved (x-radius, y) && isObserved (x+radius, y))
693 {
694 Eigen::Vector3f transformed_left;
695 if (isMaxRange (x-radius, y))
696 transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
697 else
698 {
699 Eigen::Vector3f left;
700 getPoint (x-radius, y, left);
701 transformed_left = - (transformation * left);
702 //std::cout << PVARN (transformed_left[1]);
703 transformed_left[1] = 0.0f;
704 transformed_left.normalize ();
705 }
706
707 Eigen::Vector3f transformed_right;
708 if (isMaxRange (x+radius, y))
709 transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
710 else
711 {
712 Eigen::Vector3f right;
713 getPoint (x+radius, y, right);
714 transformed_right = transformation * right;
715 //std::cout << PVARN (transformed_right[1]);
716 transformed_right[1] = 0.0f;
717 transformed_right.normalize ();
718 }
719 angle_change_x = transformed_left.dot (transformed_right);
720 angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x));
721 angle_change_x = std::acos (angle_change_x);
722 }
723
724 if (isObserved (x, y-radius) && isObserved (x, y+radius))
725 {
726 Eigen::Vector3f transformed_top;
727 if (isMaxRange (x, y-radius))
728 transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f);
729 else
730 {
731 Eigen::Vector3f top;
732 getPoint (x, y-radius, top);
733 transformed_top = - (transformation * top);
734 //std::cout << PVARN (transformed_top[0]);
735 transformed_top[0] = 0.0f;
736 transformed_top.normalize ();
737 }
738
739 Eigen::Vector3f transformed_bottom;
740 if (isMaxRange (x, y+radius))
741 transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f);
742 else
743 {
744 Eigen::Vector3f bottom;
745 getPoint (x, y+radius, bottom);
746 transformed_bottom = transformation * bottom;
747 //std::cout << PVARN (transformed_bottom[0]);
748 transformed_bottom[0] = 0.0f;
749 transformed_bottom.normalize ();
750 }
751 angle_change_y = transformed_top.dot (transformed_bottom);
752 angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y));
753 angle_change_y = std::acos (angle_change_y);
754 }
755 }
756
757
758 //inline float RangeImage::getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, const PointWithRange& neighbor2) const
759 //{
760 //if (!std::isfinite (point.range) || (!std::isfinite (neighbor1.range)&&neighbor1.range<0) || (!std::isfinite (neighbor2.range)&&neighbor2.range<0))
761 //return -std::numeric_limits<float>::infinity ();
762 //if (std::isinf (neighbor1.range))
763 //{
764 //if (std::isinf (neighbor2.range))
765 //return 0.0f;
766 //else
767 //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 ()));
768 //}
769 //if (std::isinf (neighbor2.range))
770 //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 ()));
771
772 //float d1_squared = squaredEuclideanDistance (point, neighbor1),
773 //d1 = ::sqrt (d1_squared),
774 //d2_squared = squaredEuclideanDistance (point, neighbor2),
775 //d2 = ::sqrt (d2_squared),
776 //d3_squared = squaredEuclideanDistance (neighbor1, neighbor2);
777 //float cos_surface_change = (d1_squared + d2_squared - d3_squared)/ (2.0f*d1*d2),
778 //surface_change = std::acos (cos_surface_change);
779 //if (std::isnan (surface_change))
780 //surface_change = static_cast<float> (M_PI);
781 ////std::cout << PVARN (point)<<PVARN (neighbor1)<<PVARN (neighbor2)<<PVARN (cos_surface_change)<<PVARN (surface_change)<<PVARN (d1)<<PVARN (d2)<<PVARN (d1_squared)<<PVARN (d2_squared)<<PVARN (d3_squared);
782
783 //return surface_change;
784 //}
785
786 /////////////////////////////////////////////////////////////////////////
787 float
getMaxAngleSize(const Eigen::Affine3f & viewer_pose,const Eigen::Vector3f & center,float radius)788 RangeImage::getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, float radius)
789 {
790 return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ());
791 }
792
793 /////////////////////////////////////////////////////////////////////////
794 Eigen::Vector3f
getEigenVector3f(const PointWithRange & point)795 RangeImage::getEigenVector3f (const PointWithRange& point)
796 {
797 return Eigen::Vector3f (point.x, point.y, point.z);
798 }
799
800 /////////////////////////////////////////////////////////////////////////
801 void
get1dPointAverage(int x,int y,int delta_x,int delta_y,int no_of_points,PointWithRange & average_point) const802 RangeImage::get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange& average_point) const
803 {
804 //std::cout << __PRETTY_FUNCTION__<<" called.\n";
805 //MEASURE_FUNCTION_TIME;
806 float weight_sum = 1.0f;
807 average_point = getPoint (x,y);
808 if (std::isinf (average_point.range))
809 {
810 if (average_point.range>0.0f) // The first point is max range -> return a max range point
811 return;
812 weight_sum = 0.0f;
813 average_point.x = average_point.y = average_point.z = average_point.range = 0.0f;
814 }
815
816 int x2=x, y2=y;
817 Vector4fMap average_point_eigen = average_point.getVector4fMap ();
818 //std::cout << PVARN (no_of_points);
819 for (int step=1; step<no_of_points; ++step)
820 {
821 //std::cout << PVARC (step);
822 x2+=delta_x; y2+=delta_y;
823 if (!isValid (x2, y2))
824 continue;
825 const PointWithRange& p = getPointNoCheck (x2, y2);
826 average_point_eigen+=p.getVector4fMap (); average_point.range+=p.range;
827 weight_sum += 1.0f;
828 }
829 if (weight_sum<= 0.0f)
830 {
831 average_point = unobserved_point;
832 return;
833 }
834 float normalization_factor = 1.0f/weight_sum;
835 average_point_eigen *= normalization_factor;
836 average_point.range *= normalization_factor;
837 //std::cout << PVARN (average_point);
838 }
839
840 /////////////////////////////////////////////////////////////////////////
841 float
getEuclideanDistanceSquared(int x1,int y1,int x2,int y2) const842 RangeImage::getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const
843 {
844 if (!isObserved (x1,y1)||!isObserved (x2,y2))
845 return -std::numeric_limits<float>::infinity ();
846 const PointWithRange& point1 = getPoint (x1,y1),
847 & point2 = getPoint (x2,y2);
848 if (std::isinf (point1.range) && std::isinf (point2.range))
849 return 0.0f;
850 if (std::isinf (point1.range) || std::isinf (point2.range))
851 return std::numeric_limits<float>::infinity ();
852 return squaredEuclideanDistance (point1, point2);
853 }
854
855 /////////////////////////////////////////////////////////////////////////
856 float
getAverageEuclideanDistance(int x,int y,int offset_x,int offset_y,int max_steps) const857 RangeImage::getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const
858 {
859 float average_pixel_distance = 0.0f;
860 float weight=0.0f;
861 for (int i=0; i<max_steps; ++i)
862 {
863 int x1=x+i*offset_x, y1=y+i*offset_y;
864 int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y;
865 float pixel_distance = getEuclideanDistanceSquared (x1,y1,x2,y2);
866 if (!std::isfinite (pixel_distance))
867 {
868 //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<pixel_distance<<"\n";
869 if (i==0)
870 return pixel_distance;
871 break;
872 }
873 //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<::sqrt (pixel_distance)<<"m\n";
874 weight += 1.0f;
875 average_pixel_distance += ::sqrt (pixel_distance);
876 }
877 average_pixel_distance /= weight;
878 //std::cout << x<<","<<y<<","<<offset_x<<","<<offset_y<<" => "<<average_pixel_distance<<"\n";
879 return average_pixel_distance;
880 }
881
882 /////////////////////////////////////////////////////////////////////////
883 float
getImpactAngleBasedOnLocalNormal(int x,int y,int radius) const884 RangeImage::getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const
885 {
886 if (!isValid (x,y))
887 return -std::numeric_limits<float>::infinity ();
888 const PointWithRange& point = getPoint (x, y);
889 int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> ( (radius + 1.0)), 2.0));
890 Eigen::Vector3f normal;
891 if (!getNormalForClosestNeighbors (x, y, radius, point, no_of_nearest_neighbors, normal, 1))
892 return -std::numeric_limits<float>::infinity ();
893 return deg2rad (90.0f) - std::acos (normal.dot ( (getSensorPos ()-getEigenVector3f (point)).normalized ()));
894 }
895
896
897 /////////////////////////////////////////////////////////////////////////
898 bool
getNormal(int x,int y,int radius,Eigen::Vector3f & normal,int step_size) const899 RangeImage::getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size) const
900 {
901 VectorAverage3f vector_average;
902 for (int y2=y-radius; y2<=y+radius; y2+=step_size)
903 {
904 for (int x2=x-radius; x2<=x+radius; x2+=step_size)
905 {
906 if (!isInImage (x2, y2))
907 continue;
908 const PointWithRange& point = getPoint (x2, y2);
909 if (!std::isfinite (point.range))
910 continue;
911 vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
912 }
913 }
914 if (vector_average.getNoOfSamples () < 3)
915 return false;
916 Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3;
917 vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
918 if (normal.dot ( (getSensorPos ()-vector_average.getMean ()).normalized ()) < 0.0f)
919 normal *= -1.0f;
920 return true;
921 }
922
923 /////////////////////////////////////////////////////////////////////////
924 float
getNormalBasedAcutenessValue(int x,int y,int radius) const925 RangeImage::getNormalBasedAcutenessValue (int x, int y, int radius) const
926 {
927 float impact_angle = getImpactAngleBasedOnLocalNormal (x, y, radius);
928 if (std::isinf (impact_angle))
929 return -std::numeric_limits<float>::infinity ();
930 float ret = 1.0f - static_cast<float> ( (impact_angle / (0.5f * M_PI)));
931 //std::cout << PVARAC (impact_angle)<<PVARN (ret);
932 return ret;
933 }
934
935 /////////////////////////////////////////////////////////////////////////
936 bool
getNormalForClosestNeighbors(int x,int y,int radius,const PointWithRange & point,int no_of_nearest_neighbors,Eigen::Vector3f & normal,int step_size) const937 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
938 int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size) const
939 {
940 return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal, nullptr, step_size);
941 }
942
943 /////////////////////////////////////////////////////////////////////////
944 bool
getNormalForClosestNeighbors(int x,int y,Eigen::Vector3f & normal,int radius) const945 RangeImage::getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius) const
946 {
947 if (!isValid (x,y))
948 return false;
949 int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> (radius + 1.0), 2.0));
950 return getNormalForClosestNeighbors (x, y, radius, getPoint (x,y).getVector3fMap (), no_of_nearest_neighbors, normal);
951 }
952
953 namespace
954 { // Anonymous namespace, so that this is only accessible in this file
955 struct NeighborWithDistance
956 { // local struct to help us with sorting
957 float distance;
958 const PointWithRange* neighbor;
operator <pcl::__anon4626e07d0111::NeighborWithDistance959 bool operator < (const NeighborWithDistance& other) const { return distance<other.distance;}
960 };
961 }
962
963 /////////////////////////////////////////////////////////////////////////
964 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,Eigen::Vector3f * mean_all_neighbors,Eigen::Vector3f * eigen_values_all_neighbors) const965 RangeImage::getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_closest_neighbors, int step_size,
966 float& max_closest_neighbor_distance_squared,
967 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
968 Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors,
969 Eigen::Vector3f* eigen_values_all_neighbors) const
970 {
971 max_closest_neighbor_distance_squared=0.0f;
972 normal.setZero (); mean.setZero (); eigen_values.setZero ();
973 if (normal_all_neighbors!=nullptr)
974 normal_all_neighbors->setZero ();
975 if (mean_all_neighbors!=nullptr)
976 mean_all_neighbors->setZero ();
977 if (eigen_values_all_neighbors!=nullptr)
978 eigen_values_all_neighbors->setZero ();
979
980 const auto sqrt_blocksize = 2 * radius + 1;
981 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
982
983 PointWithRange given_point;
984 given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2];
985
986 std::vector<NeighborWithDistance> ordered_neighbors (blocksize);
987 int neighbor_counter = 0;
988 for (int y2=y-radius; y2<=y+radius; y2+=step_size)
989 {
990 for (int x2=x-radius; x2<=x+radius; x2+=step_size)
991 {
992 if (!isValid (x2, y2))
993 continue;
994 NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter];
995 neighbor_with_distance.neighbor = &getPoint (x2, y2);
996 neighbor_with_distance.distance = squaredEuclideanDistance (given_point, *neighbor_with_distance.neighbor);
997 ++neighbor_counter;
998 }
999 }
1000 no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors);
1001
1002 std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter); // Normal sort seems to be the fastest method (faster than partial_sort)
1003 //std::stable_sort (ordered_neighbors, ordered_neighbors+neighbor_counter);
1004 //std::partial_sort (ordered_neighbors, ordered_neighbors+no_of_closest_neighbors, ordered_neighbors+neighbor_counter);
1005
1006 max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance;
1007 //float max_distance_squared = max_closest_neighbor_distance_squared;
1008 float max_distance_squared = max_closest_neighbor_distance_squared*4.0f; // Double the allowed distance value
1009 //max_closest_neighbor_distance_squared = max_distance_squared;
1010
1011 VectorAverage3f vector_average;
1012 //for (int neighbor_idx=0; neighbor_idx<no_of_closest_neighbors; ++neighbor_idx)
1013 int neighbor_idx;
1014 for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx)
1015 {
1016 if (ordered_neighbors[neighbor_idx].distance > max_distance_squared)
1017 break;
1018 //std::cout << ordered_neighbors[neighbor_idx].distance<<"\n";
1019 vector_average.add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ());
1020 }
1021
1022 if (vector_average.getNoOfSamples () < 3)
1023 return false;
1024 //std::cout << PVARN (vector_average.getNoOfSamples ());
1025 Eigen::Vector3f eigen_vector2, eigen_vector3;
1026 vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3);
1027 Eigen::Vector3f viewing_direction = (getSensorPos ()-point).normalized ();
1028 if (normal.dot (viewing_direction) < 0.0f)
1029 normal *= -1.0f;
1030 mean = vector_average.getMean ();
1031
1032 if (normal_all_neighbors==nullptr)
1033 return true;
1034
1035 // Add remaining neighbors
1036 for (int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2)
1037 vector_average.add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ());
1038
1039 vector_average.doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3);
1040 //std::cout << PVARN (vector_average.getNoOfSamples ())<<".\n";
1041 if (normal_all_neighbors->dot (viewing_direction) < 0.0f)
1042 *normal_all_neighbors *= -1.0f;
1043 *mean_all_neighbors = vector_average.getMean ();
1044
1045 //std::cout << viewing_direction[0]<<","<<viewing_direction[1]<<","<<viewing_direction[2]<<"\n";
1046
1047 return true;
1048 }
1049
1050 /////////////////////////////////////////////////////////////////////////
1051 float
getSquaredDistanceOfNthNeighbor(int x,int y,int radius,int n,int step_size) const1052 RangeImage::getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const
1053 {
1054 const PointWithRange& point = getPoint (x, y);
1055 if (!std::isfinite (point.range))
1056 return -std::numeric_limits<float>::infinity ();
1057
1058 const auto sqrt_blocksize = 2 * radius + 1;
1059 const auto blocksize = sqrt_blocksize * sqrt_blocksize;
1060 std::vector<float> neighbor_distances (blocksize);
1061 int neighbor_counter = 0;
1062 for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1063 {
1064 for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1065 {
1066 if (!isValid (x2, y2) || (x2==x&&y2==y))
1067 continue;
1068 const PointWithRange& neighbor = getPointNoCheck (x2,y2);
1069 float& neighbor_distance = neighbor_distances[neighbor_counter++];
1070 neighbor_distance = squaredEuclideanDistance (point, neighbor);
1071 }
1072 }
1073 std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter); // Normal sort seems to be
1074 // the fastest method (faster than partial_sort)
1075 n = (std::min) (neighbor_counter, n);
1076 return neighbor_distances[n-1];
1077 }
1078
1079
1080 /////////////////////////////////////////////////////////////////////////
1081 bool
getNormalForClosestNeighbors(int x,int y,int radius,const Eigen::Vector3f & point,int no_of_nearest_neighbors,Eigen::Vector3f & normal,Eigen::Vector3f * point_on_plane,int step_size) const1082 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_nearest_neighbors,
1083 Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane, int step_size) const
1084 {
1085 Eigen::Vector3f mean, eigen_values;
1086 float used_squared_max_distance;
1087 bool ret = getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance,
1088 normal, mean, eigen_values);
1089
1090 if (ret)
1091 {
1092 if (point_on_plane != nullptr)
1093 *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point;
1094 }
1095 return ret;
1096 }
1097
1098
1099 /////////////////////////////////////////////////////////////////////////
1100 float
getCurvature(int x,int y,int radius,int step_size) const1101 RangeImage::getCurvature (int x, int y, int radius, int step_size) const
1102 {
1103 VectorAverage3f vector_average;
1104 for (int y2=y-radius; y2<=y+radius; y2+=step_size)
1105 {
1106 for (int x2=x-radius; x2<=x+radius; x2+=step_size)
1107 {
1108 if (!isInImage (x2, y2))
1109 continue;
1110 const PointWithRange& point = getPoint (x2, y2);
1111 if (!std::isfinite (point.range))
1112 continue;
1113 vector_average.add (Eigen::Vector3f (point.x, point.y, point.z));
1114 }
1115 }
1116 if (vector_average.getNoOfSamples () < 3)
1117 return false;
1118 Eigen::Vector3f eigen_values;
1119 vector_average.doPCA (eigen_values);
1120 return eigen_values[0]/eigen_values.sum ();
1121 }
1122
1123
1124 /////////////////////////////////////////////////////////////////////////
1125 template <typename PointCloudTypeWithViewpoints> Eigen::Vector3f
getAverageViewPoint(const PointCloudTypeWithViewpoints & point_cloud)1126 RangeImage::getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud)
1127 {
1128 Eigen::Vector3f average_viewpoint (0,0,0);
1129 int point_counter = 0;
1130 for (const auto& point: point_cloud.points)
1131 {
1132 if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z))
1133 continue;
1134 average_viewpoint[0] += point.vp_x;
1135 average_viewpoint[1] += point.vp_y;
1136 average_viewpoint[2] += point.vp_z;
1137 ++point_counter;
1138 }
1139 average_viewpoint /= point_counter;
1140
1141 return average_viewpoint;
1142 }
1143
1144 /////////////////////////////////////////////////////////////////////////
1145 bool
getViewingDirection(int x,int y,Eigen::Vector3f & viewing_direction) const1146 RangeImage::getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const
1147 {
1148 if (!isValid (x, y))
1149 return false;
1150 viewing_direction = (getPoint (x,y).getVector3fMap ()-getSensorPos ()).normalized ();
1151 return true;
1152 }
1153
1154 /////////////////////////////////////////////////////////////////////////
1155 void
getViewingDirection(const Eigen::Vector3f & point,Eigen::Vector3f & viewing_direction) const1156 RangeImage::getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const
1157 {
1158 viewing_direction = (point-getSensorPos ()).normalized ();
1159 }
1160
1161 /////////////////////////////////////////////////////////////////////////
1162 Eigen::Affine3f
getTransformationToViewerCoordinateFrame(const Eigen::Vector3f & point) const1163 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const
1164 {
1165 Eigen::Affine3f transformation;
1166 getTransformationToViewerCoordinateFrame (point, transformation);
1167 return transformation;
1168 }
1169
1170 /////////////////////////////////////////////////////////////////////////
1171 void
getTransformationToViewerCoordinateFrame(const Eigen::Vector3f & point,Eigen::Affine3f & transformation) const1172 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1173 {
1174 Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1175 getTransformationFromTwoUnitVectorsAndOrigin (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, point, transformation);
1176 }
1177
1178 /////////////////////////////////////////////////////////////////////////
1179 void
getRotationToViewerCoordinateFrame(const Eigen::Vector3f & point,Eigen::Affine3f & transformation) const1180 RangeImage::getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const
1181 {
1182 Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized ();
1183 getTransformationFromTwoUnitVectors (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, transformation);
1184 }
1185
1186 /////////////////////////////////////////////////////////////////////////
1187 inline void
setAngularResolution(float angular_resolution)1188 RangeImage::setAngularResolution (float angular_resolution)
1189 {
1190 angular_resolution_x_ = angular_resolution_y_ = angular_resolution;
1191 angular_resolution_x_reciprocal_ = angular_resolution_y_reciprocal_ = 1.0f / angular_resolution;
1192 }
1193
1194 /////////////////////////////////////////////////////////////////////////
1195 inline void
setAngularResolution(float angular_resolution_x,float angular_resolution_y)1196 RangeImage::setAngularResolution (float angular_resolution_x, float angular_resolution_y)
1197 {
1198 angular_resolution_x_ = angular_resolution_x;
1199 angular_resolution_x_reciprocal_ = 1.0f / angular_resolution_x_;
1200 angular_resolution_y_ = angular_resolution_y;
1201 angular_resolution_y_reciprocal_ = 1.0f / angular_resolution_y_;
1202 }
1203
1204 /////////////////////////////////////////////////////////////////////////
1205 inline void
setTransformationToRangeImageSystem(const Eigen::Affine3f & to_range_image_system)1206 RangeImage::setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system)
1207 {
1208 to_range_image_system_ = to_range_image_system;
1209 to_world_system_ = to_range_image_system_.inverse ();
1210 }
1211
1212 /////////////////////////////////////////////////////////////////////////
1213 inline void
getAngularResolution(float & angular_resolution_x,float & angular_resolution_y) const1214 RangeImage::getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const
1215 {
1216 angular_resolution_x = angular_resolution_x_;
1217 angular_resolution_y = angular_resolution_y_;
1218 }
1219
1220 /////////////////////////////////////////////////////////////////////////
1221 template <typename PointCloudType> void
integrateFarRanges(const PointCloudType & far_ranges)1222 RangeImage::integrateFarRanges (const PointCloudType& far_ranges)
1223 {
1224 float x_real, y_real, range_of_current_point;
1225 for (const auto& point: far_ranges.points)
1226 {
1227 //if (!isFinite (point)) // Check for NAN etc
1228 //continue;
1229 Vector3fMapConst current_point = point.getVector3fMap ();
1230
1231 this->getImagePoint (current_point, x_real, y_real, range_of_current_point);
1232
1233 int floor_x = static_cast<int> (pcl_lrint (std::floor (x_real))),
1234 floor_y = static_cast<int> (pcl_lrint (std::floor (y_real))),
1235 ceil_x = static_cast<int> (pcl_lrint (std::ceil (x_real))),
1236 ceil_y = static_cast<int> (pcl_lrint (std::ceil (y_real)));
1237
1238 int neighbor_x[4], neighbor_y[4];
1239 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y;
1240 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y;
1241 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y;
1242 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y;
1243
1244 for (int i=0; i<4; ++i)
1245 {
1246 int x=neighbor_x[i], y=neighbor_y[i];
1247 if (!isInImage (x, y))
1248 continue;
1249 PointWithRange& image_point = getPoint (x, y);
1250 if (!std::isfinite (image_point.range))
1251 image_point.range = std::numeric_limits<float>::infinity ();
1252 }
1253 }
1254 }
1255
1256 } // namespace pcl
1257