Point Cloud Library (PCL)  1.12.0
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
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 }
Represents a distance map obtained from a distance transformation.
Definition: distance_map.h:47
float * getData()
Returns a pointer to the beginning of map.
Definition: distance_map.h:70
void resize(const std::size_t width, const std::size_t height)
Resizes the map to the specified size.
Definition: distance_map.h:80
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylo...
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
std::size_t getWidth() const
Definition: mask_map.h:57
unsigned char * getData()
Definition: mask_map.h:69
void resize(std::size_t width, std::size_t height)
std::size_t getHeight() const
Definition: mask_map.h:63
PCL base class.
Definition: pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:411
shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:414
Interface for a quantizable modality.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, std::size_t spreading_size)
Modality based on surface normals.
void computeSurfaceNormals()
Computes the surface normals from the input cloud.
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
void setSpreadingSize(const std::size_t spreading_size)
Sets the spreading size.
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
QuantizedMap & getSpreadedQuantizedMap() override
Returns a reference to the internal spread quantized map.
void quantizeSurfaceNormals()
Quantizes the surface normals.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
QuantizedMap & getQuantizedMap() override
Returns a reference to the internal quantized map.
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.
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
void setVariableFeatureNr(const bool enabled)
Enables/disables the use of extracting a variable number of features.
void filterQuantizedSurfaceNormals()
Filters the quantized surface normals.
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.
Defines all the PCL implemented PointT point type structures.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
#define PCL_EXPORTS
Definition: pcl_macros.h:323
#define M_PI
Definition: pcl_macros.h:201
Map that stores orientations.
std::size_t getWidth() const
Returns the width of the modality data map.
std::size_t getHeight() const
Returns the height of the modality data map.
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 ...
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates.
Feature that defines a position and quantized value in a specific modality.
std::size_t modality_index
the index of the corresponding modality.
unsigned char quantized_value
the quantized value attached to the feature.
Look-up-table for fast surface normal quantization.
int size_y
The size of the LUT in y-direction.
void initializeLUT(const int range_x_arg, const int range_y_arg, const int range_z_arg)
Initializes the LUT.
int size_x
The size of the LUT in x-direction.
unsigned char operator()(const float x, const float y, const float z) const
Operator to access an element in the LUT.
int range_y
The range of the LUT in y-direction.
int offset_x
The offset in x-direction.
unsigned char * lut
The LUT data.
int offset_z
The offset in z-direction.
int range_z
The range of the LUT in z-direction.
int size_z
The size of the LUT in z-direction.
int range_x
The range of the LUT in x-direction.
int offset_y
The offset in y-direction.
Candidate for a feature (used in feature extraction methods).
float distance
Distance to the next different quantized value.
std::size_t x
x-position of the feature.
std::size_t y
y-position of the feature.
bool operator<(const Candidate &rhs) const
Compares two candidates based on their distance to the next different quantized value.