Point Cloud Library (PCL)  1.14.1-dev
color_gradient_dot_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/pcl_base.h>
41 #include <pcl/point_cloud.h>
42 #include <pcl/point_types.h>
43 
44 #include <pcl/recognition/dot_modality.h>
45 #include <pcl/recognition/point_types.h>
46 #include <pcl/recognition/quantized_map.h>
47 
48 
49 namespace pcl
50 {
51  template <typename PointInT>
53  : public DOTModality, public PCLBase<PointInT>
54  {
55  protected:
57 
58  struct Candidate
59  {
61 
62  int x;
63  int y;
64 
65  bool operator< (const Candidate & rhs)
66  {
67  return (gradient.magnitude > rhs.gradient.magnitude);
68  }
69  };
70 
71  public:
73 
74  ColorGradientDOTModality (std::size_t bin_size);
75 
76  virtual ~ColorGradientDOTModality () = default;
77 
78  inline void
79  setGradientMagnitudeThreshold (const float threshold)
80  {
81  gradient_magnitude_threshold_ = threshold;
82  }
83 
84  //inline QuantizedMap &
85  //getDominantQuantizedMap ()
86  //{
87  // return (dominant_quantized_color_gradients_);
88  //}
89 
90  inline QuantizedMap &
92  {
93  return (dominant_quantized_color_gradients_);
94  }
95 
98  const RegionXY & region);
99 
100  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
101  * \param cloud the const boost shared pointer to a PointCloud message
102  */
103  virtual void
104  setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
105  {
106  input_ = cloud;
107  //processInputData ();
108  }
109 
110  virtual void
111  processInputData ();
112 
113  protected:
114 
115  void
117 
118  void
120 
121  //void
122  //computeInvariantQuantizedGradients ();
123 
124  private:
125  std::size_t bin_size_;
126 
127  float gradient_magnitude_threshold_;
128  pcl::PointCloud<pcl::GradientXY> color_gradients_;
129 
130  pcl::QuantizedMap dominant_quantized_color_gradients_;
131  //pcl::QuantizedMap invariant_quantized_color_gradients_;
132 
133  };
134 
135 }
136 
137 //////////////////////////////////////////////////////////////////////////////////////////////
138 template <typename PointInT>
140 ColorGradientDOTModality (const std::size_t bin_size)
141  : bin_size_ (bin_size), gradient_magnitude_threshold_ (80.0f), color_gradients_ (), dominant_quantized_color_gradients_ ()
142 {
143 }
144 
145 //////////////////////////////////////////////////////////////////////////////////////////////
146 template <typename PointInT>
147 void
150 {
151  // extract color gradients
152  computeMaxColorGradients ();
153 
154  // compute dominant quantized gradient map
155  computeDominantQuantizedGradients ();
156 
157  // compute invariant quantized gradient map
158  //computeInvariantQuantizedGradients ();
159 }
160 
161 //////////////////////////////////////////////////////////////////////////////////////////////
162 template <typename PointInT>
163 void
166 {
167  const int width = input_->width;
168  const int height = input_->height;
169 
170  color_gradients_.resize (width*height);
171  color_gradients_.width = width;
172  color_gradients_.height = height;
173 
174  constexpr float pi = std::tan(1.0f)*4;
175  for (int row_index = 0; row_index < height-2; ++row_index)
176  {
177  for (int col_index = 0; col_index < width-2; ++col_index)
178  {
179  const int index0 = row_index*width+col_index;
180  const int index_c = row_index*width+col_index+2;
181  const int index_r = (row_index+2)*width+col_index;
182 
183  const unsigned char r0 = (*input_)[index0].r;
184  const unsigned char g0 = (*input_)[index0].g;
185  const unsigned char b0 = (*input_)[index0].b;
186 
187  const unsigned char r_c = (*input_)[index_c].r;
188  const unsigned char g_c = (*input_)[index_c].g;
189  const unsigned char b_c = (*input_)[index_c].b;
190 
191  const unsigned char r_r = (*input_)[index_r].r;
192  const unsigned char g_r = (*input_)[index_r].g;
193  const unsigned char b_r = (*input_)[index_r].b;
194 
195  const float r_dx = static_cast<float> (r_c) - static_cast<float> (r0);
196  const float g_dx = static_cast<float> (g_c) - static_cast<float> (g0);
197  const float b_dx = static_cast<float> (b_c) - static_cast<float> (b0);
198 
199  const float r_dy = static_cast<float> (r_r) - static_cast<float> (r0);
200  const float g_dy = static_cast<float> (g_r) - static_cast<float> (g0);
201  const float b_dy = static_cast<float> (b_r) - static_cast<float> (b0);
202 
203  const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
204  const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
205  const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
206 
207  GradientXY gradient;
208  gradient.x = col_index;
209  gradient.y = row_index;
210  if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
211  {
212  gradient.magnitude = sqrt (sqr_mag_r);
213  gradient.angle = std::atan2 (r_dy, r_dx) * 180.0f / pi;
214  }
215  else if (sqr_mag_g > sqr_mag_b)
216  {
217  gradient.magnitude = sqrt (sqr_mag_g);
218  gradient.angle = std::atan2 (g_dy, g_dx) * 180.0f / pi;
219  }
220  else
221  {
222  gradient.magnitude = sqrt (sqr_mag_b);
223  gradient.angle = std::atan2 (b_dy, b_dx) * 180.0f / pi;
224  }
225 
226  assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
227  color_gradients_ (col_index+1, row_index+1).angle <= 180);
228 
229  color_gradients_ (col_index+1, row_index+1) = gradient;
230  }
231  }
232 
233  return;
234 }
235 
236 //////////////////////////////////////////////////////////////////////////////////////////////
237 template <typename PointInT>
238 void
241 {
242  const std::size_t input_width = input_->width;
243  const std::size_t input_height = input_->height;
244 
245  const std::size_t output_width = input_width / bin_size_;
246  const std::size_t output_height = input_height / bin_size_;
247 
248  dominant_quantized_color_gradients_.resize (output_width, output_height);
249 
250  constexpr std::size_t num_gradient_bins = 7;
251 
252  constexpr float divisor = 180.0f / (num_gradient_bins - 1.0f);
253 
254  unsigned char * peak_pointer = dominant_quantized_color_gradients_.getData ();
255  std::fill_n(peak_pointer, output_width*output_height, 0);
256 
257  for (std::size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index)
258  {
259  for (std::size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index)
260  {
261  const std::size_t x_position = col_bin_index * bin_size_;
262  const std::size_t y_position = row_bin_index * bin_size_;
263 
264  float max_gradient = 0.0f;
265  std::size_t max_gradient_pos_x = 0;
266  std::size_t max_gradient_pos_y = 0;
267 
268  // find next location and value of maximum gradient magnitude in current region
269  for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
270  {
271  for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
272  {
273  const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
274 
275  if (magnitude > max_gradient)
276  {
277  max_gradient = magnitude;
278  max_gradient_pos_x = col_sub_index;
279  max_gradient_pos_y = row_sub_index;
280  }
281  }
282  }
283 
284  if (max_gradient >= gradient_magnitude_threshold_)
285  {
286  const std::size_t angle = static_cast<std::size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f);
287  const std::size_t bin_index = static_cast<std::size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
288 
289  *peak_pointer |= 1 << bin_index;
290  }
291 
292  if (*peak_pointer == 0)
293  {
294  *peak_pointer |= 1 << 7;
295  }
296 
297  ++peak_pointer;
298  }
299  }
300 }
301 
302 //////////////////////////////////////////////////////////////////////////////////////////////
303 template <typename PointInT>
307  const RegionXY & region)
308 {
309  const std::size_t input_width = input_->width;
310  const std::size_t input_height = input_->height;
311 
312  const std::size_t output_width = input_width / bin_size_;
313  const std::size_t output_height = input_height / bin_size_;
314 
315  const std::size_t sub_start_x = region.x / bin_size_;
316  const std::size_t sub_start_y = region.y / bin_size_;
317  const std::size_t sub_width = region.width / bin_size_;
318  const std::size_t sub_height = region.height / bin_size_;
319 
320  QuantizedMap map;
321  map.resize (sub_width, sub_height);
322 
323  constexpr std::size_t num_gradient_bins = 7;
324  constexpr std::size_t max_num_of_gradients = 7;
325 
326  const float divisor = 180.0f / (num_gradient_bins - 1.0f);
327 
328  float global_max_gradient = 0.0f;
329  float local_max_gradient = 0.0f;
330 
331  unsigned char * peak_pointer = map.getData ();
332 
333  for (std::size_t row_bin_index = 0; row_bin_index < sub_height; ++row_bin_index)
334  {
335  for (std::size_t col_bin_index = 0; col_bin_index < sub_width; ++col_bin_index)
336  {
337  std::vector<std::size_t> x_coordinates;
338  std::vector<std::size_t> y_coordinates;
339  std::vector<float> values;
340 
341  for (int row_pixel_index = -static_cast<int> (bin_size_)/2;
342  row_pixel_index <= static_cast<int> (bin_size_)/2;
343  row_pixel_index += static_cast<int> (bin_size_)/2)
344  {
345  const std::size_t y_position = row_pixel_index + (sub_start_y + row_bin_index)*bin_size_;
346 
347  if (y_position < 0 || y_position >= input_height)
348  continue;
349 
350  for (int col_pixel_index = -static_cast<int> (bin_size_)/2;
351  col_pixel_index <= static_cast<int> (bin_size_)/2;
352  col_pixel_index += static_cast<int> (bin_size_)/2)
353  {
354  const std::size_t x_position = col_pixel_index + (sub_start_x + col_bin_index)*bin_size_;
355  std::size_t counter = 0;
356 
357  if (x_position < 0 || x_position >= input_width)
358  continue;
359 
360  // find maximum gradient magnitude in current bin
361  {
362  local_max_gradient = 0.0f;
363  for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
364  {
365  for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
366  {
367  const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
368 
369  if (magnitude > local_max_gradient)
370  local_max_gradient = magnitude;
371  }
372  }
373  }
374 
375  if (local_max_gradient > global_max_gradient)
376  {
377  global_max_gradient = local_max_gradient;
378  }
379 
380  // iteratively search for the largest gradients, set it to -1, search the next largest ... etc.
381  while (true)
382  {
383  float max_gradient;
384  std::size_t max_gradient_pos_x;
385  std::size_t max_gradient_pos_y;
386 
387  // find next location and value of maximum gradient magnitude in current region
388  {
389  max_gradient = 0.0f;
390  for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
391  {
392  for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
393  {
394  const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
395 
396  if (magnitude > max_gradient)
397  {
398  max_gradient = magnitude;
399  max_gradient_pos_x = col_sub_index;
400  max_gradient_pos_y = row_sub_index;
401  }
402  }
403  }
404  }
405 
406  // TODO: really localMaxGradient and not maxGradient???
407  if (local_max_gradient < gradient_magnitude_threshold_)
408  {
409  //*peakPointer |= 1 << (numOfGradientBins-1);
410  break;
411  }
412 
413  // TODO: replace gradient_magnitude_threshold_ here by a fixed ratio?
414  if (/*max_gradient < (local_max_gradient * gradient_magnitude_threshold_) ||*/
415  counter >= max_num_of_gradients)
416  {
417  break;
418  }
419 
420  ++counter;
421 
422  const std::size_t angle = static_cast<std::size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f);
423  const std::size_t bin_index = static_cast<std::size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
424 
425  *peak_pointer |= 1 << bin_index;
426 
427  x_coordinates.push_back (max_gradient_pos_x + x_position);
428  y_coordinates.push_back (max_gradient_pos_y + y_position);
429  values.push_back (max_gradient);
430 
431  color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f;
432  }
433 
434  // reset values which have been set to -1
435  for (std::size_t value_index = 0; value_index < values.size (); ++value_index)
436  {
437  color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index];
438  }
439 
440  x_coordinates.clear ();
441  y_coordinates.clear ();
442  values.clear ();
443  }
444  }
445 
446  if (*peak_pointer == 0)
447  {
448  *peak_pointer |= 1 << 7;
449  }
450  ++peak_pointer;
451  }
452  }
453 
454  return map;
455 }
void setGradientMagnitudeThreshold(const float threshold)
QuantizedMap computeInvariantQuantizedMap(const MaskMap &mask, const RegionXY &region)
virtual ~ColorGradientDOTModality()=default
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
PCL base class.
Definition: pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:414
unsigned char * getData()
Definition: quantized_map.h:62
void resize(std::size_t width, std::size_t height)
Defines all the PCL implemented PointT point type structures.
A point structure representing Euclidean xyz coordinates, and the intensity value.
Definition: point_types.h:53
Defines a region in XY-space.
Definition: region_xy.h:82
int x
x-position of the region.
Definition: region_xy.h:87
int width
width of the region.
Definition: region_xy.h:91
int y
y-position of the region.
Definition: region_xy.h:89
int height
height of the region.
Definition: region_xy.h:93