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