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&&parameters_.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