Point Cloud Library (PCL)  1.9.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  * \param[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 = std::sqrt (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 = std::sqrt (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) const
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 spread 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 Spread 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 / std::sqrt (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  * \todo Should also need camera model, or at least focal lengths? Replace distance_threshold with mask?
722  */
723 template <typename PointInT> void
725 {
726  const int width = input_->width;
727  const int height = input_->height;
728 
729  unsigned short * lp_depth = new unsigned short[width*height];
730  unsigned char * lp_normals = new unsigned char[width*height];
731  memset (lp_normals, 0, width*height);
732 
733  surface_normal_orientations_.resize (width, height, 0.0f);
734 
735  for (int row_index = 0; row_index < height; ++row_index)
736  {
737  for (int col_index = 0; col_index < width; ++col_index)
738  {
739  const float value = input_->points[row_index*width + col_index].z;
740  if (pcl_isfinite (value))
741  {
742  lp_depth[row_index*width + col_index] = static_cast<unsigned short> (value * 1000.0f);
743  }
744  else
745  {
746  lp_depth[row_index*width + col_index] = 0;
747  }
748  }
749  }
750 
751  const int l_W = width;
752  const int l_H = height;
753 
754  const int l_r = 5; // used to be 7
755  //const int l_offset0 = -l_r - l_r * l_W;
756  //const int l_offset1 = 0 - l_r * l_W;
757  //const int l_offset2 = +l_r - l_r * l_W;
758  //const int l_offset3 = -l_r;
759  //const int l_offset4 = +l_r;
760  //const int l_offset5 = -l_r + l_r * l_W;
761  //const int l_offset6 = 0 + l_r * l_W;
762  //const int l_offset7 = +l_r + l_r * l_W;
763 
764  const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
765  const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
766  const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
767  , offsets_i[1] + offsets_j[1] * l_W
768  , offsets_i[2] + offsets_j[2] * l_W
769  , offsets_i[3] + offsets_j[3] * l_W
770  , offsets_i[4] + offsets_j[4] * l_W
771  , offsets_i[5] + offsets_j[5] * l_W
772  , offsets_i[6] + offsets_j[6] * l_W
773  , offsets_i[7] + offsets_j[7] * l_W };
774 
775 
776  //const int l_offsetx = GRANULARITY / 2;
777  //const int l_offsety = GRANULARITY / 2;
778 
779  const int difference_threshold = 50;
780  const int distance_threshold = 2000;
781 
782  //const double scale = 1000.0;
783  //const double difference_threshold = 0.05 * scale;
784  //const double distance_threshold = 2.0 * scale;
785 
786  for (int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
787  {
788  unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
789  unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
790 
791  for (int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
792  {
793  long l_d = lp_line[0];
794  //float l_d = input_->points[(l_y * l_W + l_r) + l_x].z;
795  //float px = input_->points[(l_y * l_W + l_r) + l_x].x;
796  //float py = input_->points[(l_y * l_W + l_r) + l_x].y;
797 
798  if (l_d < distance_threshold)
799  {
800  // accum
801  long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
802  long l_b[2]; l_b[0] = l_b[1] = 0;
803  //double l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
804  //double l_b[2]; l_b[0] = l_b[1] = 0;
805 
806  accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
807  accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
808  accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
809  accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
810  accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
811  accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
812  accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
813  accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
814 
815  //for (size_t index = 0; index < 8; ++index)
816  //{
817  // //accumBilateral(lp_line[offsets[index]] - l_d, offsets_i[index], offsets_j[index], l_A, l_b, difference_threshold);
818 
819  // //const long delta = lp_line[offsets[index]] - l_d;
820  // //const long i = offsets_i[index];
821  // //const long j = offsets_j[index];
822  // //long * A = l_A;
823  // //long * b = l_b;
824  // //const int threshold = difference_threshold;
825 
826  // //const long f = std::abs(delta) < threshold ? 1 : 0;
827 
828  // //const long fi = f * i;
829  // //const long fj = f * j;
830 
831  // //A[0] += fi * i;
832  // //A[1] += fi * j;
833  // //A[3] += fj * j;
834  // //b[0] += fi * delta;
835  // //b[1] += fj * delta;
836 
837 
838  // const double delta = 1000.0f * (input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].z - l_d);
839  // const double i = offsets_i[index];
840  // const double j = offsets_j[index];
841  // //const float i = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].x - px;//offsets_i[index];
842  // //const float j = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].y - py;//offsets_j[index];
843  // double * A = l_A;
844  // double * b = l_b;
845  // const double threshold = difference_threshold;
846 
847  // const double f = std::fabs(delta) < threshold ? 1.0f : 0.0f;
848 
849  // const double fi = f * i;
850  // const double fj = f * j;
851 
852  // A[0] += fi * i;
853  // A[1] += fi * j;
854  // A[3] += fj * j;
855  // b[0] += fi * delta;
856  // b[1] += fj * delta;
857  //}
858 
859  //long f = std::abs(delta) < threshold ? 1 : 0;
860 
861  //const long fi = f * i;
862  //const long fj = f * j;
863 
864  //A[0] += fi * i;
865  //A[1] += fi * j;
866  //A[3] += fj * j;
867  //b[0] += fi * delta;
868  //b[1] += fj * delta;
869 
870 
871  // solve
872  long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
873  long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
874  long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
875 
876  /// @todo Magic number 1150 is focal length? This is something like
877  /// f in SXGA mode, but in VGA is more like 530.
878  float l_nx = static_cast<float>(1150 * l_ddx);
879  float l_ny = static_cast<float>(1150 * l_ddy);
880  float l_nz = static_cast<float>(-l_det * l_d);
881 
882  //// solve
883  //double l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
884  //double l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
885  //double l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
886 
887  ///// @todo Magic number 1150 is focal length? This is something like
888  ///// f in SXGA mode, but in VGA is more like 530.
889  //const double dummy_focal_length = 1150.0f;
890  //double l_nx = l_ddx * dummy_focal_length;
891  //double l_ny = l_ddy * dummy_focal_length;
892  //double l_nz = -l_det * l_d;
893 
894  float l_sqrt = std::sqrt (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
895 
896  if (l_sqrt > 0)
897  {
898  float l_norminv = 1.0f / (l_sqrt);
899 
900  l_nx *= l_norminv;
901  l_ny *= l_norminv;
902  l_nz *= l_norminv;
903 
904  float angle = 22.5f + atan2f (l_ny, l_nx) * 180.0f / 3.14f;
905 
906  if (angle < 0.0f) angle += 360.0f;
907  if (angle >= 360.0f) angle -= 360.0f;
908 
909  int bin_index = static_cast<int> (angle*8.0f/360.0f) & 7;
910 
911  surface_normal_orientations_ (l_x, l_y) = angle;
912 
913  //*lp_norm = fabs(l_nz)*255;
914 
915  //int l_val1 = static_cast<int>(l_nx * l_offsetx + l_offsetx);
916  //int l_val2 = static_cast<int>(l_ny * l_offsety + l_offsety);
917  //int l_val3 = static_cast<int>(l_nz * GRANULARITY + GRANULARITY);
918 
919  //*lp_norm = NORMAL_LUT[l_val3][l_val2][l_val1];
920  *lp_norm = static_cast<unsigned char> (0x1 << bin_index);
921  }
922  else
923  {
924  *lp_norm = 0; // Discard shadows from depth sensor
925  }
926  }
927  else
928  {
929  *lp_norm = 0; //out of depth
930  }
931  ++lp_line;
932  ++lp_norm;
933  }
934  }
935  /*cvSmooth(m_dep[0], m_dep[0], CV_MEDIAN, 5, 5);*/
936 
937  unsigned char map[255];
938  memset(map, 0, 255);
939 
940  map[0x1<<0] = 0;
941  map[0x1<<1] = 1;
942  map[0x1<<2] = 2;
943  map[0x1<<3] = 3;
944  map[0x1<<4] = 4;
945  map[0x1<<5] = 5;
946  map[0x1<<6] = 6;
947  map[0x1<<7] = 7;
948 
949  quantized_surface_normals_.resize (width, height);
950  for (int row_index = 0; row_index < height; ++row_index)
951  {
952  for (int col_index = 0; col_index < width; ++col_index)
953  {
954  quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
955  }
956  }
957 
958  delete[] lp_depth;
959  delete[] lp_normals;
960 }
961 
962 
963 //////////////////////////////////////////////////////////////////////////////////////////////
964 template <typename PointInT> void
966  const size_t nr_features,
967  const size_t modality_index,
968  std::vector<QuantizedMultiModFeature> & features) const
969 {
970  const size_t width = mask.getWidth ();
971  const size_t height = mask.getHeight ();
972 
973  //cv::Mat maskImage(height, width, CV_8U, mask.mask);
974  //cv::erode(maskImage, maskImage
975 
976  // create distance maps for every quantization value
977  //cv::Mat distance_maps[8];
978  //for (int map_index = 0; map_index < 8; ++map_index)
979  //{
980  // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U);
981  //}
982 
983  MaskMap mask_maps[8];
984  for (size_t map_index = 0; map_index < 8; ++map_index)
985  mask_maps[map_index].resize (width, height);
986 
987  unsigned char map[255];
988  memset(map, 0, 255);
989 
990  map[0x1<<0] = 0;
991  map[0x1<<1] = 1;
992  map[0x1<<2] = 2;
993  map[0x1<<3] = 3;
994  map[0x1<<4] = 4;
995  map[0x1<<5] = 5;
996  map[0x1<<6] = 6;
997  map[0x1<<7] = 7;
998 
999  QuantizedMap distance_map_indices (width, height);
1000  //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
1001 
1002  for (size_t row_index = 0; row_index < height; ++row_index)
1003  {
1004  for (size_t col_index = 0; col_index < width; ++col_index)
1005  {
1006  if (mask (col_index, row_index) != 0)
1007  {
1008  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1009  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1010 
1011  if (quantized_value == 0)
1012  continue;
1013  const int dist_map_index = map[quantized_value];
1014 
1015  distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1016  //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
1017  mask_maps[dist_map_index] (col_index, row_index) = 255;
1018  }
1019  }
1020  }
1021 
1022  DistanceMap distance_maps[8];
1023  for (int map_index = 0; map_index < 8; ++map_index)
1024  computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1025 
1026  DistanceMap mask_distance_maps;
1027  computeDistanceMap (mask, mask_distance_maps);
1028 
1029  std::list<Candidate> list1;
1030  std::list<Candidate> list2;
1031 
1032  float weights[8] = {0,0,0,0,0,0,0,0};
1033 
1034  const size_t off = 4;
1035  for (size_t row_index = off; row_index < height-off; ++row_index)
1036  {
1037  for (size_t col_index = off; col_index < width-off; ++col_index)
1038  {
1039  if (mask (col_index, row_index) != 0)
1040  {
1041  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1042  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1043 
1044  //const float nx = surface_normals_ (col_index, row_index).normal_x;
1045  //const float ny = surface_normals_ (col_index, row_index).normal_y;
1046  //const float nz = surface_normals_ (col_index, row_index).normal_z;
1047 
1048  if (quantized_value != 0)// && !(pcl_isnan (nx) || pcl_isnan (ny) || pcl_isnan (nz)))
1049  {
1050  const int distance_map_index = map[quantized_value];
1051 
1052  //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
1053  const float distance = distance_maps[distance_map_index] (col_index, row_index);
1054  const float distance_to_border = mask_distance_maps (col_index, row_index);
1055 
1056  if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1057  {
1058  Candidate candidate;
1059 
1060  candidate.distance = distance;
1061  candidate.x = col_index;
1062  candidate.y = row_index;
1063  candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1064 
1065  list1.push_back (candidate);
1066 
1067  ++weights[distance_map_index];
1068  }
1069  }
1070  }
1071  }
1072  }
1073 
1074  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1075  iter->distance *= 1.0f / weights[iter->bin_index];
1076 
1077  list1.sort ();
1078 
1079  if (variable_feature_nr_)
1080  {
1081  int distance = static_cast<int> (list1.size ());
1082  bool feature_selection_finished = false;
1083  while (!feature_selection_finished)
1084  {
1085  const int sqr_distance = distance*distance;
1086  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1087  {
1088  bool candidate_accepted = true;
1089  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1090  {
1091  const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1092  const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1093  const int tmp_distance = dx*dx + dy*dy;
1094 
1095  if (tmp_distance < sqr_distance)
1096  {
1097  candidate_accepted = false;
1098  break;
1099  }
1100  }
1101 
1102 
1103  float min_min_sqr_distance = std::numeric_limits<float>::max ();
1104  float max_min_sqr_distance = 0;
1105  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1106  {
1107  float min_sqr_distance = std::numeric_limits<float>::max ();
1108  for (typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
1109  {
1110  if (iter2 == iter3)
1111  continue;
1112 
1113  const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
1114  const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
1115 
1116  const float sqr_distance = dx*dx + dy*dy;
1117 
1118  if (sqr_distance < min_sqr_distance)
1119  {
1120  min_sqr_distance = sqr_distance;
1121  }
1122 
1123  //std::cerr << min_sqr_distance;
1124  }
1125  //std::cerr << std::endl;
1126 
1127  // check current feature
1128  {
1129  const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter1->x);
1130  const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter1->y);
1131 
1132  const float sqr_distance = dx*dx + dy*dy;
1133 
1134  if (sqr_distance < min_sqr_distance)
1135  {
1136  min_sqr_distance = sqr_distance;
1137  }
1138  }
1139 
1140  if (min_sqr_distance < min_min_sqr_distance)
1141  min_min_sqr_distance = min_sqr_distance;
1142  if (min_sqr_distance > max_min_sqr_distance)
1143  max_min_sqr_distance = min_sqr_distance;
1144 
1145  //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl;
1146  }
1147 
1148  if (candidate_accepted)
1149  {
1150  //std::cerr << "feature_index: " << list2.size () << std::endl;
1151  //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl;
1152  //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl;
1153 
1154  if (min_min_sqr_distance < 50)
1155  {
1156  feature_selection_finished = true;
1157  break;
1158  }
1159 
1160  list2.push_back (*iter1);
1161  }
1162 
1163  //if (list2.size () == nr_features)
1164  //{
1165  // feature_selection_finished = true;
1166  // break;
1167  //}
1168  }
1169  --distance;
1170  }
1171  }
1172  else
1173  {
1174  if (list1.size () <= nr_features)
1175  {
1176  features.reserve (list1.size ());
1177  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1178  {
1179  QuantizedMultiModFeature feature;
1180 
1181  feature.x = static_cast<int> (iter->x);
1182  feature.y = static_cast<int> (iter->y);
1183  feature.modality_index = modality_index;
1184  feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1185 
1186  features.push_back (feature);
1187  }
1188 
1189  return;
1190  }
1191 
1192  int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:!
1193  while (list2.size () != nr_features)
1194  {
1195  const int sqr_distance = distance*distance;
1196  for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1197  {
1198  bool candidate_accepted = true;
1199 
1200  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1201  {
1202  const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1203  const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1204  const int tmp_distance = dx*dx + dy*dy;
1205 
1206  if (tmp_distance < sqr_distance)
1207  {
1208  candidate_accepted = false;
1209  break;
1210  }
1211  }
1212 
1213  if (candidate_accepted)
1214  list2.push_back (*iter1);
1215 
1216  if (list2.size () == nr_features) break;
1217  }
1218  --distance;
1219  }
1220  }
1221 
1222  for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1223  {
1224  QuantizedMultiModFeature feature;
1225 
1226  feature.x = static_cast<int> (iter2->x);
1227  feature.y = static_cast<int> (iter2->y);
1228  feature.modality_index = modality_index;
1229  feature.quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1230 
1231  features.push_back (feature);
1232  }
1233 }
1234 
1235 //////////////////////////////////////////////////////////////////////////////////////////////
1236 template <typename PointInT> void
1238  const MaskMap & mask, const size_t, const size_t modality_index,
1239  std::vector<QuantizedMultiModFeature> & features) const
1240 {
1241  const size_t width = mask.getWidth ();
1242  const size_t height = mask.getHeight ();
1243 
1244  //cv::Mat maskImage(height, width, CV_8U, mask.mask);
1245  //cv::erode(maskImage, maskImage
1246 
1247  // create distance maps for every quantization value
1248  //cv::Mat distance_maps[8];
1249  //for (int map_index = 0; map_index < 8; ++map_index)
1250  //{
1251  // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U);
1252  //}
1253 
1254  MaskMap mask_maps[8];
1255  for (size_t map_index = 0; map_index < 8; ++map_index)
1256  mask_maps[map_index].resize (width, height);
1257 
1258  unsigned char map[255];
1259  memset(map, 0, 255);
1260 
1261  map[0x1<<0] = 0;
1262  map[0x1<<1] = 1;
1263  map[0x1<<2] = 2;
1264  map[0x1<<3] = 3;
1265  map[0x1<<4] = 4;
1266  map[0x1<<5] = 5;
1267  map[0x1<<6] = 6;
1268  map[0x1<<7] = 7;
1269 
1270  QuantizedMap distance_map_indices (width, height);
1271  //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
1272 
1273  for (size_t row_index = 0; row_index < height; ++row_index)
1274  {
1275  for (size_t col_index = 0; col_index < width; ++col_index)
1276  {
1277  if (mask (col_index, row_index) != 0)
1278  {
1279  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1280  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1281 
1282  if (quantized_value == 0)
1283  continue;
1284  const int dist_map_index = map[quantized_value];
1285 
1286  distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1287  //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
1288  mask_maps[dist_map_index] (col_index, row_index) = 255;
1289  }
1290  }
1291  }
1292 
1293  DistanceMap distance_maps[8];
1294  for (int map_index = 0; map_index < 8; ++map_index)
1295  computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1296 
1297  DistanceMap mask_distance_maps;
1298  computeDistanceMap (mask, mask_distance_maps);
1299 
1300  std::list<Candidate> list1;
1301  std::list<Candidate> list2;
1302 
1303  float weights[8] = {0,0,0,0,0,0,0,0};
1304 
1305  const size_t off = 4;
1306  for (size_t row_index = off; row_index < height-off; ++row_index)
1307  {
1308  for (size_t col_index = off; col_index < width-off; ++col_index)
1309  {
1310  if (mask (col_index, row_index) != 0)
1311  {
1312  //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1313  const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1314 
1315  //const float nx = surface_normals_ (col_index, row_index).normal_x;
1316  //const float ny = surface_normals_ (col_index, row_index).normal_y;
1317  //const float nz = surface_normals_ (col_index, row_index).normal_z;
1318 
1319  if (quantized_value != 0)// && !(pcl_isnan (nx) || pcl_isnan (ny) || pcl_isnan (nz)))
1320  {
1321  const int distance_map_index = map[quantized_value];
1322 
1323  //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
1324  const float distance = distance_maps[distance_map_index] (col_index, row_index);
1325  const float distance_to_border = mask_distance_maps (col_index, row_index);
1326 
1327  if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1328  {
1329  Candidate candidate;
1330 
1331  candidate.distance = distance;
1332  candidate.x = col_index;
1333  candidate.y = row_index;
1334  candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1335 
1336  list1.push_back (candidate);
1337 
1338  ++weights[distance_map_index];
1339  }
1340  }
1341  }
1342  }
1343  }
1344 
1345  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1346  iter->distance *= 1.0f / weights[iter->bin_index];
1347 
1348  list1.sort ();
1349 
1350  features.reserve (list1.size ());
1351  for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1352  {
1353  QuantizedMultiModFeature feature;
1354 
1355  feature.x = static_cast<int> (iter->x);
1356  feature.y = static_cast<int> (iter->y);
1357  feature.modality_index = modality_index;
1358  feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1359 
1360  features.push_back (feature);
1361  }
1362 }
1363 
1364 //////////////////////////////////////////////////////////////////////////////////////////////
1365 template <typename PointInT> void
1367 {
1368  const size_t width = input_->width;
1369  const size_t height = input_->height;
1370 
1371  quantized_surface_normals_.resize (width, height);
1372 
1373  for (size_t row_index = 0; row_index < height; ++row_index)
1374  {
1375  for (size_t col_index = 0; col_index < width; ++col_index)
1376  {
1377  const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1378  const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1379  const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1380 
1381  if (pcl_isnan(normal_x) || pcl_isnan(normal_y) || pcl_isnan(normal_z) || normal_z > 0)
1382  {
1383  quantized_surface_normals_ (col_index, row_index) = 0;
1384  continue;
1385  }
1386 
1387  //quantized_surface_normals_.data[row_index*width+col_index] =
1388  // normal_lookup_(normal_x, normal_y, normal_z);
1389 
1390  float angle = 11.25f + atan2f (normal_y, normal_x)*180.0f/3.14f;
1391 
1392  if (angle < 0.0f) angle += 360.0f;
1393  if (angle >= 360.0f) angle -= 360.0f;
1394 
1395  int bin_index = static_cast<int> (angle*8.0f/360.0f);
1396 
1397  //quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << bin_index;
1398  quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (bin_index);
1399  }
1400  }
1401 
1402  return;
1403 }
1404 
1405 //////////////////////////////////////////////////////////////////////////////////////////////
1406 template <typename PointInT> void
1408 {
1409  const int width = input_->width;
1410  const int height = input_->height;
1411 
1412  filtered_quantized_surface_normals_.resize (width, height);
1413 
1414  //for (int row_index = 2; row_index < height-2; ++row_index)
1415  //{
1416  // for (int col_index = 2; col_index < width-2; ++col_index)
1417  // {
1418  // std::list<unsigned char> values;
1419  // values.reserve (25);
1420 
1421  // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1422  // values.push_back (dataPtr[0]);
1423  // values.push_back (dataPtr[1]);
1424  // values.push_back (dataPtr[2]);
1425  // values.push_back (dataPtr[3]);
1426  // values.push_back (dataPtr[4]);
1427  // dataPtr += width;
1428  // values.push_back (dataPtr[0]);
1429  // values.push_back (dataPtr[1]);
1430  // values.push_back (dataPtr[2]);
1431  // values.push_back (dataPtr[3]);
1432  // values.push_back (dataPtr[4]);
1433  // dataPtr += width;
1434  // values.push_back (dataPtr[0]);
1435  // values.push_back (dataPtr[1]);
1436  // values.push_back (dataPtr[2]);
1437  // values.push_back (dataPtr[3]);
1438  // values.push_back (dataPtr[4]);
1439  // dataPtr += width;
1440  // values.push_back (dataPtr[0]);
1441  // values.push_back (dataPtr[1]);
1442  // values.push_back (dataPtr[2]);
1443  // values.push_back (dataPtr[3]);
1444  // values.push_back (dataPtr[4]);
1445  // dataPtr += width;
1446  // values.push_back (dataPtr[0]);
1447  // values.push_back (dataPtr[1]);
1448  // values.push_back (dataPtr[2]);
1449  // values.push_back (dataPtr[3]);
1450  // values.push_back (dataPtr[4]);
1451 
1452  // values.sort ();
1453 
1454  // filtered_quantized_surface_normals_ (col_index, row_index) = values[12];
1455  // }
1456  //}
1457 
1458 
1459  //for (int row_index = 2; row_index < height-2; ++row_index)
1460  //{
1461  // for (int col_index = 2; col_index < width-2; ++col_index)
1462  // {
1463  // filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << (quantized_surface_normals_ (col_index, row_index) - 1));
1464  // }
1465  //}
1466 
1467 
1468  // filter data
1469  for (int row_index = 2; row_index < height-2; ++row_index)
1470  {
1471  for (int col_index = 2; col_index < width-2; ++col_index)
1472  {
1473  unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1474 
1475  //{
1476  // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-1;
1477  // ++histogram[dataPtr[0]];
1478  // ++histogram[dataPtr[1]];
1479  // ++histogram[dataPtr[2]];
1480  //}
1481  //{
1482  // unsigned char * dataPtr = quantized_surface_normals_.getData () + row_index*width+col_index-1;
1483  // ++histogram[dataPtr[0]];
1484  // ++histogram[dataPtr[1]];
1485  // ++histogram[dataPtr[2]];
1486  //}
1487  //{
1488  // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-1;
1489  // ++histogram[dataPtr[0]];
1490  // ++histogram[dataPtr[1]];
1491  // ++histogram[dataPtr[2]];
1492  //}
1493 
1494  {
1495  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1496  ++histogram[dataPtr[0]];
1497  ++histogram[dataPtr[1]];
1498  ++histogram[dataPtr[2]];
1499  ++histogram[dataPtr[3]];
1500  ++histogram[dataPtr[4]];
1501  }
1502  {
1503  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2;
1504  ++histogram[dataPtr[0]];
1505  ++histogram[dataPtr[1]];
1506  ++histogram[dataPtr[2]];
1507  ++histogram[dataPtr[3]];
1508  ++histogram[dataPtr[4]];
1509  }
1510  {
1511  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2;
1512  ++histogram[dataPtr[0]];
1513  ++histogram[dataPtr[1]];
1514  ++histogram[dataPtr[2]];
1515  ++histogram[dataPtr[3]];
1516  ++histogram[dataPtr[4]];
1517  }
1518  {
1519  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2;
1520  ++histogram[dataPtr[0]];
1521  ++histogram[dataPtr[1]];
1522  ++histogram[dataPtr[2]];
1523  ++histogram[dataPtr[3]];
1524  ++histogram[dataPtr[4]];
1525  }
1526  {
1527  unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2;
1528  ++histogram[dataPtr[0]];
1529  ++histogram[dataPtr[1]];
1530  ++histogram[dataPtr[2]];
1531  ++histogram[dataPtr[3]];
1532  ++histogram[dataPtr[4]];
1533  }
1534 
1535 
1536  unsigned char max_hist_value = 0;
1537  int max_hist_index = -1;
1538 
1539  if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1540  if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1541  if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1542  if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1543  if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1544  if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1545  if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1546  if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1547 
1548  if (max_hist_index != -1 && max_hist_value >= 1)
1549  {
1550  filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index);
1551  }
1552  else
1553  {
1554  filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1555  }
1556 
1557  //filtered_quantized_color_gradients_.data[row_index*width+col_index] = quantized_color_gradients_.data[row_index*width+col_index];
1558  }
1559  }
1560 
1561 
1562 
1563  //cv::Mat data1(quantized_surface_normals_.height, quantized_surface_normals_.width, CV_8U, quantized_surface_normals_.data);
1564  //cv::Mat data2(filtered_quantized_surface_normals_.height, filtered_quantized_surface_normals_.width, CV_8U, filtered_quantized_surface_normals_.data);
1565 
1566  //cv::medianBlur(data1, data2, 3);
1567 
1568  //for (int row_index = 0; row_index < height; ++row_index)
1569  //{
1570  // for (int col_index = 0; col_index < width; ++col_index)
1571  // {
1572  // filtered_quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << filtered_quantized_surface_normals_.data[row_index*width+col_index];
1573  // }
1574  //}
1575 }
1576 
1577 //////////////////////////////////////////////////////////////////////////////////////////////
1578 template <typename PointInT> void
1580 {
1581  const size_t width = input.getWidth ();
1582  const size_t height = input.getHeight ();
1583 
1584  output.resize (width, height);
1585 
1586  // compute distance map
1587  //float *distance_map = new float[input_->points.size ()];
1588  const unsigned char * mask_map = input.getData ();
1589  float * distance_map = output.getData ();
1590  for (size_t index = 0; index < width*height; ++index)
1591  {
1592  if (mask_map[index] == 0)
1593  distance_map[index] = 0.0f;
1594  else
1595  distance_map[index] = static_cast<float> (width + height);
1596  }
1597 
1598  // first pass
1599  float * previous_row = distance_map;
1600  float * current_row = previous_row + width;
1601  for (size_t ri = 1; ri < height; ++ri)
1602  {
1603  for (size_t ci = 1; ci < width; ++ci)
1604  {
1605  const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f;
1606  const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f;
1607  const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f;
1608  const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f;
1609  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
1610 
1611  const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1612 
1613  if (min_value < center)
1614  current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value;
1615  }
1616  previous_row = current_row;
1617  current_row += width;
1618  }
1619 
1620  // second pass
1621  float * next_row = distance_map + width * (height - 1);
1622  current_row = next_row - width;
1623  for (int ri = static_cast<int> (height)-2; ri >= 0; --ri)
1624  {
1625  for (int ci = static_cast<int> (width)-2; ci >= 0; --ci)
1626  {
1627  const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f;
1628  const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f;
1629  const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f;
1630  const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f;
1631  const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
1632 
1633  const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1634 
1635  if (min_value < center)
1636  current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value;
1637  }
1638  next_row = current_row;
1639  current_row -= width;
1640  }
1641 }
1642 
1643 
1644 #endif
pcl::QuantizedNormalLookUpTable::operator()
unsigned char operator()(const float x, const float y, const float z) const
Operator to access an element in the LUT.
Definition: surface_normal_modality.h:268
pcl::QuantizedNormalLookUpTable::offset_y
int offset_y
The offset in y-direction.
Definition: surface_normal_modality.h:137
pcl::SurfaceNormalModality::~SurfaceNormalModality
virtual ~SurfaceNormalModality()
Destructor.
Definition: surface_normal_modality.h:508
pcl::SurfaceNormalModality::Candidate
Candidate for a feature (used in feature extraction methods).
Definition: surface_normal_modality.h:300
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
point_types.h
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:791
pcl::SurfaceNormalModality::processInputData
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
Definition: surface_normal_modality.h:514
pcl::PointCloud::VectorType
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:426
pcl::QuantizedNormalLookUpTable::lut
unsigned char * lut
The LUT data.
Definition: surface_normal_modality.h:149
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:61
pcl::QuantizedNormalLookUpTable::range_z
int range_z
The range of the LUT in z-direction.
Definition: surface_normal_modality.h:132
pcl::SurfaceNormalModality::Candidate::distance
float distance
Distance to the next different quantized value.
Definition: surface_normal_modality.h:308
pcl::PCLBase< PointInT >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
pcl::LINEMOD_OrientationMap
Map that stores orientations.
Definition: surface_normal_modality.h:55
pcl::SurfaceNormalModality::Candidate::Candidate
Candidate()
Constructor.
Definition: surface_normal_modality.h:303
pcl::SurfaceNormalModality::getOrientationMap
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
Definition: surface_normal_modality.h:384
pcl::QuantizedNormalLookUpTable::QuantizedNormalLookUpTable
QuantizedNormalLookUpTable()
Constructor.
Definition: surface_normal_modality.h:152
pcl::QuantizedNormalLookUpTable::size_z
int size_z
The size of the LUT in z-direction.
Definition: surface_normal_modality.h:146
pcl::SurfaceNormalModality
Modality based on surface normals.
Definition: surface_normal_modality.h:294
pcl::SurfaceNormalModality::getSurfaceNormals
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
Definition: surface_normal_modality.h:363
pcl::QuantizableModality
Interface for a quantizable modality.
Definition: quantizable_modality.h:53
pcl::QuantizedMap
Definition: quantized_map.h:46
pcl::SurfaceNormalModality::setInputCloud
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
Definition: surface_normal_modality.h:414
pcl::SurfaceNormalModality::Candidate::y
size_t y
y-position of the feature.
Definition: surface_normal_modality.h:316
pcl::SurfaceNormalModality::PointCloudIn
pcl::PointCloud< PointInT > PointCloudIn
Definition: surface_normal_modality.h:329
pcl::SurfaceNormalModality::computeSurfaceNormals
void computeSurfaceNormals()
Computes the surface normals from the input cloud.
Definition: surface_normal_modality.h:545
pcl::LINEMOD_OrientationMap::getWidth
size_t getWidth() const
Returns the width of the modality data map.
Definition: surface_normal_modality.h:65
pcl::SurfaceNormalModality::setVariableFeatureNr
void setVariableFeatureNr(const bool enabled)
Enables/disables the use of extracting a variable number of features.
Definition: surface_normal_modality.h:349
pcl::QuantizedMultiModFeature::y
int y
y-position.
Definition: sparse_quantized_multi_mod_template.h:59
pcl::SurfaceNormalModality::computeDistanceMap
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
Definition: surface_normal_modality.h:1579
pcl::QuantizedMultiModFeature::quantized_value
unsigned char quantized_value
the quantized value attached to the feature.
Definition: sparse_quantized_multi_mod_template.h:63
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:68
pcl::LINEMOD_OrientationMap::resize
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 ...
Definition: surface_normal_modality.h:84
pcl::LinearLeastSquaresNormalEstimation::setDepthDependentSmoothing
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
Definition: linear_least_squares_normal.h:100
pcl::PointCloud< PointInT >
pcl::QuantizedNormalLookUpTable::offset_z
int offset_z
The offset in z-direction.
Definition: surface_normal_modality.h:139
pcl::QuantizedMultiModFeature
Feature that defines a position and quantized value in a specific modality.
Definition: sparse_quantized_multi_mod_template.h:51
pcl::SurfaceNormalModality::Candidate::normal
Normal normal
Normal.
Definition: surface_normal_modality.h:306
pcl::LINEMOD_OrientationMap::~LINEMOD_OrientationMap
~LINEMOD_OrientationMap()
Destructor.
Definition: surface_normal_modality.h:61
pcl::SurfaceNormalModality::extractFeatures
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.
Definition: surface_normal_modality.h:965
pcl::LINEMOD_OrientationMap::LINEMOD_OrientationMap
LINEMOD_OrientationMap()
Constructor.
Definition: surface_normal_modality.h:59
pcl::SurfaceNormalModality::getQuantizedMap
QuantizedMap & getQuantizedMap()
Returns a reference to the internal quantized map.
Definition: surface_normal_modality.h:370
pcl::QuantizedNormalLookUpTable::size_y
int size_y
The size of the LUT in y-direction.
Definition: surface_normal_modality.h:144
pcl::QuantizedMultiModFeature::x
int x
x-position.
Definition: sparse_quantized_multi_mod_template.h:57
pcl::LinearLeastSquaresNormalEstimation::setNormalSmoothingSize
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Definition: linear_least_squares_normal.h:91
pcl::LINEMOD_OrientationMap::getHeight
size_t getHeight() const
Returns the height of the modality data map.
Definition: surface_normal_modality.h:72
pcl::LinearLeastSquaresNormalEstimation::setInputCloud
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
Definition: linear_least_squares_normal.h:119
pcl::MaskMap::getWidth
size_t getWidth() const
Definition: mask_map.h:58
pcl::LinearLeastSquaresNormalEstimation::setMaxDepthChangeFactor
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
Definition: linear_least_squares_normal.h:110
pcl::SurfaceNormalModality::getSurfaceNormals
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
Definition: surface_normal_modality.h:356
pcl::SurfaceNormalModality::filterQuantizedSurfaceNormals
void filterQuantizedSurfaceNormals()
Filters the quantized surface normals.
Definition: surface_normal_modality.h:1407
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:287
pcl::SurfaceNormalModality::quantizeSurfaceNormals
void quantizeSurfaceNormals()
Quantizes the surface normals.
Definition: surface_normal_modality.h:1366
pcl::SurfaceNormalModality::getSpreadedQuantizedMap
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internal spread quantized map.
Definition: surface_normal_modality.h:377
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
pcl::LinearLeastSquaresNormalEstimation
Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylo...
Definition: linear_least_squares_normal.h:52
pcl::QuantizedNormalLookUpTable::offset_x
int offset_x
The offset in x-direction.
Definition: surface_normal_modality.h:135
pcl::SurfaceNormalModality::Candidate::bin_index
unsigned char bin_index
Quantized value.
Definition: surface_normal_modality.h:311
pcl::QuantizedNormalLookUpTable::size_x
int size_x
The size of the LUT in x-direction.
Definition: surface_normal_modality.h:142
pcl::SurfaceNormalModality::computeAndQuantizeSurfaceNormals2
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
Definition: surface_normal_modality.h:724
pcl::MaskMap::getData
unsigned char * getData()
Definition: mask_map.h:64
pcl::QuantizedNormalLookUpTable
Look-up-table for fast surface normal quantization.
Definition: surface_normal_modality.h:125
pcl::DistanceMap::getData
float * getData()
Returns a pointer to the beginning of map.
Definition: distance_map.h:71
pcl::SurfaceNormalModality::processInputDataFromFiltered
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
Definition: surface_normal_modality.h:535
pcl::DistanceMap::resize
void resize(const size_t width, const size_t height)
Resizes the map to the specified size.
Definition: distance_map.h:81
pcl::MaskMap
Definition: mask_map.h:47
pcl::QuantizedNormalLookUpTable::initializeLUT
void initializeLUT(const int range_x_arg, const int range_y_arg, const int range_z_arg)
Initializes the LUT.
Definition: surface_normal_modality.h:171
pcl::SurfaceNormalModality::Candidate::x
size_t x
x-position of the feature.
Definition: surface_normal_modality.h:314
pcl::SurfaceNormalModality::SurfaceNormalModality
SurfaceNormalModality()
Constructor.
Definition: surface_normal_modality.h:492
pcl::QuantizedNormalLookUpTable::range_x
int range_x
The range of the LUT in x-direction.
Definition: surface_normal_modality.h:128
pcl::QuantizedMap::spreadQuantizedMap
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, size_t spreading_size)
pcl::QuantizedMultiModFeature::modality_index
size_t modality_index
the index of the corresponding modality.
Definition: sparse_quantized_multi_mod_template.h:61
pcl::PointCloud< PointXYZT >::ConstPtr
boost::shared_ptr< const PointCloud< PointXYZT > > ConstPtr
Definition: point_cloud.h:429
pcl::MaskMap::getHeight
size_t getHeight() const
Definition: mask_map.h:61
pcl::SurfaceNormalModality::computeAndQuantizeSurfaceNormals
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
Definition: surface_normal_modality.h:558
pcl::SurfaceNormalModality::Candidate::operator<
bool operator<(const Candidate &rhs) const
Compares two candidates based on their distance to the next different quantized value.
Definition: surface_normal_modality.h:322
pcl::DistanceMap
Represents a distance map obtained from a distance transformation.
Definition: distance_map.h:47
pcl::QuantizedNormalLookUpTable::range_y
int range_y
The range of the LUT in y-direction.
Definition: surface_normal_modality.h:130
pcl::SurfaceNormalModality::setSpreadingSize
void setSpreadingSize(const size_t spreading_size)
Sets the spreading size.
Definition: surface_normal_modality.h:340
pcl::SurfaceNormalModality::extractAllFeatures
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.
Definition: surface_normal_modality.h:1237
pcl::QuantizedNormalLookUpTable::~QuantizedNormalLookUpTable
~QuantizedNormalLookUpTable()
Destructor.
Definition: surface_normal_modality.h:159