Point Cloud Library (PCL)  1.9.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 ();
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>
149 {
150 }
151 
152 //////////////////////////////////////////////////////////////////////////////////////////////
153 template <typename PointInT>
154 void
157 {
158  // extract color gradients
160 
161  // compute dominant quantized gradient map
163 
164  // compute invariant quantized gradient map
165  //computeInvariantQuantizedGradients ();
166 }
167 
168 //////////////////////////////////////////////////////////////////////////////////////////////
169 template <typename PointInT>
170 void
173 {
174  const int width = input_->width;
175  const int height = input_->height;
176 
177  color_gradients_.points.resize (width*height);
178  color_gradients_.width = width;
179  color_gradients_.height = height;
180 
181  constexpr float pi = std::tan(1.0f)*4;
182  for (int row_index = 0; row_index < height-2; ++row_index)
183  {
184  for (int col_index = 0; col_index < width-2; ++col_index)
185  {
186  const int index0 = row_index*width+col_index;
187  const int index_c = row_index*width+col_index+2;
188  const int index_r = (row_index+2)*width+col_index;
189 
190  const unsigned char r0 = input_->points[index0].r;
191  const unsigned char g0 = input_->points[index0].g;
192  const unsigned char b0 = input_->points[index0].b;
193 
194  const unsigned char r_c = input_->points[index_c].r;
195  const unsigned char g_c = input_->points[index_c].g;
196  const unsigned char b_c = input_->points[index_c].b;
197 
198  const unsigned char r_r = input_->points[index_r].r;
199  const unsigned char g_r = input_->points[index_r].g;
200  const unsigned char b_r = input_->points[index_r].b;
201 
202  const float r_dx = static_cast<float> (r_c) - static_cast<float> (r0);
203  const float g_dx = static_cast<float> (g_c) - static_cast<float> (g0);
204  const float b_dx = static_cast<float> (b_c) - static_cast<float> (b0);
205 
206  const float r_dy = static_cast<float> (r_r) - static_cast<float> (r0);
207  const float g_dy = static_cast<float> (g_r) - static_cast<float> (g0);
208  const float b_dy = static_cast<float> (b_r) - static_cast<float> (b0);
209 
210  const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
211  const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
212  const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
213 
214  GradientXY gradient;
215  gradient.x = col_index;
216  gradient.y = row_index;
217  if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
218  {
219  gradient.magnitude = sqrt (sqr_mag_r);
220  gradient.angle = std::atan2 (r_dy, r_dx) * 180.0f / pi;
221  }
222  else if (sqr_mag_g > sqr_mag_b)
223  {
224  gradient.magnitude = sqrt (sqr_mag_g);
225  gradient.angle = std::atan2 (g_dy, g_dx) * 180.0f / pi;
226  }
227  else
228  {
229  gradient.magnitude = sqrt (sqr_mag_b);
230  gradient.angle = std::atan2 (b_dy, b_dx) * 180.0f / pi;
231  }
232 
233  assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
234  color_gradients_ (col_index+1, row_index+1).angle <= 180);
235 
236  color_gradients_ (col_index+1, row_index+1) = gradient;
237  }
238  }
239 
240  return;
241 }
242 
243 //////////////////////////////////////////////////////////////////////////////////////////////
244 template <typename PointInT>
245 void
248 {
249  const std::size_t input_width = input_->width;
250  const std::size_t input_height = input_->height;
251 
252  const std::size_t output_width = input_width / bin_size_;
253  const std::size_t output_height = input_height / bin_size_;
254 
255  dominant_quantized_color_gradients_.resize (output_width, output_height);
256 
257  constexpr std::size_t num_gradient_bins = 7;
258 
259  constexpr float divisor = 180.0f / (num_gradient_bins - 1.0f);
260 
261  unsigned char * peak_pointer = dominant_quantized_color_gradients_.getData ();
262  memset (peak_pointer, 0, output_width*output_height);
263 
264  for (std::size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index)
265  {
266  for (std::size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index)
267  {
268  const std::size_t x_position = col_bin_index * bin_size_;
269  const std::size_t y_position = row_bin_index * bin_size_;
270 
271  float max_gradient = 0.0f;
272  std::size_t max_gradient_pos_x = 0;
273  std::size_t max_gradient_pos_y = 0;
274 
275  // find next location and value of maximum gradient magnitude in current region
276  for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
277  {
278  for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
279  {
280  const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
281 
282  if (magnitude > max_gradient)
283  {
284  max_gradient = magnitude;
285  max_gradient_pos_x = col_sub_index;
286  max_gradient_pos_y = row_sub_index;
287  }
288  }
289  }
290 
291  if (max_gradient >= gradient_magnitude_threshold_)
292  {
293  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);
294  const std::size_t bin_index = static_cast<std::size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
295 
296  *peak_pointer |= 1 << bin_index;
297  }
298 
299  if (*peak_pointer == 0)
300  {
301  *peak_pointer |= 1 << 7;
302  }
303 
304  ++peak_pointer;
305  }
306  }
307 }
308 
309 //////////////////////////////////////////////////////////////////////////////////////////////
310 template <typename PointInT>
314  const RegionXY & region)
315 {
316  const std::size_t input_width = input_->width;
317  const std::size_t input_height = input_->height;
318 
319  const std::size_t output_width = input_width / bin_size_;
320  const std::size_t output_height = input_height / bin_size_;
321 
322  const std::size_t sub_start_x = region.x / bin_size_;
323  const std::size_t sub_start_y = region.y / bin_size_;
324  const std::size_t sub_width = region.width / bin_size_;
325  const std::size_t sub_height = region.height / bin_size_;
326 
327  QuantizedMap map;
328  map.resize (sub_width, sub_height);
329 
330  constexpr std::size_t num_gradient_bins = 7;
331  constexpr std::size_t max_num_of_gradients = 7;
332 
333  const float divisor = 180.0f / (num_gradient_bins - 1.0f);
334 
335  float global_max_gradient = 0.0f;
336  float local_max_gradient = 0.0f;
337 
338  unsigned char * peak_pointer = map.getData ();
339 
340  for (std::size_t row_bin_index = 0; row_bin_index < sub_height; ++row_bin_index)
341  {
342  for (std::size_t col_bin_index = 0; col_bin_index < sub_width; ++col_bin_index)
343  {
344  std::vector<std::size_t> x_coordinates;
345  std::vector<std::size_t> y_coordinates;
346  std::vector<float> values;
347 
348  for (int row_pixel_index = -static_cast<int> (bin_size_)/2;
349  row_pixel_index <= static_cast<int> (bin_size_)/2;
350  row_pixel_index += static_cast<int> (bin_size_)/2)
351  {
352  const std::size_t y_position = row_pixel_index + (sub_start_y + row_bin_index)*bin_size_;
353 
354  if (y_position < 0 || y_position >= input_height)
355  continue;
356 
357  for (int col_pixel_index = -static_cast<int> (bin_size_)/2;
358  col_pixel_index <= static_cast<int> (bin_size_)/2;
359  col_pixel_index += static_cast<int> (bin_size_)/2)
360  {
361  const std::size_t x_position = col_pixel_index + (sub_start_x + col_bin_index)*bin_size_;
362  std::size_t counter = 0;
363 
364  if (x_position < 0 || x_position >= input_width)
365  continue;
366 
367  // find maximum gradient magnitude in current bin
368  {
369  local_max_gradient = 0.0f;
370  for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
371  {
372  for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
373  {
374  const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
375 
376  if (magnitude > local_max_gradient)
377  local_max_gradient = magnitude;
378  }
379  }
380  }
381 
382  if (local_max_gradient > global_max_gradient)
383  {
384  global_max_gradient = local_max_gradient;
385  }
386 
387  // iteratively search for the largest gradients, set it to -1, search the next largest ... etc.
388  while (true)
389  {
390  float max_gradient;
391  std::size_t max_gradient_pos_x;
392  std::size_t max_gradient_pos_y;
393 
394  // find next location and value of maximum gradient magnitude in current region
395  {
396  max_gradient = 0.0f;
397  for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
398  {
399  for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
400  {
401  const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
402 
403  if (magnitude > max_gradient)
404  {
405  max_gradient = magnitude;
406  max_gradient_pos_x = col_sub_index;
407  max_gradient_pos_y = row_sub_index;
408  }
409  }
410  }
411  }
412 
413  // TODO: really localMaxGradient and not maxGradient???
414  if (local_max_gradient < gradient_magnitude_threshold_)
415  {
416  //*peakPointer |= 1 << (numOfGradientBins-1);
417  break;
418  }
419 
420  // TODO: replace gradient_magnitude_threshold_ here by a fixed ratio?
421  if (/*max_gradient < (local_max_gradient * gradient_magnitude_threshold_) ||*/
422  counter >= max_num_of_gradients)
423  {
424  break;
425  }
426 
427  ++counter;
428 
429  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);
430  const std::size_t bin_index = static_cast<std::size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
431 
432  *peak_pointer |= 1 << bin_index;
433 
434  x_coordinates.push_back (max_gradient_pos_x + x_position);
435  y_coordinates.push_back (max_gradient_pos_y + y_position);
436  values.push_back (max_gradient);
437 
438  color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f;
439  }
440 
441  // reset values which have been set to -1
442  for (std::size_t value_index = 0; value_index < values.size (); ++value_index)
443  {
444  color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index];
445  }
446 
447  x_coordinates.clear ();
448  y_coordinates.clear ();
449  values.clear ();
450  }
451  }
452 
453  if (*peak_pointer == 0)
454  {
455  *peak_pointer |= 1 << 7;
456  }
457  ++peak_pointer;
458  }
459  }
460 
461  return map;
462 }
QuantizedMap computeInvariantQuantizedMap(const MaskMap &mask, const RegionXY &region)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:393
void resize(std::size_t width, std::size_t height)
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:463
Defines a region in XY-space.
Definition: region_xy.h:81
A point structure representing Euclidean xyz coordinates, and the intensity value.
Definition: point_types.h:51
int x
x-position of the region.
Definition: region_xy.h:87
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:396
int y
y-position of the region.
Definition: region_xy.h:89
Defines all the PCL implemented PointT point type structures.
PCL base class.
Definition: pcl_base.h:69
int height
height of the region.
Definition: region_xy.h:93
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:588
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:398
void setGradientMagnitudeThreshold(const float threshold)
unsigned char * getData()
Definition: quantized_map.h:62
boost::shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:412
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
int width
width of the region.
Definition: region_xy.h:91