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