Point Cloud Library (PCL)  1.7.1
surface_normal_modality.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_RECOGNITION_SURFACE_NORMAL_MODALITY
39 #define PCL_RECOGNITION_SURFACE_NORMAL_MODALITY
40 
41 #include <pcl/recognition/quantizable_modality.h>
42 #include <pcl/recognition/distance_map.h>
43 
44 #include <pcl/pcl_base.h>
45 #include <pcl/point_cloud.h>
46 #include <pcl/point_types.h>
47 #include <pcl/features/linear_least_squares_normal.h>
48 
49 namespace pcl
50 {
51 
52  /** \brief Map that stores orientations.
53  * \author Stefan Holzer
54  */
55  struct PCL_EXPORTS LINEMOD_OrientationMap
56  {
57  public:
58  /** \brief Constructor. */
59  inline LINEMOD_OrientationMap () : width_ (0), height_ (0), map_ () {}
60  /** \brief Destructor. */
62 
63  /** \brief Returns the width of the modality data map. */
64  inline size_t
65  getWidth () const
66  {
67  return width_;
68  }
69 
70  /** \brief Returns the height of the modality data map. */
71  inline size_t
72  getHeight () const
73  {
74  return height_;
75  }
76 
77  /** \brief Resizes the map to the specific width and height and initializes
78  * all new elements with the specified value.
79  * \param[in] width the width of the resized map.
80  * \param[in] height the height of the resized map.
81  * \param[in] value the value all new elements will be initialized with.
82  */
83  inline void
84  resize (const size_t width, const size_t height, const float value)
85  {
86  width_ = width;
87  height_ = height;
88  map_.clear ();
89  map_.resize (width*height, value);
90  }
91 
92  /** \brief Operator to access elements of the map.
93  * \param[in] col_index the column index of the element to access.
94  * \param[in] row_index the row index of the element to access.
95  */
96  inline float &
97  operator() (const size_t col_index, const size_t row_index)
98  {
99  return map_[row_index * width_ + col_index];
100  }
101 
102  /** \brief Operator to access elements of the map.
103  * \param[in] col_index the column index of the element to access.
104  * \param[in] row_index the row index of the element to access.
105  */
106  inline const float &
107  operator() (const size_t col_index, const size_t row_index) const
108  {
109  return map_[row_index * width_ + col_index];
110  }
111 
112  private:
113  /** \brief The width of the map. */
114  size_t width_;
115  /** \brief The height of the map. */
116  size_t height_;
117  /** \brief Storage for the data of the map. */
118  std::vector<float> map_;
119 
120  };
121 
122  /** \brief Look-up-table for fast surface normal quantization.
123  * \author Stefan Holzer
124  */
126  {
127  /** \brief The range of the LUT in x-direction. */
128  int range_x;
129  /** \brief The range of the LUT in y-direction. */
130  int range_y;
131  /** \brief The range of the LUT in z-direction. */
132  int range_z;
133 
134  /** \brief The offset in x-direction. */
135  int offset_x;
136  /** \brief The offset in y-direction. */
137  int offset_y;
138  /** \brief The offset in z-direction. */
139  int offset_z;
140 
141  /** \brief The size of the LUT in x-direction. */
142  int size_x;
143  /** \brief The size of the LUT in y-direction. */
144  int size_y;
145  /** \brief The size of the LUT in z-direction. */
146  int size_z;
147 
148  /** \brief The LUT data. */
149  unsigned char * lut;
150 
151  /** \brief Constructor. */
153  range_x (-1), range_y (-1), range_z (-1),
154  offset_x (-1), offset_y (-1), offset_z (-1),
155  size_x (-1), size_y (-1), size_z (-1), lut (NULL)
156  {}
157 
158  /** \brief Destructor. */
160  {
161  if (lut != NULL)
162  delete[] lut;
163  }
164 
165  /** \brief Initializes the LUT.
166  * \param[in] range_x_arg the range of the LUT in x-direction.
167  * \param[in] range_y_arg the range of the LUT in y-direction.
168  * \parma[in] range_z_arg the range of the LUT in z-direction.
169  */
170  void
171  initializeLUT (const int range_x_arg, const int range_y_arg, const int range_z_arg)
172  {
173  range_x = range_x_arg;
174  range_y = range_y_arg;
175  range_z = range_z_arg;
176  size_x = range_x_arg+1;
177  size_y = range_y_arg+1;
178  size_z = range_z_arg+1;
179  offset_x = range_x_arg/2;
180  offset_y = range_y_arg/2;
181  offset_z = range_z_arg;
182 
183  //if (lut != NULL) free16(lut);
184  //lut = malloc16(size_x*size_y*size_z);
185 
186  if (lut != NULL)
187  delete[] lut;
188  lut = new unsigned char[size_x*size_y*size_z];
189 
190  const int nr_normals = 8;
191  pcl::PointCloud<PointXYZ>::VectorType ref_normals (nr_normals);
192 
193  const float normal0_angle = 40.0f * 3.14f / 180.0f;
194  ref_normals[0].x = cosf (normal0_angle);
195  ref_normals[0].y = 0.0f;
196  ref_normals[0].z = -sinf (normal0_angle);
197 
198  const float inv_nr_normals = 1.0f / static_cast<float> (nr_normals);
199  for (int normal_index = 1; normal_index < nr_normals; ++normal_index)
200  {
201  const float angle = 2.0f * static_cast<float> (M_PI * normal_index * inv_nr_normals);
202 
203  ref_normals[normal_index].x = cosf (angle) * ref_normals[0].x - sinf (angle) * ref_normals[0].y;
204  ref_normals[normal_index].y = sinf (angle) * ref_normals[0].x + cosf (angle) * ref_normals[0].y;
205  ref_normals[normal_index].z = ref_normals[0].z;
206  }
207 
208  // normalize normals
209  for (int normal_index = 0; normal_index < nr_normals; ++normal_index)
210  {
211  const float length = sqrtf (ref_normals[normal_index].x * ref_normals[normal_index].x +
212  ref_normals[normal_index].y * ref_normals[normal_index].y +
213  ref_normals[normal_index].z * ref_normals[normal_index].z);
214 
215  const float inv_length = 1.0f / length;
216 
217  ref_normals[normal_index].x *= inv_length;
218  ref_normals[normal_index].y *= inv_length;
219  ref_normals[normal_index].z *= inv_length;
220  }
221 
222  // set LUT
223  for (int z_index = 0; z_index < size_z; ++z_index)
224  {
225  for (int y_index = 0; y_index < size_y; ++y_index)
226  {
227  for (int x_index = 0; x_index < size_x; ++x_index)
228  {
229  PointXYZ normal (static_cast<float> (x_index - range_x/2),
230  static_cast<float> (y_index - range_y/2),
231  static_cast<float> (z_index - range_z));
232  const float length = sqrtf (normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
233  const float inv_length = 1.0f / (length + 0.00001f);
234 
235  normal.x *= inv_length;
236  normal.y *= inv_length;
237  normal.z *= inv_length;
238 
239  float max_response = -1.0f;
240  int max_index = -1;
241 
242  for (int normal_index = 0; normal_index < nr_normals; ++normal_index)
243  {
244  const float response = normal.x * ref_normals[normal_index].x +
245  normal.y * ref_normals[normal_index].y +
246  normal.z * ref_normals[normal_index].z;
247 
248  const float abs_response = fabsf (response);
249  if (max_response < abs_response)
250  {
251  max_response = abs_response;
252  max_index = normal_index;
253  }
254 
255  lut[z_index*size_y*size_x + y_index*size_x + x_index] = static_cast<unsigned char> (0x1 << max_index);
256  }
257  }
258  }
259  }
260  }
261 
262  /** \brief Operator to access an element in the LUT.
263  * \param[in] x the x-component of the normal.
264  * \param[in] y the y-component of the normal.
265  * \param[in] z the z-component of the normal.
266  */
267  inline unsigned char
268  operator() (const float x, const float y, const float z) const
269  {
270  const size_t x_index = static_cast<size_t> (x * static_cast<float> (offset_x) + static_cast<float> (offset_x));
271  const size_t y_index = static_cast<size_t> (y * static_cast<float> (offset_y) + static_cast<float> (offset_y));
272  const size_t z_index = static_cast<size_t> (z * static_cast<float> (range_z) + static_cast<float> (range_z));
273 
274  const size_t index = z_index*size_y*size_x + y_index*size_x + x_index;
275 
276  return (lut[index]);
277  }
278 
279  /** \brief Operator to access an element in the LUT.
280  * \param[in] index the index of the element.
281  */
282  inline unsigned char
283  operator() (const int index) const
284  {
285  return (lut[index]);
286  }
287  };
288 
289 
290  /** \brief Modality based on surface normals.
291  * \author Stefan Holzer
292  */
293  template <typename PointInT>
294  class SurfaceNormalModality : public QuantizableModality, public PCLBase<PointInT>
295  {
296  protected:
298 
299  /** \brief Candidate for a feature (used in feature extraction methods). */
300  struct Candidate
301  {
302  /** \brief Constructor. */
303  Candidate () : normal (), distance (0.0f), bin_index (0), x (0), y (0) {}
304 
305  /** \brief Normal. */
307  /** \brief Distance to the next different quantized value. */
308  float distance;
309 
310  /** \brief Quantized value. */
311  unsigned char bin_index;
312 
313  /** \brief x-position of the feature. */
314  size_t x;
315  /** \brief y-position of the feature. */
316  size_t y;
317 
318  /** \brief Compares two candidates based on their distance to the next different quantized value.
319  * \param[in] rhs the candidate to compare with.
320  */
321  bool
322  operator< (const Candidate & rhs)
323  {
324  return (distance > rhs.distance);
325  }
326  };
327 
328  public:
330 
331  /** \brief Constructor. */
333  /** \brief Destructor. */
334  virtual ~SurfaceNormalModality ();
335 
336  /** \brief Sets the spreading size.
337  * \param[in] spreading_size the spreading size.
338  */
339  inline void
340  setSpreadingSize (const size_t spreading_size)
341  {
342  spreading_size_ = spreading_size;
343  }
344 
345  /** \brief Enables/disables the use of extracting a variable number of features.
346  * \param[in] enabled specifies whether extraction of a variable number of features will be enabled/disabled.
347  */
348  inline void
349  setVariableFeatureNr (const bool enabled)
350  {
351  variable_feature_nr_ = enabled;
352  }
353 
354  /** \brief Returns the surface normals. */
357  {
358  return surface_normals_;
359  }
360 
361  /** \brief Returns the surface normals. */
362  inline const pcl::PointCloud<pcl::Normal> &
364  {
365  return surface_normals_;
366  }
367 
368  /** \brief Returns a reference to the internal quantized map. */
369  inline QuantizedMap &
371  {
372  return (filtered_quantized_surface_normals_);
373  }
374 
375  /** \brief Returns a reference to the internal spreaded quantized map. */
376  inline QuantizedMap &
378  {
379  return (spreaded_quantized_surface_normals_);
380  }
381 
382  /** \brief Returns a reference to the orientation map. */
383  inline LINEMOD_OrientationMap &
385  {
386  return (surface_normal_orientations_);
387  }
388 
389  /** \brief Extracts features from this modality within the specified mask.
390  * \param[in] mask defines the areas where features are searched in.
391  * \param[in] nr_features defines the number of features to be extracted
392  * (might be less if not sufficient information is present in the modality).
393  * \param[in] modality_index the index which is stored in the extracted features.
394  * \param[out] features the destination for the extracted features.
395  */
396  void
397  extractFeatures (const MaskMap & mask, size_t nr_features, size_t modality_index,
398  std::vector<QuantizedMultiModFeature> & features) const;
399 
400  /** \brief Extracts all possible features from the modality within the specified mask.
401  * \param[in] mask defines the areas where features are searched in.
402  * \param[in] nr_features IGNORED (TODO: remove this parameter).
403  * \param[in] modality_index the index which is stored in the extracted features.
404  * \param[out] features the destination for the extracted features.
405  */
406  void
407  extractAllFeatures (const MaskMap & mask, size_t nr_features, size_t modality_index,
408  std::vector<QuantizedMultiModFeature> & features) const;
409 
410  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
411  * \param[in] cloud the const boost shared pointer to a PointCloud message
412  */
413  virtual void
414  setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
415  {
416  input_ = cloud;
417  }
418 
419  /** \brief Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading). */
420  virtual void
421  processInputData ();
422 
423  /** \brief Processes the input data assuming that everything up to filtering is already done/available
424  * (so only spreading is performed). */
425  virtual void
427 
428  protected:
429 
430  /** \brief Computes the surface normals from the input cloud. */
431  void
433 
434  /** \brief Computes and quantizes the surface normals. */
435  void
437 
438  /** \brief Computes and quantizes the surface normals. */
439  void
441 
442  /** \brief Quantizes the surface normals. */
443  void
445 
446  /** \brief Filters the quantized surface normals. */
447  void
449 
450  /** \brief Computes a distance map from the supplied input mask.
451  * \param[in] input the mask for which a distance map will be computed.
452  * \param[out] output the destination for the distance map.
453  */
454  void
455  computeDistanceMap (const MaskMap & input, DistanceMap & output) const;
456 
457  private:
458 
459  /** \brief Determines whether variable numbers of features are extracted or not. */
460  bool variable_feature_nr_;
461 
462  /** \brief The feature distance threshold. */
463  float feature_distance_threshold_;
464  /** \brief Minimum distance of a feature to a border. */
465  float min_distance_to_border_;
466 
467  /** \brief Look-up-table for quantizing surface normals. */
468  QuantizedNormalLookUpTable normal_lookup_;
469 
470  /** \brief The spreading size. */
471  size_t spreading_size_;
472 
473  /** \brief Point cloud holding the computed surface normals. */
474  pcl::PointCloud<pcl::Normal> surface_normals_;
475  /** \brief Quantized surface normals. */
476  pcl::QuantizedMap quantized_surface_normals_;
477  /** \brief Filtered quantized surface normals. */
478  pcl::QuantizedMap filtered_quantized_surface_normals_;
479  /** \brief Spreaded quantized surface normals. */
480  pcl::QuantizedMap spreaded_quantized_surface_normals_;
481 
482  /** \brief Map containing surface normal orientations. */
483  pcl::LINEMOD_OrientationMap surface_normal_orientations_;
484 
485  };
486 
487 }
488 
489 //////////////////////////////////////////////////////////////////////////////////////////////
490 template <typename PointInT>
493  : variable_feature_nr_ (false)
494  , feature_distance_threshold_ (2.0f)
495  , min_distance_to_border_ (2.0f)
496  , normal_lookup_ ()
497  , spreading_size_ (8)
498  , surface_normals_ ()
499  , quantized_surface_normals_ ()
500  , filtered_quantized_surface_normals_ ()
501  , spreaded_quantized_surface_normals_ ()
502  , surface_normal_orientations_ ()
503 {
504 }
505 
506 //////////////////////////////////////////////////////////////////////////////////////////////
507 template <typename PointInT>
509 {
510 }
511 
512 //////////////////////////////////////////////////////////////////////////////////////////////
513 template <typename PointInT> void
515 {
516  // compute surface normals
517  //computeSurfaceNormals ();
518 
519  // quantize surface normals
520  //quantizeSurfaceNormals ();
521 
522  computeAndQuantizeSurfaceNormals2 ();
523 
524  // filter quantized surface normals
525  filterQuantizedSurfaceNormals ();
526 
527  // spread quantized surface normals
528  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_,
529  spreaded_quantized_surface_normals_,
530  spreading_size_);
531 }
532 
533 //////////////////////////////////////////////////////////////////////////////////////////////
534 template <typename PointInT> void
536 {
537  // spread quantized surface normals
538  pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_,
539  spreaded_quantized_surface_normals_,
540  spreading_size_);
541 }
542 
543 //////////////////////////////////////////////////////////////////////////////////////////////
544 template <typename PointInT> void
546 {
547  // compute surface normals
549  ne.setMaxDepthChangeFactor(0.05f);
550  ne.setNormalSmoothingSize(5.0f);
551  ne.setDepthDependentSmoothing(false);
552  ne.setInputCloud (input_);
553  ne.compute (surface_normals_);
554 }
555 
556 //////////////////////////////////////////////////////////////////////////////////////////////
557 template <typename PointInT> void
559 {
560  // compute surface normals
561  //pcl::LinearLeastSquaresNormalEstimation<PointInT, pcl::Normal> ne;
562  //ne.setMaxDepthChangeFactor(0.05f);
563  //ne.setNormalSmoothingSize(5.0f);
564  //ne.setDepthDependentSmoothing(false);
565  //ne.setInputCloud (input_);
566  //ne.compute (surface_normals_);
567 
568 
569  const float bad_point = std::numeric_limits<float>::quiet_NaN ();
570 
571  const int width = input_->width;
572  const int height = input_->height;
573 
574  surface_normals_.resize (width*height);
575  surface_normals_.width = width;
576  surface_normals_.height = height;
577  surface_normals_.is_dense = false;
578 
579  quantized_surface_normals_.resize (width, height);
580 
581  // we compute the normals as follows:
582  // ----------------------------------
583  //
584  // for the depth-gradient you can make the following first-order Taylor approximation:
585  // D(x + dx) - D(x) = dx^T \Delta D + h.o.t.
586  //
587  // build linear system by stacking up equation for 8 neighbor points:
588  // Y = X \Delta D
589  //
590  // => \Delta D = (X^T X)^{-1} X^T Y
591  // => \Delta D = (A)^{-1} b
592 
593  for (int y = 0; y < height; ++y)
594  {
595  for (int x = 0; x < width; ++x)
596  {
597  const int index = y * width + x;
598 
599  const float px = input_->points[index].x;
600  const float py = input_->points[index].y;
601  const float pz = input_->points[index].z;
602 
603  if (pcl_isnan(px) || pz > 2.0f)
604  {
605  surface_normals_.points[index].normal_x = bad_point;
606  surface_normals_.points[index].normal_y = bad_point;
607  surface_normals_.points[index].normal_z = bad_point;
608  surface_normals_.points[index].curvature = bad_point;
609 
610  quantized_surface_normals_ (x, y) = 0;
611 
612  continue;
613  }
614 
615  const int smoothingSizeInt = 5;
616 
617  float matA0 = 0.0f;
618  float matA1 = 0.0f;
619  float matA3 = 0.0f;
620 
621  float vecb0 = 0.0f;
622  float vecb1 = 0.0f;
623 
624  for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
625  {
626  for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
627  {
628  if (u < 0 || u >= width || v < 0 || v >= height) continue;
629 
630  const size_t index2 = v * width + u;
631 
632  const float qx = input_->points[index2].x;
633  const float qy = input_->points[index2].y;
634  const float qz = input_->points[index2].z;
635 
636  if (pcl_isnan(qx)) continue;
637 
638  const float delta = qz - pz;
639  const float i = qx - px;
640  const float j = qy - py;
641 
642  const float f = fabs(delta) < 0.05f ? 1.0f : 0.0f;
643 
644  matA0 += f * i * i;
645  matA1 += f * i * j;
646  matA3 += f * j * j;
647  vecb0 += f * i * delta;
648  vecb1 += f * j * delta;
649  }
650  }
651 
652  const float det = matA0 * matA3 - matA1 * matA1;
653  const float ddx = matA3 * vecb0 - matA1 * vecb1;
654  const float ddy = -matA1 * vecb0 + matA0 * vecb1;
655 
656  const float nx = ddx;
657  const float ny = ddy;
658  const float nz = -det * pz;
659 
660  const float length = nx * nx + ny * ny + nz * nz;
661 
662  if (length <= 0.0f)
663  {
664  surface_normals_.points[index].normal_x = bad_point;
665  surface_normals_.points[index].normal_y = bad_point;
666  surface_normals_.points[index].normal_z = bad_point;
667  surface_normals_.points[index].curvature = bad_point;
668 
669  quantized_surface_normals_ (x, y) = 0;
670  }
671  else
672  {
673  const float normInv = 1.0f / sqrtf (length);
674 
675  const float normal_x = nx * normInv;
676  const float normal_y = ny * normInv;
677  const float normal_z = nz * normInv;
678 
679  surface_normals_.points[index].normal_x = normal_x;
680  surface_normals_.points[index].normal_y = normal_y;
681  surface_normals_.points[index].normal_z = normal_z;
682  surface_normals_.points[index].curvature = bad_point;
683 
684  float angle = 11.25f + atan2 (normal_y, normal_x)*180.0f/3.14f;
685 
686  if (angle < 0.0f) angle += 360.0f;
687  if (angle >= 360.0f) angle -= 360.0f;
688 
689  int bin_index = static_cast<int> (angle*8.0f/360.0f) & 7;
690 
691  quantized_surface_normals_ (x, y) = static_cast<unsigned char> (bin_index);
692  }
693  }
694  }
695 }
696 
697 
698 //////////////////////////////////////////////////////////////////////////////////////////////
699 // Contains GRANULARITY and NORMAL_LUT
700 //#include "normal_lut.i"
701 
702 static void accumBilateral(long delta, long i, long j, long * A, long * b, int threshold)
703 {
704  long f = std::abs(delta) < threshold ? 1 : 0;
705 
706  const long fi = f * i;
707  const long fj = f * j;
708 
709  A[0] += fi * i;
710  A[1] += fi * j;
711  A[3] += fj * j;
712  b[0] += fi * delta;
713  b[1] += fj * delta;
714 }
715 
716 /**
717  * \brief Compute quantized normal image from depth image.
718  *
719  * Implements section 2.6 "Extension to Dense Depth Sensors."
720  *
721  * \param[in] src The source 16-bit depth image (in mm).
722  * \param[out] dst The destination 8-bit image. Each bit represents one bin of
723  * the view cone.
724  * \param distance_threshold Ignore pixels beyond this distance.
725  * \param difference_threshold When computing normals, ignore contributions of pixels whose
726  * depth difference with the central pixel is above this threshold.
727  *
728  * \todo Should also need camera model, or at least focal lengths? Replace distance_threshold with mask?
729  */
730 template <typename PointInT> void
732 {
733  const int width = input_->width;
734  const int height = input_->height;
735 
736  unsigned short * lp_depth = new unsigned short[width*height];
737  unsigned char * lp_normals = new unsigned char[width*height];
738  memset (lp_normals, 0, width*height);
739 
740  surface_normal_orientations_.resize (width, height, 0.0f);
741 
742  for (size_t row_index = 0; row_index < height; ++row_index)
743  {
744  for (size_t col_index = 0; col_index < width; ++col_index)
745  {
746  const float value = input_->points[row_index*width + col_index].z;
747  if (pcl_isfinite (value))
748  {
749  lp_depth[row_index*width + col_index] = static_cast<unsigned short> (value * 1000.0f);
750  }
751  else
752  {
753  lp_depth[row_index*width + col_index] = 0;
754  }
755  }
756  }
757 
758  const int l_W = width;
759  const int l_H = height;
760 
761  const int l_r = 5; // used to be 7
762  //const int l_offset0 = -l_r - l_r * l_W;
763  //const int l_offset1 = 0 - l_r * l_W;
764  //const int l_offset2 = +l_r - l_r * l_W;
765  //const int l_offset3 = -l_r;
766  //const int l_offset4 = +l_r;
767  //const int l_offset5 = -l_r + l_r * l_W;
768  //const int l_offset6 = 0 + l_r * l_W;
769  //const int l_offset7 = +l_r + l_r * l_W;
770 
771  const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
772  const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
773  const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
774  , offsets_i[1] + offsets_j[1] * l_W
775  , offsets_i[2] + offsets_j[2] * l_W
776  , offsets_i[3] + offsets_j[3] * l_W
777  , offsets_i[4] + offsets_j[4] * l_W
778  , offsets_i[5] + offsets_j[5] * l_W
779  , offsets_i[6] + offsets_j[6] * l_W
780  , offsets_i[7] + offsets_j[7] * l_W };
781 
782 
783  //const int l_offsetx = GRANULARITY / 2;
784  //const int l_offsety = GRANULARITY / 2;
785 
786  const int difference_threshold = 50;
787  const int distance_threshold = 2000;
788 
789  //const double scale = 1000.0;
790  //const double difference_threshold = 0.05 * scale;
791  //const double distance_threshold = 2.0 * scale;
792 
793  for (int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
794  {
795  unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
796  unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
797 
798  for (int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
799  {
800  long l_d = lp_line[0];
801  //float l_d = input_->points[(l_y * l_W + l_r) + l_x].z;
802  //float px = input_->points[(l_y * l_W + l_r) + l_x].x;
803  //float py = input_->points[(l_y * l_W + l_r) + l_x].y;
804 
805  if (l_d < distance_threshold)
806  {
807  // accum
808  long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
809  long l_b[2]; l_b[0] = l_b[1] = 0;
810  //double l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
811  //double l_b[2]; l_b[0] = l_b[1] = 0;
812 
813  accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
814  accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
815  accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
816  accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
817  accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
818  accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
819  accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
820  accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
821 
822  //for (size_t index = 0; index < 8; ++index)
823  //{
824  // //accumBilateral(lp_line[offsets[index]] - l_d, offsets_i[index], offsets_j[index], l_A, l_b, difference_threshold);
825 
826  // //const long delta = lp_line[offsets[index]] - l_d;
827  // //const long i = offsets_i[index];
828  // //const long j = offsets_j[index];
829  // //long * A = l_A;
830  // //long * b = l_b;
831  // //const int threshold = difference_threshold;
832 
833  // //const long f = std::abs(delta) < threshold ? 1 : 0;
834 
835  // //const long fi = f * i;
836  // //const long fj = f * j;
837 
838  // //A[0] += fi * i;
839  // //A[1] += fi * j;
840  // //A[3] += fj * j;
841  // //b[0] += fi * delta;
842  // //b[1] += fj * delta;
843 
844 
845  // const double delta = 1000.0f * (input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].z - l_d);
846  // const double i = offsets_i[index];
847  // const double j = offsets_j[index];
848  // //const float i = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].x - px;//offsets_i[index];
849  // //const float j = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].y - py;//offsets_j[index];
850  // double * A = l_A;
851  // double * b = l_b;
852  // const double threshold = difference_threshold;
853 
854  // const double f = std::fabs(delta) < threshold ? 1.0f : 0.0f;
855 
856  // const double fi = f * i;
857  // const double fj = f * j;
858 
859  // A[0] += fi * i;
860  // A[1] += fi * j;
861  // A[3] += fj * j;
862  // b[0] += fi * delta;
863  // b[1] += fj * delta;
864  //}
865 
866  //long f = std::abs(delta) < threshold ? 1 : 0;
867 
868  //const long fi = f * i;
869  //const long fj = f * j;
870 
871  //A[0] += fi * i;
872  //A[1] += fi * j;
873  //A[3] += fj * j;
874  //b[0] += fi * delta;
875  //b[1] += fj * delta;
876 
877 
878  // solve
879  long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
880  long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
881  long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
882 
883  /// @todo Magic number 1150 is focal length? This is something like
884  /// f in SXGA mode, but in VGA is more like 530.
885  float l_nx = static_cast<float>(1150 * l_ddx);
886  float l_ny = static_cast<float>(1150 * l_ddy);
887  float l_nz = static_cast<float>(-l_det * l_d);
888 
889  //// solve
890  //double l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
891  //double l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
892  //double l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
893 
894  ///// @todo Magic number 1150 is focal length? This is something like
895  ///// f in SXGA mode, but in VGA is more like 530.
896  //const double dummy_focal_length = 1150.0f;
897  //double l_nx = l_ddx * dummy_focal_length;
898  //double l_ny = l_ddy * dummy_focal_length;
899  //double l_nz = -l_det * l_d;
900 
901  float l_sqrt = sqrtf (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
902 
903  if (l_sqrt > 0)
904  {
905  float l_norminv = 1.0f / (l_sqrt);
906 
907  l_nx *= l_norminv;
908  l_ny *= l_norminv;
909  l_nz *= l_norminv;
910 
911  float angle = 22.5f + atan2f (l_ny, l_nx) * 180.0f / 3.14f;
912 
913  if (angle < 0.0f) angle += 360.0f;
914  if (angle >= 360.0f) angle -= 360.0f;
915 
916  int bin_index = static_cast<int> (angle*8.0f/360.0f) & 7;
917 
918  surface_normal_orientations_ (l_x, l_y) = angle;
919 
920  //*lp_norm = fabs(l_nz)*255;
921 
922  //int l_val1 = static_cast<int>(l_nx * l_offsetx + l_offsetx);
923  //int l_val2 = static_cast<int>(l_ny * l_offsety + l_offsety);
924  //int l_val3 = static_cast<int>(l_nz * GRANULARITY + GRANULARITY);
925 
926  //*lp_norm = NORMAL_LUT[l_val3][l_val2][l_val1];
927  *lp_norm = static_cast<unsigned char> (0x1 << bin_index);
928  }
929  else
930  {
931  *lp_norm = 0; // Discard shadows from depth sensor
932  }
933  }
934  else
935  {
936  *lp_norm = 0; //out of depth
937  }
938  ++lp_line;
939  ++lp_norm;
940  }
941  }
942  /*cvSmooth(m_dep[0], m_dep[0], CV_MEDIAN, 5, 5);*/
943 
944  unsigned char map[255];
945  memset(map, 0, 255);
946 
947  map[0x1<<0] = 0;
948  map[0x1<<1] = 1;
949  map[0x1<<2] = 2;
950  map[0x1<<3] = 3;
951  map[0x1<<4] = 4;
952  map[0x1<<5] = 5;
953  map[0x1<<6] = 6;
954  map[0x1<<7] = 7;
955 
956  quantized_surface_normals_.resize (width, height);
957  for (size_t row_index = 0; row_index < height; ++row_index)
958  {
959  for (size_t col_index = 0; col_index < width; ++col_index)
960  {
961  quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
962  }
963  }
964 
965  delete[] lp_depth;
966  delete[] lp_normals;
967 }
968 
969 
970 //////////////////////////////////////////////////////////////////////////////////////////////
971 template <typename PointInT> void
973  const size_t nr_features,
974  const size_t modality_index,
975  std::vector<QuantizedMultiModFeature> & features) const
976 {
977  const size_t width = mask.getWidth ();
978  const size_t height = mask.getHeight ();
979 
980  //cv::Mat maskImage(height, width, CV_8U, mask.mask);
981  //cv::erode(maskImage, maskImage
982 
983  // create distance maps for every quantization value
984  //cv::Mat distance_maps[8];
985  //for (int map_index = 0; map_index < 8; ++map_index)
986  //{
987  // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U);
988  //}
989 
990  MaskMap mask_maps[8];
991  for (size_t map_index = 0; map_index < 8; ++map_index)
992  mask_maps[map_index].resize (width, height);
993 
994  unsigned char map[255];
995  memset(map, 0, 255);
996 
997  map[0x1<<0] = 0;
998  map[0x1<<1] = 1;
999  map[0x1<<2] = 2;
1000  map[0x1<<3] = 3;
1001  map[0x1<<4] = 4;
1002  map[0x1<<5] = 5;
1003  map[0x1<<6] = 6;
1004  map[0x1<<7] = 7;
1005 
1006  QuantizedMap distance_map_indices (width, height);
1007  //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
1008 
1009  for (size_t row_index = 0; row_index < height; ++row_index)
1010  {
1011  for (size_t col_index = 0; col_index < width; ++col_index)
1012  {
1013  if (mask (col_index, row_index) != 0)
1014  {
1015  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1016  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1017 
1018  if (quantized_value == 0)
1019  continue;
1020  const int dist_map_index = map[quantized_value];
1021 
1022  distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1023  //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
1024  mask_maps[dist_map_index] (col_index, row_index) = 255;
1025  }
1026  }
1027  }
1028 
1029  DistanceMap distance_maps[8];
1030  for (int map_index = 0; map_index < 8; ++map_index)
1031  computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1032 
1033  DistanceMap mask_distance_maps;
1034  computeDistanceMap (mask, mask_distance_maps);
1035 
1036  std::list<Candidate> list1;
1037  std::list<Candidate> list2;
1038 
1039  float weights[8] = {0,0,0,0,0,0,0,0};
1040 
1041  const size_t off = 4;
1042  for (size_t row_index = off; row_index < height-off; ++row_index)
1043  {
1044  for (size_t col_index = off; col_index < width-off; ++col_index)
1045  {
1046  if (mask (col_index, row_index) != 0)
1047  {
1048  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1049  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1050 
1051  //const float nx = surface_normals_ (col_index, row_index).normal_x;
1052  //const float ny = surface_normals_ (col_index, row_index).normal_y;
1053  //const float nz = surface_normals_ (col_index, row_index).normal_z;
1054 
1055  if (quantized_value != 0)// && !(pcl_isnan (nx) || pcl_isnan (ny) || pcl_isnan (nz)))
1056  {
1057  const int distance_map_index = map[quantized_value];
1058 
1059  //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
1060  const float distance = distance_maps[distance_map_index] (col_index, row_index);
1061  const float distance_to_border = mask_distance_maps (col_index, row_index);
1062 
1063  if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1064  {
1065  Candidate candidate;
1066 
1067  candidate.distance = distance;
1068  candidate.x = col_index;
1069  candidate.y = row_index;
1070  candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1071 
1072  list1.push_back (candidate);
1073 
1074  ++weights[distance_map_index];
1075  }
1076  }
1077  }
1078  }
1079  }
1080 
1081  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1082  iter->distance *= 1.0f / weights[iter->bin_index];
1083 
1084  list1.sort ();
1085 
1086  if (variable_feature_nr_)
1087  {
1088  int distance = static_cast<int> (list1.size ());
1089  bool feature_selection_finished = false;
1090  while (!feature_selection_finished)
1091  {
1092  const int sqr_distance = distance*distance;
1093  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1094  {
1095  bool candidate_accepted = true;
1096  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1097  {
1098  const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1099  const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1100  const int tmp_distance = dx*dx + dy*dy;
1101 
1102  if (tmp_distance < sqr_distance)
1103  {
1104  candidate_accepted = false;
1105  break;
1106  }
1107  }
1108 
1109 
1110  float min_min_sqr_distance = std::numeric_limits<float>::max ();
1111  float max_min_sqr_distance = 0;
1112  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1113  {
1114  float min_sqr_distance = std::numeric_limits<float>::max ();
1115  for (typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
1116  {
1117  if (iter2 == iter3)
1118  continue;
1119 
1120  const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
1121  const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
1122 
1123  const float sqr_distance = dx*dx + dy*dy;
1124 
1125  if (sqr_distance < min_sqr_distance)
1126  {
1127  min_sqr_distance = sqr_distance;
1128  }
1129 
1130  //std::cerr << min_sqr_distance;
1131  }
1132  //std::cerr << std::endl;
1133 
1134  // check current feature
1135  {
1136  const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter1->x);
1137  const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter1->y);
1138 
1139  const float sqr_distance = dx*dx + dy*dy;
1140 
1141  if (sqr_distance < min_sqr_distance)
1142  {
1143  min_sqr_distance = sqr_distance;
1144  }
1145  }
1146 
1147  if (min_sqr_distance < min_min_sqr_distance)
1148  min_min_sqr_distance = min_sqr_distance;
1149  if (min_sqr_distance > max_min_sqr_distance)
1150  max_min_sqr_distance = min_sqr_distance;
1151 
1152  //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl;
1153  }
1154 
1155  if (candidate_accepted)
1156  {
1157  //std::cerr << "feature_index: " << list2.size () << std::endl;
1158  //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl;
1159  //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl;
1160 
1161  if (min_min_sqr_distance < 50)
1162  {
1163  feature_selection_finished = true;
1164  break;
1165  }
1166 
1167  list2.push_back (*iter1);
1168  }
1169 
1170  //if (list2.size () == nr_features)
1171  //{
1172  // feature_selection_finished = true;
1173  // break;
1174  //}
1175  }
1176  --distance;
1177  }
1178  }
1179  else
1180  {
1181  if (list1.size () <= nr_features)
1182  {
1183  features.reserve (list1.size ());
1184  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1185  {
1186  QuantizedMultiModFeature feature;
1187 
1188  feature.x = static_cast<int> (iter->x);
1189  feature.y = static_cast<int> (iter->y);
1190  feature.modality_index = modality_index;
1191  feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1192 
1193  features.push_back (feature);
1194  }
1195 
1196  return;
1197  }
1198 
1199  int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:!
1200  while (list2.size () != nr_features)
1201  {
1202  const int sqr_distance = distance*distance;
1203  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1204  {
1205  bool candidate_accepted = true;
1206 
1207  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1208  {
1209  const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1210  const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1211  const int tmp_distance = dx*dx + dy*dy;
1212 
1213  if (tmp_distance < sqr_distance)
1214  {
1215  candidate_accepted = false;
1216  break;
1217  }
1218  }
1219 
1220  if (candidate_accepted)
1221  list2.push_back (*iter1);
1222 
1223  if (list2.size () == nr_features) break;
1224  }
1225  --distance;
1226  }
1227  }
1228 
1229  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1230  {
1231  QuantizedMultiModFeature feature;
1232 
1233  feature.x = static_cast<int> (iter2->x);
1234  feature.y = static_cast<int> (iter2->y);
1235  feature.modality_index = modality_index;
1236  feature.quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1237 
1238  features.push_back (feature);
1239  }
1240 }
1241 
1242 //////////////////////////////////////////////////////////////////////////////////////////////
1243 template <typename PointInT> void
1245  const MaskMap & mask, const size_t, const size_t modality_index,
1246  std::vector<QuantizedMultiModFeature> & features) const
1247 {
1248  const size_t width = mask.getWidth ();
1249  const size_t height = mask.getHeight ();
1250 
1251  //cv::Mat maskImage(height, width, CV_8U, mask.mask);
1252  //cv::erode(maskImage, maskImage
1253 
1254  // create distance maps for every quantization value
1255  //cv::Mat distance_maps[8];
1256  //for (int map_index = 0; map_index < 8; ++map_index)
1257  //{
1258  // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U);
1259  //}
1260 
1261  MaskMap mask_maps[8];
1262  for (size_t map_index = 0; map_index < 8; ++map_index)
1263  mask_maps[map_index].resize (width, height);
1264 
1265  unsigned char map[255];
1266  memset(map, 0, 255);
1267 
1268  map[0x1<<0] = 0;
1269  map[0x1<<1] = 1;
1270  map[0x1<<2] = 2;
1271  map[0x1<<3] = 3;
1272  map[0x1<<4] = 4;
1273  map[0x1<<5] = 5;
1274  map[0x1<<6] = 6;
1275  map[0x1<<7] = 7;
1276 
1277  QuantizedMap distance_map_indices (width, height);
1278  //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
1279 
1280  for (size_t row_index = 0; row_index < height; ++row_index)
1281  {
1282  for (size_t col_index = 0; col_index < width; ++col_index)
1283  {
1284  if (mask (col_index, row_index) != 0)
1285  {
1286  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1287  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1288 
1289  if (quantized_value == 0)
1290  continue;
1291  const int dist_map_index = map[quantized_value];
1292 
1293  distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1294  //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
1295  mask_maps[dist_map_index] (col_index, row_index) = 255;
1296  }
1297  }
1298  }
1299 
1300  DistanceMap distance_maps[8];
1301  for (int map_index = 0; map_index < 8; ++map_index)
1302  computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1303 
1304  DistanceMap mask_distance_maps;
1305  computeDistanceMap (mask, mask_distance_maps);
1306 
1307  std::list<Candidate> list1;
1308  std::list<Candidate> list2;
1309 
1310  float weights[8] = {0,0,0,0,0,0,0,0};
1311 
1312  const size_t off = 4;
1313  for (size_t row_index = off; row_index < height-off; ++row_index)
1314  {
1315  for (size_t col_index = off; col_index < width-off; ++col_index)
1316  {
1317  if (mask (col_index, row_index) != 0)
1318  {
1319  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1320  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1321 
1322  //const float nx = surface_normals_ (col_index, row_index).normal_x;
1323  //const float ny = surface_normals_ (col_index, row_index).normal_y;
1324  //const float nz = surface_normals_ (col_index, row_index).normal_z;
1325 
1326  if (quantized_value != 0)// && !(pcl_isnan (nx) || pcl_isnan (ny) || pcl_isnan (nz)))
1327  {
1328  const int distance_map_index = map[quantized_value];
1329 
1330  //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
1331  const float distance = distance_maps[distance_map_index] (col_index, row_index);
1332  const float distance_to_border = mask_distance_maps (col_index, row_index);
1333 
1334  if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1335  {
1336  Candidate candidate;
1337 
1338  candidate.distance = distance;
1339  candidate.x = col_index;
1340  candidate.y = row_index;
1341  candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1342 
1343  list1.push_back (candidate);
1344 
1345  ++weights[distance_map_index];
1346  }
1347  }
1348  }
1349  }
1350  }
1351 
1352  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1353  iter->distance *= 1.0f / weights[iter->bin_index];
1354 
1355  list1.sort ();
1356 
1357  features.reserve (list1.size ());
1358  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1359  {
1360  QuantizedMultiModFeature feature;
1361 
1362  feature.x = static_cast<int> (iter->x);
1363  feature.y = static_cast<int> (iter->y);
1364  feature.modality_index = modality_index;
1365  feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1366 
1367  features.push_back (feature);
1368  }
1369 }
1370 
1371 //////////////////////////////////////////////////////////////////////////////////////////////
1372 template <typename PointInT> void
1374 {
1375  const size_t width = input_->width;
1376  const size_t height = input_->height;
1377 
1378  quantized_surface_normals_.resize (width, height);
1379 
1380  for (size_t row_index = 0; row_index < height; ++row_index)
1381  {
1382  for (size_t col_index = 0; col_index < width; ++col_index)
1383  {
1384  const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1385  const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1386  const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1387 
1388  if (pcl_isnan(normal_x) || pcl_isnan(normal_y) || pcl_isnan(normal_z) || normal_z > 0)
1389  {
1390  quantized_surface_normals_ (col_index, row_index) = 0;
1391  continue;
1392  }
1393 
1394  //quantized_surface_normals_.data[row_index*width+col_index] =
1395  // normal_lookup_(normal_x, normal_y, normal_z);
1396 
1397  float angle = 11.25f + atan2f (normal_y, normal_x)*180.0f/3.14f;
1398 
1399  if (angle < 0.0f) angle += 360.0f;
1400  if (angle >= 360.0f) angle -= 360.0f;
1401 
1402  int bin_index = static_cast<int> (angle*8.0f/360.0f);
1403 
1404  //quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << bin_index;
1405  quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (bin_index);
1406  }
1407  }
1408 
1409  return;
1410 }
1411 
1412 //////////////////////////////////////////////////////////////////////////////////////////////
1413 template <typename PointInT> void
1415 {
1416  const int width = input_->width;
1417  const int height = input_->height;
1418 
1419  filtered_quantized_surface_normals_.resize (width, height);
1420 
1421  //for (int row_index = 2; row_index < height-2; ++row_index)
1422  //{
1423  // for (int col_index = 2; col_index < width-2; ++col_index)
1424  // {
1425  // std::list<unsigned char> values;
1426  // values.reserve (25);
1427 
1428  // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1429  // values.push_back (dataPtr[0]);
1430  // values.push_back (dataPtr[1]);
1431  // values.push_back (dataPtr[2]);
1432  // values.push_back (dataPtr[3]);
1433  // values.push_back (dataPtr[4]);
1434  // dataPtr += width;
1435  // values.push_back (dataPtr[0]);
1436  // values.push_back (dataPtr[1]);
1437  // values.push_back (dataPtr[2]);
1438  // values.push_back (dataPtr[3]);
1439  // values.push_back (dataPtr[4]);
1440  // dataPtr += width;
1441  // values.push_back (dataPtr[0]);
1442  // values.push_back (dataPtr[1]);
1443  // values.push_back (dataPtr[2]);
1444  // values.push_back (dataPtr[3]);
1445  // values.push_back (dataPtr[4]);
1446  // dataPtr += width;
1447  // values.push_back (dataPtr[0]);
1448  // values.push_back (dataPtr[1]);
1449  // values.push_back (dataPtr[2]);
1450  // values.push_back (dataPtr[3]);
1451  // values.push_back (dataPtr[4]);
1452  // dataPtr += width;
1453  // values.push_back (dataPtr[0]);
1454  // values.push_back (dataPtr[1]);
1455  // values.push_back (dataPtr[2]);
1456  // values.push_back (dataPtr[3]);
1457  // values.push_back (dataPtr[4]);
1458 
1459  // values.sort ();
1460 
1461  // filtered_quantized_surface_normals_ (col_index, row_index) = values[12];
1462  // }
1463  //}
1464 
1465 
1466  //for (int row_index = 2; row_index < height-2; ++row_index)
1467  //{
1468  // for (int col_index = 2; col_index < width-2; ++col_index)
1469  // {
1470  // filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << (quantized_surface_normals_ (col_index, row_index) - 1));
1471  // }
1472  //}
1473 
1474 
1475  // filter data
1476  for (int row_index = 2; row_index < height-2; ++row_index)
1477  {
1478  for (int col_index = 2; col_index < width-2; ++col_index)
1479  {
1480  unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1481 
1482  //{
1483  // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-1;
1484  // ++histogram[dataPtr[0]];
1485  // ++histogram[dataPtr[1]];
1486  // ++histogram[dataPtr[2]];
1487  //}
1488  //{
1489  // unsigned char * dataPtr = quantized_surface_normals_.getData () + row_index*width+col_index-1;
1490  // ++histogram[dataPtr[0]];
1491  // ++histogram[dataPtr[1]];
1492  // ++histogram[dataPtr[2]];
1493  //}
1494  //{
1495  // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-1;
1496  // ++histogram[dataPtr[0]];
1497  // ++histogram[dataPtr[1]];
1498  // ++histogram[dataPtr[2]];
1499  //}
1500 
1501  {
1502  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1503  ++histogram[dataPtr[0]];
1504  ++histogram[dataPtr[1]];
1505  ++histogram[dataPtr[2]];
1506  ++histogram[dataPtr[3]];
1507  ++histogram[dataPtr[4]];
1508  }
1509  {
1510  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2;
1511  ++histogram[dataPtr[0]];
1512  ++histogram[dataPtr[1]];
1513  ++histogram[dataPtr[2]];
1514  ++histogram[dataPtr[3]];
1515  ++histogram[dataPtr[4]];
1516  }
1517  {
1518  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2;
1519  ++histogram[dataPtr[0]];
1520  ++histogram[dataPtr[1]];
1521  ++histogram[dataPtr[2]];
1522  ++histogram[dataPtr[3]];
1523  ++histogram[dataPtr[4]];
1524  }
1525  {
1526  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2;
1527  ++histogram[dataPtr[0]];
1528  ++histogram[dataPtr[1]];
1529  ++histogram[dataPtr[2]];
1530  ++histogram[dataPtr[3]];
1531  ++histogram[dataPtr[4]];
1532  }
1533  {
1534  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2;
1535  ++histogram[dataPtr[0]];
1536  ++histogram[dataPtr[1]];
1537  ++histogram[dataPtr[2]];
1538  ++histogram[dataPtr[3]];
1539  ++histogram[dataPtr[4]];
1540  }
1541 
1542 
1543  unsigned char max_hist_value = 0;
1544  int max_hist_index = -1;
1545 
1546  if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1547  if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1548  if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1549  if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1550  if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1551  if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1552  if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1553  if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1554 
1555  if (max_hist_index != -1 && max_hist_value >= 1)
1556  {
1557  filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index);
1558  }
1559  else
1560  {
1561  filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1562  }
1563 
1564  //filtered_quantized_color_gradients_.data[row_index*width+col_index] = quantized_color_gradients_.data[row_index*width+col_index];
1565  }
1566  }
1567 
1568 
1569 
1570  //cv::Mat data1(quantized_surface_normals_.height, quantized_surface_normals_.width, CV_8U, quantized_surface_normals_.data);
1571  //cv::Mat data2(filtered_quantized_surface_normals_.height, filtered_quantized_surface_normals_.width, CV_8U, filtered_quantized_surface_normals_.data);
1572 
1573  //cv::medianBlur(data1, data2, 3);
1574 
1575  //for (int row_index = 0; row_index < height; ++row_index)
1576  //{
1577  // for (int col_index = 0; col_index < width; ++col_index)
1578  // {
1579  // filtered_quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << filtered_quantized_surface_normals_.data[row_index*width+col_index];
1580  // }
1581  //}
1582 }
1583 
1584 //////////////////////////////////////////////////////////////////////////////////////////////
1585 template <typename PointInT> void
1587 {
1588  const size_t width = input.getWidth ();
1589  const size_t height = input.getHeight ();
1590 
1591  output.resize (width, height);
1592 
1593  // compute distance map
1594  //float *distance_map = new float[input_->points.size ()];
1595  const unsigned char * mask_map = input.getData ();
1596  float * distance_map = output.getData ();
1597  for (size_t index = 0; index < width*height; ++index)
1598  {
1599  if (mask_map[index] == 0)
1600  distance_map[index] = 0.0f;
1601  else
1602  distance_map[index] = static_cast<float> (width + height);
1603  }
1604 
1605  // first pass
1606  float * previous_row = distance_map;
1607  float * current_row = previous_row + width;
1608  for (size_t ri = 1; ri < height; ++ri)
1609  {
1610  for (size_t ci = 1; ci < width; ++ci)
1611  {
1612  const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f;
1613  const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f;
1614  const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f;
1615  const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f;
1616  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
1617 
1618  const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1619 
1620  if (min_value < center)
1621  current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value;
1622  }
1623  previous_row = current_row;
1624  current_row += width;
1625  }
1626 
1627  // second pass
1628  float * next_row = distance_map + width * (height - 1);
1629  current_row = next_row - width;
1630  for (int ri = static_cast<int> (height)-2; ri >= 0; --ri)
1631  {
1632  for (int ci = static_cast<int> (width)-2; ci >= 0; --ci)
1633  {
1634  const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f;
1635  const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f;
1636  const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f;
1637  const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f;
1638  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
1639 
1640  const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1641 
1642  if (min_value < center)
1643  current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value;
1644  }
1645  next_row = current_row;
1646  current_row -= width;
1647  }
1648 }
1649 
1650 
1651 #endif
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
bool operator<(const Candidate &rhs)
Compares two candidates based on their distance to the next different quantized value.
Represents a distance map obtained from a distance transformation.
Definition: distance_map.h:47
unsigned char operator()(const float x, const float y, const float z) const
Operator to access an element in the LUT.
int offset_x
The offset in x-direction.
int offset_y
The offset in y-direction.
void resize(const size_t width, const size_t height)
Resizes the map to the specified size.
Definition: distance_map.h:81
void quantizeSurfaceNormals()
Quantizes the surface normals.
int offset_z
The offset in z-direction.
int range_z
The range of the LUT in z-direction.
Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylo...
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
float distance
Distance to the next different quantized value.
int range_x
The range of the LUT in x-direction.
size_t getWidth() const
Returns the width of the modality data map.
void initializeLUT(const int range_x_arg, const int range_y_arg, const int range_z_arg)
Initializes the LUT.
Interface for a quantizable modality.
size_t x
x-position of the feature.
unsigned char * getData()
Definition: mask_map.h:64
unsigned char * lut
The LUT data.
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
virtual ~SurfaceNormalModality()
Destructor.
int range_y
The range of the LUT in y-direction.
Map that stores orientations.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:426
pcl::PointCloud< PointInT > PointCloudIn
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
void setSpreadingSize(const size_t spreading_size)
Sets the spreading size.
size_t getHeight() const
Returns the height of the modality data map.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
size_t getWidth() const
Definition: mask_map.h:58
QuantizedMap & getQuantizedMap()
Returns a reference to the internal quantized map.
Look-up-table for fast surface normal quantization.
void extractAllFeatures(const MaskMap &mask, size_t nr_features, size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const
Extracts all possible features from the modality within the specified mask.
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internal spreaded quantized map.
PCL base class.
Definition: pcl_base.h:68
float * getData()
Returns a pointer to the beginning of map.
Definition: distance_map.h:71
A point structure representing normal coordinates and the surface curvature estimate.
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
int size_z
The size of the LUT in z-direction.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
size_t y
y-position of the feature.
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
unsigned char bin_index
Quantized value.
void filterQuantizedSurfaceNormals()
Filters the quantized surface normals.
void computeSurfaceNormals()
Computes the surface normals from the input cloud.
Feature that defines a position and quantized value in a specific modality.
int size_x
The size of the LUT in x-direction.
unsigned char quantized_value
the quantized value attached to the feature.
size_t getHeight() const
Definition: mask_map.h:61
size_t modality_index
the index of the corresponding modality.
Modality based on surface normals.
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
Candidate for a feature (used in feature extraction methods).
int size_y
The size of the LUT in y-direction.
A point structure representing Euclidean xyz coordinates.
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
void resize(const size_t width, const size_t height, const float value)
Resizes the map to the specific width and height and initializes all new elements with the specified ...
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:146
void setVariableFeatureNr(const bool enabled)
Enables/disables the use of extracting a variable number of features.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, size_t spreading_size)
void extractFeatures(const MaskMap &mask, size_t nr_features, size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const
Extracts features from this modality within the specified mask.