1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 */
35
36 #include <iostream>
37 #include <vector>
38 #include <pcl/keypoints/narf_keypoint.h>
39 #include <pcl/features/range_image_border_extractor.h>
40 #include <pcl/pcl_macros.h>
41 #include <pcl/common/polynomial_calculations.h>
42 #include <pcl/range_image/range_image.h>
43
44 namespace pcl
45 {
46
47 /////////////////////////////////////////////////////////////////////////
NarfKeypoint(RangeImageBorderExtractor * range_image_border_extractor,float support_size)48 NarfKeypoint::NarfKeypoint (RangeImageBorderExtractor* range_image_border_extractor, float support_size) :
49 interest_image_ (nullptr), interest_points_ (nullptr)
50 {
51 name_ = "NarfKeypoint";
52 clearData ();
53 setRangeImageBorderExtractor (range_image_border_extractor);
54 if (support_size > 0.0f)
55 parameters_.support_size = support_size;
56 }
57
58 /////////////////////////////////////////////////////////////////////////
~NarfKeypoint()59 NarfKeypoint::~NarfKeypoint ()
60 {
61 //std::cerr << __PRETTY_FUNCTION__<<" called.\n";
62 clearData ();
63 }
64
65 /////////////////////////////////////////////////////////////////////////
66 void
clearData()67 NarfKeypoint::clearData ()
68 {
69 //std::cerr << __PRETTY_FUNCTION__<<" called.\n";
70
71 for (std::size_t scale_space_idx = 1; scale_space_idx<border_extractor_scale_space_.size (); ++scale_space_idx)
72 delete border_extractor_scale_space_[scale_space_idx];
73 border_extractor_scale_space_.clear ();
74 for (std::size_t scale_space_idx = 1; scale_space_idx<range_image_scale_space_.size (); ++scale_space_idx)
75 delete range_image_scale_space_[scale_space_idx];
76 range_image_scale_space_.clear ();
77 for (std::size_t scale_space_idx = 1; scale_space_idx<interest_image_scale_space_.size (); ++scale_space_idx)
78 delete[] interest_image_scale_space_[scale_space_idx];
79 interest_image_scale_space_.clear ();
80 is_interest_point_image_.clear ();
81 delete[] interest_image_; interest_image_=nullptr;
82 delete interest_points_; interest_points_=nullptr;
83 }
84
85 /////////////////////////////////////////////////////////////////////////
86 void
setRangeImageBorderExtractor(RangeImageBorderExtractor * range_image_border_extractor)87 NarfKeypoint::setRangeImageBorderExtractor (RangeImageBorderExtractor* range_image_border_extractor)
88 {
89 clearData ();
90 range_image_border_extractor_ = range_image_border_extractor;
91 }
92
93 /////////////////////////////////////////////////////////////////////////
94 void
setRangeImage(const RangeImage * range_image)95 NarfKeypoint::setRangeImage (const RangeImage* range_image)
96 {
97 clearData ();
98 range_image_border_extractor_->setRangeImage (range_image);
99 }
100
101 /////////////////////////////////////////////////////////////////////////
102 void
calculateScaleSpace()103 NarfKeypoint::calculateScaleSpace ()
104 {
105 //MEASURE_FUNCTION_TIME;
106
107 if (range_image_border_extractor_ == nullptr || !range_image_border_extractor_->hasRangeImage () ||
108 !border_extractor_scale_space_.empty ()) // Nothing to compute or already done
109 return;
110 border_extractor_scale_space_.push_back (range_image_border_extractor_);
111 range_image_scale_space_.push_back (const_cast<RangeImage*> (reinterpret_cast<const RangeImage*> (&range_image_border_extractor_->getRangeImage ())));
112
113 if (!parameters_.use_recursive_scale_reduction)
114 return;
115
116 while (0.5f*range_image_scale_space_.back ()->getAngularResolution () < deg2rad (2.0f))
117 {
118 range_image_scale_space_.push_back (getRangeImage ().getNew ());
119 range_image_scale_space_[range_image_scale_space_.size ()-2]->getHalfImage (*range_image_scale_space_.back ());
120 border_extractor_scale_space_.push_back (new RangeImageBorderExtractor);
121 border_extractor_scale_space_.back ()->getParameters () = range_image_border_extractor_->getParameters ();
122 border_extractor_scale_space_.back ()->setRangeImage (range_image_scale_space_.back ());
123 }
124 }
125
126 #define USE_BEAM_AVERAGE 1
127
128 namespace
129 { // Some helper functions in an anonymous namespace - only available in this file
130 inline void
nkdGetScores(float distance_factor,float surface_change_score,float pixelDistance,float optimal_distance,float & negative_score,float & positive_score)131 nkdGetScores (float distance_factor, float surface_change_score, float pixelDistance,
132 float optimal_distance, float& negative_score, float& positive_score)
133 {
134 negative_score = 1.0f - 0.5f * surface_change_score * (std::max) (1.0f - distance_factor/optimal_distance, 0.0f);
135 negative_score = powf (negative_score, 2);
136
137 if (pixelDistance < 2.0)
138 positive_score = surface_change_score;
139 else
140 positive_score = surface_change_score * (1.0f-distance_factor);
141 }
142
143 inline float
nkdGetDirectionAngle(const Eigen::Vector3f & direction,const Eigen::Affine3f & rotation)144 nkdGetDirectionAngle (const Eigen::Vector3f& direction, const Eigen::Affine3f& rotation)
145 {
146 Eigen::Vector3f rotated_direction = rotation*direction;
147 Eigen::Vector2f direction_vector (rotated_direction[0], rotated_direction[1]);
148 direction_vector.normalize ();
149 float angle = 0.5f*normAngle (2.0f*std::acos (direction_vector[0]));
150 return (angle);
151 }
152
153 inline void
propagateInvalidBeams(int new_radius,std::vector<bool> & old_beams,std::vector<bool> & new_beams)154 propagateInvalidBeams (int new_radius, std::vector<bool>& old_beams, std::vector<bool>& new_beams)
155 {
156 new_beams.clear ();
157 new_beams.resize (std::max (8*new_radius,1), false);
158 if (new_radius >= 2)
159 {
160 float mapping_factor = 1.0f + (1.0f / static_cast<float> (new_radius-1));
161 for (std::size_t old_idx=0; old_idx<old_beams.size (); ++old_idx)
162 {
163 if (old_beams[old_idx])
164 {
165 int middle_idx = static_cast<int> (pcl_lrint (mapping_factor * static_cast<float> (old_idx)));
166 for (int idx_offset=-1; idx_offset<=1; ++idx_offset)
167 {
168 if (idx_offset != 0)
169 {
170 int old_neighbor_idx = static_cast<int> (old_idx) + idx_offset;
171 if (old_neighbor_idx<0)
172 old_neighbor_idx += static_cast<int> (old_beams.size ());
173 if (old_neighbor_idx>= static_cast<int> (old_beams.size ()))
174 old_neighbor_idx -= static_cast<int> (old_beams.size ());
175 if (!old_beams[old_neighbor_idx])
176 continue;
177 }
178 int new_idx = middle_idx+idx_offset;
179 if (new_idx<0)
180 new_idx += static_cast<int> (new_beams.size ());
181 if (new_idx>= static_cast<int> (new_beams.size ()))
182 new_idx -= static_cast<int> (new_beams.size ());
183 new_beams[new_idx] = true;
184 }
185 }
186 }
187 }
188 }
189
190 inline bool
isBetterInterestPoint(const InterestPoint & p1,const InterestPoint & p2)191 isBetterInterestPoint (const InterestPoint& p1, const InterestPoint& p2)
192 {
193 return (p1.strength > p2.strength);
194 }
195
196 inline bool
secondPairElementIsGreater(const std::pair<int,float> & p1,const std::pair<int,float> & p2)197 secondPairElementIsGreater (const std::pair<int, float>& p1, const std::pair<int, float>& p2)
198 {
199 return (p1.second > p2.second);
200 }
201
202 } // end anonymous namespace
203
204 void
calculateInterestImage()205 NarfKeypoint::calculateInterestImage ()
206 {
207 //std::cout << __PRETTY_FUNCTION__ << " called.\n";
208
209 if (interest_image_!=nullptr) // Already done
210 return;
211
212 if (parameters_.calculate_sparse_interest_image)
213 calculateSparseInterestImage ();
214 else
215 calculateCompleteInterestImage ();
216 }
217
218 void
calculateCompleteInterestImage()219 NarfKeypoint::calculateCompleteInterestImage ()
220 {
221 //std::cout << __PRETTY_FUNCTION__ << " called.\n";
222
223 if (parameters_.support_size <= 0.0f)
224 {
225 std::cerr << __PRETTY_FUNCTION__<<": parameters_.support_size is not set!\n";
226 return;
227 }
228 if (range_image_border_extractor_==nullptr)
229 {
230 std::cerr << __PRETTY_FUNCTION__<<": range_image_border_extractor_ is not set!\n";
231 return;
232 }
233
234 float search_radius = 0.5f * parameters_.support_size,
235 radius_squared = search_radius*search_radius,
236 radius_reciprocal = 1.0f / search_radius;
237
238 calculateScaleSpace ();
239 //std::cout << PVARN(range_image_scale_space_.size ());
240
241 std::vector<float> start_usage_ranges;
242 start_usage_ranges.resize (range_image_scale_space_.size ());
243 start_usage_ranges[int (range_image_scale_space_.size ())-1] = 0.0f;
244 for (int scale_idx = int (range_image_scale_space_.size ())-2; scale_idx >= 0; --scale_idx)
245 {
246 start_usage_ranges[scale_idx] = parameters_.support_size /
247 tanf (static_cast<float> (parameters_.optimal_range_image_patch_size) * range_image_scale_space_[scale_idx+1]->getAngularResolution ());
248 //std::cout << PVARN(start_usage_ranges[scale_idx]);
249 }
250
251 //double interest_value_calculation_start_time = getTime ();
252 interest_image_scale_space_.clear ();
253 interest_image_scale_space_.resize (range_image_scale_space_.size (), nullptr);
254 for (int scale_idx = int (range_image_scale_space_.size ())-1; scale_idx >= 0; --scale_idx)
255 {
256 const RangeImage& range_image = *range_image_scale_space_[scale_idx];
257 RangeImageBorderExtractor& border_extractor = *border_extractor_scale_space_[scale_idx];
258 int original_max_no_of_threads = border_extractor.getParameters ().max_no_of_threads;
259 border_extractor.getParameters ().max_no_of_threads = parameters_.max_no_of_threads;
260 const ::pcl::PointCloud<BorderDescription>& border_descriptions = border_extractor.getBorderDescriptions ();
261 const float* surface_change_scores = border_extractor.getSurfaceChangeScores ();
262 const Eigen::Vector3f* surface_change_directions = border_extractor.getSurfaceChangeDirections ();
263 float start_usage_range = start_usage_ranges[scale_idx];
264
265 int width = range_image.width,
266 height = range_image.height,
267 array_size = width*height;
268
269 float* interest_image = new float[array_size];
270 interest_image_scale_space_[scale_idx] = interest_image;
271 //for (int i=0; i<array_size; ++i)
272 //interest_image[i] = -1.0f;
273
274 const int angle_histogram_size = 18;
275 std::vector<float> angle_histogram;
276 angle_histogram.resize(angle_histogram_size);
277
278 std::vector<bool> was_touched;
279 was_touched.resize (array_size, false);
280 std::vector<int> neighbors_to_check;
281
282 #pragma omp parallel for \
283 default(none) \
284 shared(array_size, border_descriptions, interest_image, range_image, radius_reciprocal, radius_squared, scale_idx, \
285 surface_change_directions, surface_change_scores, start_usage_range) \
286 firstprivate(was_touched, neighbors_to_check, angle_histogram) \
287 schedule(dynamic, 10) \
288 num_threads(parameters_.max_no_of_threads)
289 for (int index=0; index<array_size; ++index)
290 {
291 float& interest_value = interest_image[index];
292 interest_value = 0.0f;
293 if (!range_image.isValid (index))
294 continue;
295 int y = index/range_image.width,
296 x = index - y*range_image.width;
297
298 const BorderTraits& border_traits = border_descriptions[index].traits;
299 if (border_traits[BORDER_TRAIT__SHADOW_BORDER] || border_traits[BORDER_TRAIT__VEIL_POINT])
300 continue;
301
302 const PointWithRange& point = range_image.getPoint (index);
303
304 if (point.range < start_usage_range) // Point is close enough that we can use the value calculated at a lower resolution
305 {
306 const RangeImage& half_range_image = *range_image_scale_space_[scale_idx+1];
307 float* half_interest_image = interest_image_scale_space_[scale_idx+1];
308 int half_x = std::min (x/2, int (half_range_image.width)-1),
309 half_y = std::min (y/2, int (half_range_image.height)-1);
310 interest_value = half_interest_image[half_y*half_range_image.width + half_x];
311 continue;
312 }
313
314 Eigen::Affine3f rotation_to_viewer_coordinate_system;
315 range_image.getRotationToViewerCoordinateFrame (point.getVector3fMap (),
316 rotation_to_viewer_coordinate_system);
317 float negative_score = 1.0f;
318
319 // -----Start region growing to cover all connected points within the support size-----
320 neighbors_to_check.clear ();
321 neighbors_to_check.push_back (index);
322 was_touched[index] = true;
323
324 angle_histogram.clear ();
325 angle_histogram.resize(angle_histogram_size, 0);
326 for (std::size_t neighbors_to_check_idx=0; neighbors_to_check_idx<neighbors_to_check.size (); ++neighbors_to_check_idx)
327 {
328 int index2 = neighbors_to_check[neighbors_to_check_idx];
329 if (!range_image.isValid (index2))
330 continue;
331 const BorderTraits& border_traits2 = border_descriptions[index2].traits;
332 if (border_traits2[BORDER_TRAIT__SHADOW_BORDER] || border_traits2[BORDER_TRAIT__VEIL_POINT])
333 continue;
334 int y2 = index2/range_image.width,
335 x2 = index2 - y2*range_image.width;
336 const PointWithRange& point2 = range_image.getPoint (index2);
337
338 float pixelDistance = static_cast<float> (std::max (std::abs (x2-x), std::abs (y2-y)));
339 float distance_squared = squaredEuclideanDistance (point, point2);
340 if (pixelDistance > 2.0f) // Always consider immediate neighbors, even if to far away
341 {
342 if (distance_squared>radius_squared)
343 continue;
344 }
345
346 for (int y3=std::max (0,y2-1); y3<=std::min (y2+1,int (range_image.height)-1); ++y3)
347 {
348 for (int x3=std::max (0,x2-1); x3<=std::min (x2+1,int (range_image.width)-1); ++x3)
349 {
350 int index3 = y3*range_image.width + x3;
351 if (!was_touched[index3])
352 {
353 neighbors_to_check.push_back (index3);
354 was_touched[index3] = true;
355 }
356 }
357 }
358
359 float surface_change_score = surface_change_scores[index2];
360 if (surface_change_score < parameters_.min_surface_change_score) // Pixel not 'interesting' enough to consider?
361 continue;
362 const Eigen::Vector3f& surface_change_direction = surface_change_directions[index2];
363
364 float distance = ::sqrt (distance_squared);
365 float distance_factor = radius_reciprocal*distance;
366 float positive_score, current_negative_score;
367 nkdGetScores (distance_factor, surface_change_score, pixelDistance,
368 parameters_.optimal_distance_to_high_surface_change,
369 current_negative_score, positive_score);
370 float angle = nkdGetDirectionAngle (surface_change_direction, rotation_to_viewer_coordinate_system);
371 int histogram_cell = (std::min) (angle_histogram_size-1,
372 static_cast<int> (pcl_lrint (std::floor ( (angle+deg2rad (90.0f))/deg2rad (180.0f) * angle_histogram_size))));
373 float& histogram_value = angle_histogram[histogram_cell];
374
375 histogram_value = (std::max) (histogram_value, positive_score);
376 negative_score = (std::min) (negative_score, current_negative_score);
377 }
378
379 // Reset was_touched to false
380 for (const int &neighbors_to_check_idx : neighbors_to_check)
381 was_touched[neighbors_to_check_idx] = false;
382
383 float angle_change_value = 0.0f;
384 for (int histogram_cell1=0; histogram_cell1<angle_histogram_size-1; ++histogram_cell1)
385 {
386 if (angle_histogram[histogram_cell1]==0.0f)
387 continue;
388 for (int histogram_cell2=histogram_cell1+1; histogram_cell2<angle_histogram_size; ++histogram_cell2)
389 {
390 if (angle_histogram[histogram_cell2]==0.0f)
391 continue;
392 // TODO: lookup table for the following:
393 float normalized_angle_diff = 2.0f*float (histogram_cell2-histogram_cell1)/float (angle_histogram_size);
394 normalized_angle_diff = (normalized_angle_diff <= 1.0f ? normalized_angle_diff : 2.0f-normalized_angle_diff);
395
396 angle_change_value = std::max (angle_histogram[histogram_cell1] * angle_histogram[histogram_cell2] *
397 normalized_angle_diff, angle_change_value);
398 }
399 }
400 angle_change_value = ::sqrt (angle_change_value);
401 interest_value = negative_score * angle_change_value;
402
403 if (parameters_.add_points_on_straight_edges)
404 {
405 float max_histogram_cell_value = 0.0f;
406 for (int histogram_cell=0; histogram_cell<angle_histogram_size; ++histogram_cell)
407 max_histogram_cell_value = (std::max) (max_histogram_cell_value, angle_histogram[histogram_cell]);
408 //std::cout << PVARN(max_histogram_cell_value);
409 interest_value = 0.5f*(interest_value+max_histogram_cell_value);
410 //std::cout << PVARN(interest_value);
411 }
412 }
413
414 border_extractor.getParameters ().max_no_of_threads = original_max_no_of_threads;
415 }
416
417 if (interest_image_scale_space_.empty ())
418 interest_image_ = nullptr;
419 else
420 interest_image_ = interest_image_scale_space_[0];
421 }
422
423 void
calculateSparseInterestImage()424 NarfKeypoint::calculateSparseInterestImage ()
425 {
426 if (parameters_.support_size <= 0.0f)
427 {
428 std::cerr << __PRETTY_FUNCTION__<<": parameters_.support_size is not set!\n";
429 return;
430 }
431 if (range_image_border_extractor_==nullptr)
432 {
433 std::cerr << __PRETTY_FUNCTION__<<": range_image_border_extractor_ is not set!\n";
434 return;
435 }
436
437 float search_radius = 0.5f * parameters_.support_size,
438 radius_reciprocal = 1.0f / search_radius,
439 increased_radius = 1.5f * search_radius,
440 increased_radius_squared = increased_radius*increased_radius,
441 radius_overhead = increased_radius-search_radius,
442 radius_overhead_squared = radius_overhead*radius_overhead;
443
444 const RangeImage& range_image = range_image_border_extractor_->getRangeImage ();
445 RangeImageBorderExtractor& border_extractor = *range_image_border_extractor_;
446 int original_max_no_of_threads = border_extractor.getParameters ().max_no_of_threads;
447 border_extractor.getParameters ().max_no_of_threads = parameters_.max_no_of_threads;
448 const ::pcl::PointCloud<BorderDescription>& border_descriptions = border_extractor.getBorderDescriptions ();
449 const float* surface_change_scores = border_extractor.getSurfaceChangeScores ();
450 const Eigen::Vector3f* surface_change_directions = border_extractor.getSurfaceChangeDirections ();
451
452 int width = range_image.width,
453 height = range_image.height,
454 array_size = width*height;
455
456 interest_image_ = new float[array_size];
457
458 for (int index=0; index<array_size; ++index)
459 {
460 interest_image_[index] = 0.0f;
461 if (!range_image.isValid (index))
462 continue;
463 const BorderTraits& border_traits = border_descriptions[index].traits;
464 if (border_traits[BORDER_TRAIT__SHADOW_BORDER] || border_traits[BORDER_TRAIT__VEIL_POINT])
465 continue;
466 interest_image_[index] = 2.0f;
467 }
468
469 const int angle_histogram_size = 18;
470 std::vector<float> angle_histogram;
471 angle_histogram.resize(angle_histogram_size);
472 std::vector<std::vector<std::pair<int, float> > > angle_elements (angle_histogram_size);
473 std::vector<bool> relevant_point_still_valid;
474
475 std::vector<bool> was_touched;
476 was_touched.resize (array_size, false);
477 std::vector<int> neighbors_to_check,
478 neighbors_within_radius_overhead;
479
480 //double interest_value_calculation_start_time = getTime ();
481 #pragma omp parallel for \
482 default(none) \
483 shared(array_size, border_descriptions, increased_radius_squared, radius_reciprocal, radius_overhead_squared, range_image, search_radius, \
484 surface_change_directions, surface_change_scores) \
485 num_threads(parameters_.max_no_of_threads) \
486 schedule(guided, 10) \
487 firstprivate(was_touched, neighbors_to_check, angle_histogram, neighbors_within_radius_overhead, angle_elements, relevant_point_still_valid)
488 for (int index=0; index<array_size; ++index)
489 {
490 if (interest_image_[index] <= 1.0f)
491 continue;
492 int y = index/range_image.width,
493 x = index - y*range_image.width;
494
495 const PointWithRange& point = range_image.getPoint (index);
496
497 Eigen::Affine3f rotation_to_viewer_coordinate_system;
498 range_image.getRotationToViewerCoordinateFrame (point.getVector3fMap (),
499 rotation_to_viewer_coordinate_system);
500
501 // -----Start region growing to cover all connected points within the support size-----
502 neighbors_to_check.clear ();
503 neighbors_to_check.push_back (index);
504 neighbors_within_radius_overhead.clear ();
505 was_touched[index] = true;
506
507 for (int angle_histogram_idx=0; angle_histogram_idx<angle_histogram_size; ++angle_histogram_idx)
508 {
509 angle_histogram[angle_histogram_idx] = 0;
510 angle_elements[angle_histogram_idx].clear ();
511 }
512
513 for (std::size_t neighbors_to_check_idx=0; neighbors_to_check_idx<neighbors_to_check.size (); ++neighbors_to_check_idx)
514 {
515 int index2 = neighbors_to_check[neighbors_to_check_idx];
516 if (!range_image.isValid (index2))
517 continue;
518 const BorderTraits& border_traits2 = border_descriptions[index2].traits;
519 if (border_traits2[BORDER_TRAIT__SHADOW_BORDER] || border_traits2[BORDER_TRAIT__VEIL_POINT])
520 continue;
521 int y2 = index2/range_image.width,
522 x2 = index2 - y2*range_image.width;
523 const PointWithRange& point2 = range_image.getPoint (index2);
524
525 float pixelDistance = static_cast<float> (std::max (std::abs (x2-x), std::abs (y2-y)));
526
527 float distance_squared = squaredEuclideanDistance (point, point2);
528 if (distance_squared <= radius_overhead_squared)
529 neighbors_within_radius_overhead.push_back (index2);
530
531 if (pixelDistance > 2.0f) // Always consider immediate neighbors, even if to far away
532 {
533 if (distance_squared>increased_radius_squared)
534 continue;
535 }
536
537 for (int y3=std::max (0,y2-1); y3<=std::min (y2+1,int (range_image.height)-1); ++y3)
538 {
539 for (int x3=std::max (0,x2-1); x3<=std::min (x2+1,int (range_image.width)-1); ++x3)
540 {
541 int index3 = y3*range_image.width + x3;
542 if (!was_touched[index3])
543 {
544 neighbors_to_check.push_back (index3);
545 was_touched[index3] = true;
546 }
547 }
548 }
549
550 float surface_change_score = surface_change_scores[index2];
551 if (surface_change_score < parameters_.min_surface_change_score) // Pixel not 'interesting' enough to consider?
552 continue;
553 const Eigen::Vector3f& surface_change_direction = surface_change_directions[index2];
554
555 float angle = nkdGetDirectionAngle (surface_change_direction, rotation_to_viewer_coordinate_system);
556 int histogram_cell = (std::min) (angle_histogram_size-1,
557 static_cast<int> (pcl_lrint (std::floor ( (angle+deg2rad (90.0f))/deg2rad (180.0f) * angle_histogram_size))));
558 float& histogram_value = angle_histogram[histogram_cell];
559 histogram_value = (std::max) (histogram_value, surface_change_score);
560 angle_elements[histogram_cell].push_back (std::make_pair(index2, surface_change_score));
561 }
562
563 // Reset was_touched to false
564 for (const int &neighbors_to_check_idx : neighbors_to_check)
565 was_touched[neighbors_to_check_idx] = false;
566
567 float angle_change_value = 0.0f;
568 for (int histogram_cell1=0; histogram_cell1<angle_histogram_size-1; ++histogram_cell1)
569 {
570 if (angle_histogram[histogram_cell1]==0.0f)
571 continue;
572 for (int histogram_cell2=histogram_cell1+1; histogram_cell2<angle_histogram_size; ++histogram_cell2)
573 {
574 if (angle_histogram[histogram_cell2]==0.0f)
575 continue;
576 // TODO: lookup table for the following:
577 float normalized_angle_diff = 2.0f*float (histogram_cell2-histogram_cell1)/float (angle_histogram_size);
578 normalized_angle_diff = (normalized_angle_diff <= 1.0f ? normalized_angle_diff : 2.0f-normalized_angle_diff);
579
580 angle_change_value = std::max (angle_histogram[histogram_cell1] * angle_histogram[histogram_cell2] *
581 normalized_angle_diff, angle_change_value);
582 }
583 }
584 angle_change_value = ::sqrt (angle_change_value);
585 float maximum_interest_value = angle_change_value;
586
587 if (parameters_.add_points_on_straight_edges)
588 {
589 float max_histogram_cell_value = 0.0f;
590 for (int histogram_cell=0; histogram_cell<angle_histogram_size; ++histogram_cell)
591 max_histogram_cell_value = (std::max) (max_histogram_cell_value, angle_histogram[histogram_cell]);
592 maximum_interest_value = 0.5f * (maximum_interest_value+max_histogram_cell_value);
593 }
594
595 // Every point in distance search_radius cannot have a higher value
596 // Therefore: if too low, set all to zero. Else calculate properly
597 if (maximum_interest_value < parameters_.min_interest_value)
598 for (const int &neighbors_idx : neighbors_within_radius_overhead)
599 interest_image_[neighbors_idx] = 0.0f;
600 else
601 {
602 // Reduce number of neighbors to go through by filtering close by points with the same angle
603 float min_distance_between_relevant_points = 0.25f * search_radius,
604 min_distance_between_relevant_points_squared = powf(min_distance_between_relevant_points, 2);
605 for (int angle_histogram_idx=0; angle_histogram_idx<angle_histogram_size; ++angle_histogram_idx)
606 {
607 std::vector<std::pair<int,float> >& relevent_point_indices = angle_elements[angle_histogram_idx];
608 std::sort(relevent_point_indices.begin(), relevent_point_indices.end(), secondPairElementIsGreater);
609 relevant_point_still_valid.clear();
610 relevant_point_still_valid.resize(relevent_point_indices.size(), true);
611 for (int rpi_idx1=0; rpi_idx1<int(relevent_point_indices.size ())-1; ++rpi_idx1)
612 {
613 if (!relevant_point_still_valid[rpi_idx1])
614 continue;
615 const PointWithRange& relevant_point1 = range_image.getPoint (relevent_point_indices[rpi_idx1].first);
616 for (int rpi_idx2=rpi_idx1+1; rpi_idx2<int(relevent_point_indices.size ()); ++rpi_idx2)
617 {
618 if (!relevant_point_still_valid[rpi_idx2])
619 continue;
620 const PointWithRange& relevant_point2 = range_image.getPoint (relevent_point_indices[rpi_idx2].first);
621 float distance_squared = (relevant_point1.getVector3fMap ()-relevant_point2.getVector3fMap ()).norm ();
622 if (distance_squared > min_distance_between_relevant_points_squared)
623 continue;
624 relevant_point_still_valid[rpi_idx2] = false;
625 }
626 }
627 int newPointIdx=0;
628 for (int oldPointIdx=0; oldPointIdx<int(relevant_point_still_valid.size()); ++oldPointIdx) {
629 if (relevant_point_still_valid[oldPointIdx])
630 relevent_point_indices[newPointIdx++] = relevent_point_indices[oldPointIdx];
631 }
632 relevent_point_indices.resize(newPointIdx);
633 }
634
635 // Caclulate interest values for neighbors
636 for (const int &index2 : neighbors_within_radius_overhead)
637 {
638 int y2 = index2/range_image.width,
639 x2 = index2 - y2*range_image.width;
640 const PointWithRange& point2 = range_image.getPoint (index2);
641 float& interest_value = interest_image_[index2];
642 if (interest_value <= 1.0f)
643 continue;
644 float negative_score = 1.0;
645
646 for (int angle_histogram_idx=0; angle_histogram_idx<angle_histogram_size; ++angle_histogram_idx)
647 {
648 float& histogram_value = angle_histogram[angle_histogram_idx];
649 histogram_value = 0;
650 const std::vector<std::pair<int,float> >& relevent_point_indices = angle_elements[angle_histogram_idx];
651 for (const auto &relevent_point_index : relevent_point_indices)
652 {
653 int index3 = relevent_point_index.first;
654 int y3 = index3/range_image.width,
655 x3 = index3 - y3*range_image.width;
656 const PointWithRange& point3 = range_image.getPoint (index3);
657 float surface_change_score = relevent_point_index.second;
658
659 float pixelDistance = static_cast<float> (std::max (std::abs (x3-x2), std::abs (y3-y2)));
660 float distance = (point3.getVector3fMap ()-point2.getVector3fMap ()).norm ();
661 float distance_factor = radius_reciprocal*distance;
662 float positive_score, current_negative_score;
663 nkdGetScores (distance_factor, surface_change_score, pixelDistance,
664 parameters_.optimal_distance_to_high_surface_change,
665 current_negative_score, positive_score);
666 histogram_value = (std::max) (histogram_value, positive_score);
667 negative_score = (std::min) (negative_score, current_negative_score);
668 }
669 }
670 float angle_change_value = 0.0f;
671 for (int histogram_cell1=0; histogram_cell1<angle_histogram_size-1; ++histogram_cell1)
672 {
673 if (angle_histogram[histogram_cell1]==0.0f)
674 continue;
675 for (int histogram_cell2=histogram_cell1+1; histogram_cell2<angle_histogram_size; ++histogram_cell2)
676 {
677 if (angle_histogram[histogram_cell2]==0.0f)
678 continue;
679 // TODO: lookup table for the following:
680 float normalized_angle_diff = 2.0f*float (histogram_cell2-histogram_cell1)/float (angle_histogram_size);
681 normalized_angle_diff = (normalized_angle_diff <= 1.0f ? normalized_angle_diff : 2.0f-normalized_angle_diff);
682 angle_change_value = std::max (angle_change_value, angle_histogram[histogram_cell1] *
683 angle_histogram[histogram_cell2] *
684 normalized_angle_diff);
685 }
686 }
687 angle_change_value = ::sqrt (angle_change_value);
688 interest_value = negative_score * angle_change_value;
689 if (parameters_.add_points_on_straight_edges)
690 {
691 float max_histogram_cell_value = 0.0f;
692 for (int histogram_cell=0; histogram_cell<angle_histogram_size; ++histogram_cell)
693 max_histogram_cell_value = (std::max) (max_histogram_cell_value, angle_histogram[histogram_cell]);
694 //std::cout << PVARN(max_histogram_cell_value);
695 interest_value = 0.5f*(interest_value+max_histogram_cell_value);
696 //std::cout << PVARN(interest_value);
697 }
698 }
699 }
700 }
701
702 border_extractor.getParameters ().max_no_of_threads = original_max_no_of_threads;
703 }
704
705 void
calculateInterestPoints()706 NarfKeypoint::calculateInterestPoints ()
707 {
708 //std::cout << __PRETTY_FUNCTION__ << " called.\n";
709
710 if (interest_points_ != nullptr)
711 return;
712
713 calculateInterestImage ();
714
715 interest_points_ = new ::pcl::PointCloud<InterestPoint>;
716
717 float max_distance_squared = powf (0.3f*parameters_.support_size, 2);
718
719 const RangeImage& range_image = range_image_border_extractor_->getRangeImage ();
720 const ::pcl::PointCloud<BorderDescription>& border_descriptions =
721 range_image_border_extractor_->getBorderDescriptions ();
722 int width = range_image.width,
723 height = range_image.height,
724 size = width*height;
725 is_interest_point_image_.clear ();
726 is_interest_point_image_.resize (size, false);
727
728 using RealForPolynomial = double;
729 PolynomialCalculationsT<RealForPolynomial> polynomial_calculations;
730 BivariatePolynomialT<RealForPolynomial> polynomial (2);
731 std::vector<Eigen::Matrix<RealForPolynomial, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<RealForPolynomial, 3, 1> > > sample_points;
732 std::vector<RealForPolynomial> x_values, y_values;
733 std::vector<int> types;
734 std::vector<bool> invalid_beams, old_invalid_beams;
735
736 pcl::PointCloud<InterestPoint>::VectorType tmp_interest_points;
737 for (int y=0; y<height; ++y)
738 {
739 for (int x=0; x<width; ++x)
740 {
741 int index = y*width + x;
742 float interest_value = interest_image_[index];
743 if (!range_image.isValid (index) || interest_value < parameters_.min_interest_value)
744 continue;
745 const PointWithRange& point = range_image.getPoint (index);
746 bool is_maximum = true;
747 for (int y2=y-1; y2<=y+1&&is_maximum&¶meters_.do_non_maximum_suppression; ++y2)
748 {
749 for (int x2=x-1; x2<=x+1; ++x2)
750 {
751 if (!range_image.isInImage (x2,y2))
752 continue;
753 int index2 = y2*width + x2;
754 float interest_value2 = interest_image_[index2];
755 if (interest_value2 <= interest_value)
756 continue;
757 is_maximum = false;
758 break;
759 }
760 }
761 if (!is_maximum)
762 continue;
763
764 PointWithRange keypoint_3d = point;
765 int keypoint_x_int=x, keypoint_y_int=y;
766
767 int no_of_polynomial_approximations_per_point = parameters_.no_of_polynomial_approximations_per_point;
768 if (!parameters_.do_non_maximum_suppression)
769 no_of_polynomial_approximations_per_point = 0;
770
771 for (int poly_step=0; poly_step<no_of_polynomial_approximations_per_point; ++poly_step)
772 {
773 sample_points.clear ();
774 invalid_beams.clear ();
775 old_invalid_beams.clear ();
776 for (int radius=0, stop=false; !stop; ++radius)
777 {
778 std::swap (invalid_beams, old_invalid_beams);
779 propagateInvalidBeams (radius, old_invalid_beams, invalid_beams);
780 int x2=keypoint_x_int-radius-1, y2=keypoint_y_int-radius; // Top left - 1
781 stop = true;
782 for (int i=0; (radius==0&&i==0) || i<8*radius; ++i)
783 {
784 if (i<=2*radius) ++x2; else if (i<=4*radius) ++y2; else if (i<=6*radius) --x2; else --y2;
785 if (invalid_beams[i] || !range_image.isValid (x2, y2))
786 continue;
787 int index2 = y2*width + x2;
788 const BorderTraits& neighbor_border_traits = border_descriptions[index2].traits;
789 if (neighbor_border_traits[BORDER_TRAIT__SHADOW_BORDER] || neighbor_border_traits[BORDER_TRAIT__VEIL_POINT])
790 {
791 invalid_beams[i] = true;
792 continue;
793 }
794 const PointWithRange& neighbor = range_image.getPoint (index2);
795 float distance_squared = squaredEuclideanDistance (point, neighbor);
796 if (distance_squared>max_distance_squared)
797 {
798 invalid_beams[i] = true;
799 continue;
800 }
801 stop = false; // There is a point in range -> Have to check further distances
802
803 float interest_value2 = interest_image_[index2];
804 sample_points.emplace_back(x2-keypoint_x_int, y2-keypoint_y_int, interest_value2);
805 }
806 }
807 if (!polynomial_calculations.bivariatePolynomialApproximation (sample_points, 2, polynomial))
808 continue;
809
810 polynomial.findCriticalPoints (x_values, y_values, types);
811
812 if (!types.empty () && types[0]==0)
813 {
814 float keypoint_x = static_cast<float> (x_values[0]+keypoint_x_int),
815 keypoint_y = static_cast<float> (y_values[0]+keypoint_y_int);
816
817 keypoint_x_int = static_cast<int> (pcl_lrint (keypoint_x));
818 keypoint_y_int = static_cast<int> (pcl_lrint (keypoint_y));
819
820 range_image.calculate3DPoint (keypoint_x, keypoint_y, keypoint_3d);
821 if (!std::isfinite (keypoint_3d.range))
822 {
823 keypoint_3d = point;
824 break;
825 }
826 }
827 else
828 {
829 break;
830 }
831 }
832
833 InterestPoint interest_point;
834 interest_point.getVector3fMap () = keypoint_3d.getVector3fMap ();
835 interest_point.strength = interest_value;
836 tmp_interest_points.push_back (interest_point);
837 }
838 }
839
840 std::sort (tmp_interest_points.begin (), tmp_interest_points.end (), isBetterInterestPoint);
841
842 float min_distance_squared = powf (parameters_.min_distance_between_interest_points*parameters_.support_size, 2);
843 for (const auto &interest_point : tmp_interest_points)
844 {
845 if (parameters_.max_no_of_interest_points > 0 && int (interest_points_->size ()) >= parameters_.max_no_of_interest_points)
846 break;
847 bool better_point_too_close = false;
848 for (const auto &interest_point2 : interest_points_->points)
849 {
850 float distance_squared = (interest_point.getVector3fMap ()-interest_point2.getVector3fMap ()).squaredNorm ();
851 if (distance_squared < min_distance_squared)
852 {
853 better_point_too_close = true;
854 break;
855 }
856 }
857 if (better_point_too_close)
858 continue;
859 interest_points_->points.push_back (interest_point);
860 int image_x, image_y;
861 //std::cout << interest_point.x<<","<<interest_point.y<<","<<interest_point.z<<", "<<std::flush;
862 range_image.getImagePoint (interest_point.getVector3fMap (), image_x, image_y);
863 if (range_image.isValid (image_x, image_y))
864 is_interest_point_image_[image_y*width + image_x] = true;
865 }
866 interest_points_->width = interest_points_->size ();
867 interest_points_->height = 1;
868 interest_points_->is_dense = true;
869 }
870
871 const RangeImage&
getRangeImage()872 NarfKeypoint::getRangeImage ()
873 {
874 return (range_image_border_extractor_->getRangeImage ());
875 }
876
877 void
detectKeypoints(NarfKeypoint::PointCloudOut & output)878 NarfKeypoint::detectKeypoints (NarfKeypoint::PointCloudOut& output)
879 {
880 output.clear ();
881
882 if (indices_)
883 {
884 std::cerr << __PRETTY_FUNCTION__
885 << ": Sorry, usage of indices for the extraction is not supported for NARF interest points (yet).\n\n";
886 return;
887 }
888
889 if (range_image_border_extractor_ == nullptr)
890 {
891 std::cerr << __PRETTY_FUNCTION__
892 << ": RangeImageBorderExtractor member is not set. "
893 << "Sorry, this is needed for the NARF keypoint extraction.\n\n";
894 return;
895 }
896
897 if (!range_image_border_extractor_->hasRangeImage ())
898 {
899 std::cerr << __PRETTY_FUNCTION__
900 << ": RangeImage is not set. Sorry, the NARF keypoint extraction works on range images, "
901 "not on normal point clouds.\n\n"
902 << " Use setRangeImage (...).\n\n";
903 return;
904 }
905
906 calculateInterestPoints ();
907
908 int size = getRangeImage ().width * getRangeImage ().height;
909
910 for (int index=0; index<size; ++index)
911 {
912 if (!is_interest_point_image_[index])
913 continue;
914 output.push_back (index);
915 }
916 }
917
918 void
compute(NarfKeypoint::PointCloudOut & output)919 NarfKeypoint::compute (NarfKeypoint::PointCloudOut& output)
920 {
921 //std::cout << __PRETTY_FUNCTION__ << " called.\n";
922 detectKeypoints (output);
923 }
924
925 } // end namespace pcl
926