Point Cloud Library (PCL) 1.12.0
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
49namespace 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
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
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//////////////////////////////////////////////////////////////////////////////////////////////
138template <typename PointInT>
140ColorGradientDOTModality (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//////////////////////////////////////////////////////////////////////////////////////////////
146template <typename PointInT>
149{
150}
151
152//////////////////////////////////////////////////////////////////////////////////////////////
153template <typename PointInT>
154void
157{
158 // extract color gradients
159 computeMaxColorGradients ();
160
161 // compute dominant quantized gradient map
162 computeDominantQuantizedGradients ();
163
164 // compute invariant quantized gradient map
165 //computeInvariantQuantizedGradients ();
166}
167
168//////////////////////////////////////////////////////////////////////////////////////////////
169template <typename PointInT>
170void
173{
174 const int width = input_->width;
175 const int height = input_->height;
176
177 color_gradients_.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_)[index0].r;
191 const unsigned char g0 = (*input_)[index0].g;
192 const unsigned char b0 = (*input_)[index0].b;
193
194 const unsigned char r_c = (*input_)[index_c].r;
195 const unsigned char g_c = (*input_)[index_c].g;
196 const unsigned char b_c = (*input_)[index_c].b;
197
198 const unsigned char r_r = (*input_)[index_r].r;
199 const unsigned char g_r = (*input_)[index_r].g;
200 const unsigned char b_r = (*input_)[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//////////////////////////////////////////////////////////////////////////////////////////////
244template <typename PointInT>
245void
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//////////////////////////////////////////////////////////////////////////////////////////////
310template <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}
void setGradientMagnitudeThreshold(const float threshold)
QuantizedMap computeInvariantQuantizedMap(const MaskMap &mask, const RegionXY &region)
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