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