Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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:
56 using PCLBase<PointInT>::input_;
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
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>
147void
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//////////////////////////////////////////////////////////////////////////////////////////////
162template <typename PointInT>
163void
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//////////////////////////////////////////////////////////////////////////////////////////////
237template <typename PointInT>
238void
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//////////////////////////////////////////////////////////////////////////////////////////////
303template <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
unsigned char * getData()
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