Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
color_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/recognition/point_types.h>
47
48#include <algorithm>
49#include <cstddef>
50#include <list>
51#include <vector>
52
53namespace pcl
54{
55
56 // --------------------------------------------------------------------------
57
58 template <typename PointInT>
60 : public QuantizableModality, public PCLBase<PointInT>
61 {
62 protected:
63 using PCLBase<PointInT>::input_;
64
65 struct Candidate
66 {
67 float distance;
68
69 unsigned char bin_index;
70
71 std::size_t x;
72 std::size_t y;
73
74 bool
75 operator< (const Candidate & rhs)
76 {
77 return (distance > rhs.distance);
78 }
79 };
80
81 public:
83
85
86 virtual ~ColorModality ();
87
88 inline QuantizedMap &
90 {
91 return (filtered_quantized_colors_);
92 }
93
94 inline QuantizedMap &
96 {
97 return (spreaded_filtered_quantized_colors_);
98 }
99
100 void
101 extractFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modalityIndex,
102 std::vector<QuantizedMultiModFeature> & features) const;
103
104 /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
105 * \param cloud the const boost shared pointer to a PointCloud message
106 */
107 virtual void
108 setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
109 {
110 input_ = cloud;
111 }
112
113 virtual void
115
116 protected:
117
118 void
120
121 void
123
124 static inline int
125 quantizeColorOnRGBExtrema (const float r,
126 const float g,
127 const float b);
128
129 void
130 computeDistanceMap (const MaskMap & input, DistanceMap & output) const;
131
132 private:
133 float feature_distance_threshold_;
134
135 pcl::QuantizedMap quantized_colors_;
136 pcl::QuantizedMap filtered_quantized_colors_;
137 pcl::QuantizedMap spreaded_filtered_quantized_colors_;
138
139 };
140
141}
142
143//////////////////////////////////////////////////////////////////////////////////////////////
144template <typename PointInT>
146 : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ ()
147{
148}
149
150//////////////////////////////////////////////////////////////////////////////////////////////
151template <typename PointInT>
153{
154}
155
156//////////////////////////////////////////////////////////////////////////////////////////////
157template <typename PointInT>
158void
160{
161 // quantize gradients
162 quantizeColors ();
163
164 // filter quantized gradients to get only dominants one + thresholding
165 filterQuantizedColors ();
166
167 // spread filtered quantized gradients
168 //spreadFilteredQunatizedColorGradients ();
169 const int spreading_size = 8;
170 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_colors_,
171 spreaded_filtered_quantized_colors_, spreading_size);
172}
173
174//////////////////////////////////////////////////////////////////////////////////////////////
175template <typename PointInT>
177 const std::size_t nr_features,
178 const std::size_t modality_index,
179 std::vector<QuantizedMultiModFeature> & features) const
180{
181 const std::size_t width = mask.getWidth ();
182 const std::size_t height = mask.getHeight ();
183
184 MaskMap mask_maps[8];
185 for (std::size_t map_index = 0; map_index < 8; ++map_index)
186 mask_maps[map_index].resize (width, height);
187
188 unsigned char map[255];
189 memset(map, 0, 255);
190
191 map[0x1<<0] = 0;
192 map[0x1<<1] = 1;
193 map[0x1<<2] = 2;
194 map[0x1<<3] = 3;
195 map[0x1<<4] = 4;
196 map[0x1<<5] = 5;
197 map[0x1<<6] = 6;
198 map[0x1<<7] = 7;
199
200 QuantizedMap distance_map_indices (width, height);
201 //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
202
203 for (std::size_t row_index = 0; row_index < height; ++row_index)
204 {
205 for (std::size_t col_index = 0; col_index < width; ++col_index)
206 {
207 if (mask (col_index, row_index) != 0)
208 {
209 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
210 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
211
212 if (quantized_value == 0)
213 continue;
214 const int dist_map_index = map[quantized_value];
215
216 distance_map_indices (col_index, row_index) = dist_map_index;
217 //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
218 mask_maps[dist_map_index] (col_index, row_index) = 255;
219 }
220 }
221 }
222
223 DistanceMap distance_maps[8];
224 for (int map_index = 0; map_index < 8; ++map_index)
225 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
226
227 std::list<Candidate> list1;
228 std::list<Candidate> list2;
229
230 float weights[8] = {0,0,0,0,0,0,0,0};
231
232 const std::size_t off = 4;
233 for (std::size_t row_index = off; row_index < height-off; ++row_index)
234 {
235 for (std::size_t col_index = off; col_index < width-off; ++col_index)
236 {
237 if (mask (col_index, row_index) != 0)
238 {
239 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
240 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
241
242 //const float nx = surface_normals_ (col_index, row_index).normal_x;
243 //const float ny = surface_normals_ (col_index, row_index).normal_y;
244 //const float nz = surface_normals_ (col_index, row_index).normal_z;
245
246 if (quantized_value != 0)
247 {
248 const int distance_map_index = map[quantized_value];
249
250 //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
251 const float distance = distance_maps[distance_map_index] (col_index, row_index);
252
253 if (distance >= feature_distance_threshold_)
254 {
255 Candidate candidate;
256
257 candidate.distance = distance;
258 candidate.x = col_index;
259 candidate.y = row_index;
260 candidate.bin_index = distance_map_index;
261
262 list1.push_back (candidate);
263
264 ++weights[distance_map_index];
265 }
266 }
267 }
268 }
269 }
270
271 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
272 iter->distance *= 1.0f / weights[iter->bin_index];
273
274 list1.sort ();
275
276 if (list1.size () <= nr_features)
277 {
278 features.reserve (list1.size ());
279 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
280 {
282
283 feature.x = static_cast<int> (iter->x);
284 feature.y = static_cast<int> (iter->y);
285 feature.modality_index = modality_index;
286 feature.quantized_value = filtered_quantized_colors_ (iter->x, iter->y);
287
288 features.push_back (feature);
289 }
290
291 return;
292 }
293
294 int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:!
295 while (list2.size () != nr_features)
296 {
297 const int sqr_distance = distance*distance;
298 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
299 {
300 bool candidate_accepted = true;
301
302 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
303 {
304 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
305 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
306 const int tmp_distance = dx*dx + dy*dy;
307
308 if (tmp_distance < sqr_distance)
309 {
310 candidate_accepted = false;
311 break;
312 }
313 }
314
315 if (candidate_accepted)
316 list2.push_back (*iter1);
317
318 if (list2.size () == nr_features) break;
319 }
320 --distance;
321 }
322
323 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
324 {
326
327 feature.x = static_cast<int> (iter2->x);
328 feature.y = static_cast<int> (iter2->y);
329 feature.modality_index = modality_index;
330 feature.quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y);
331
332 features.push_back (feature);
333 }
334}
335
336//////////////////////////////////////////////////////////////////////////////////////////////
337template <typename PointInT>
338void
340{
341 const std::size_t width = input_->width;
342 const std::size_t height = input_->height;
343
344 quantized_colors_.resize (width, height);
345
346 for (std::size_t row_index = 0; row_index < height; ++row_index)
347 {
348 for (std::size_t col_index = 0; col_index < width; ++col_index)
349 {
350 const float r = static_cast<float> ((*input_) (col_index, row_index).r);
351 const float g = static_cast<float> ((*input_) (col_index, row_index).g);
352 const float b = static_cast<float> ((*input_) (col_index, row_index).b);
353
354 quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b);
355 }
356 }
357}
358
359//////////////////////////////////////////////////////////////////////////////////////////////
360template <typename PointInT>
361void
363{
364 const std::size_t width = input_->width;
365 const std::size_t height = input_->height;
366
367 filtered_quantized_colors_.resize (width, height);
368
369 // filter data
370 for (std::size_t row_index = 1; row_index < height-1; ++row_index)
371 {
372 for (std::size_t col_index = 1; col_index < width-1; ++col_index)
373 {
374 unsigned char histogram[8] = {0,0,0,0,0,0,0,0};
375
376 {
377 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index-1)*width+col_index-1;
378 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
379 0 <= data_ptr[1] && data_ptr[1] < 9 &&
380 0 <= data_ptr[2] && data_ptr[2] < 9);
381 ++histogram[data_ptr[0]];
382 ++histogram[data_ptr[1]];
383 ++histogram[data_ptr[2]];
384 }
385 {
386 const unsigned char * data_ptr = quantized_colors_.getData () + row_index*width+col_index-1;
387 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
388 0 <= data_ptr[1] && data_ptr[1] < 9 &&
389 0 <= data_ptr[2] && data_ptr[2] < 9);
390 ++histogram[data_ptr[0]];
391 ++histogram[data_ptr[1]];
392 ++histogram[data_ptr[2]];
393 }
394 {
395 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index+1)*width+col_index-1;
396 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
397 0 <= data_ptr[1] && data_ptr[1] < 9 &&
398 0 <= data_ptr[2] && data_ptr[2] < 9);
399 ++histogram[data_ptr[0]];
400 ++histogram[data_ptr[1]];
401 ++histogram[data_ptr[2]];
402 }
403
404 unsigned char max_hist_value = 0;
405 int max_hist_index = -1;
406
407 // for (int i = 0; i < 8; ++i)
408 // {
409 // if (max_hist_value < histogram[i+1])
410 // {
411 // max_hist_index = i;
412 // max_hist_value = histogram[i+1]
413 // }
414 // }
415 // Unrolled for performance optimization:
416 if (max_hist_value < histogram[0]) {max_hist_index = 0; max_hist_value = histogram[0];}
417 if (max_hist_value < histogram[1]) {max_hist_index = 1; max_hist_value = histogram[1];}
418 if (max_hist_value < histogram[2]) {max_hist_index = 2; max_hist_value = histogram[2];}
419 if (max_hist_value < histogram[3]) {max_hist_index = 3; max_hist_value = histogram[3];}
420 if (max_hist_value < histogram[4]) {max_hist_index = 4; max_hist_value = histogram[4];}
421 if (max_hist_value < histogram[5]) {max_hist_index = 5; max_hist_value = histogram[5];}
422 if (max_hist_value < histogram[6]) {max_hist_index = 6; max_hist_value = histogram[6];}
423 if (max_hist_value < histogram[7]) {max_hist_index = 7; max_hist_value = histogram[7];}
424
425 //if (max_hist_index != -1 && max_hist_value >= 5)
426 filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index;
427 //else
428 // filtered_quantized_color_gradients_ (col_index, row_index) = 0;
429
430 }
431 }
432}
433
434//////////////////////////////////////////////////////////////////////////////////////////////
435template <typename PointInT>
436int
438 const float g,
439 const float b)
440{
441 const float r_inv = 255.0f-r;
442 const float g_inv = 255.0f-g;
443 const float b_inv = 255.0f-b;
444
445 const float dist_0 = (r*r + g*g + b*b)*2.0f;
446 const float dist_1 = r*r + g*g + b_inv*b_inv;
447 const float dist_2 = r*r + g_inv*g_inv+ b*b;
448 const float dist_3 = r*r + g_inv*g_inv + b_inv*b_inv;
449 const float dist_4 = r_inv*r_inv + g*g + b*b;
450 const float dist_5 = r_inv*r_inv + g*g + b_inv*b_inv;
451 const float dist_6 = r_inv*r_inv + g_inv*g_inv+ b*b;
452 const float dist_7 = (r_inv*r_inv + g_inv*g_inv + b_inv*b_inv)*1.5f;
453
454 const float min_dist = std::min (std::min (std::min (dist_0, dist_1), std::min (dist_2, dist_3)), std::min (std::min (dist_4, dist_5), std::min (dist_6, dist_7)));
455
456 if (min_dist == dist_0)
457 {
458 return 0;
459 }
460 if (min_dist == dist_1)
461 {
462 return 1;
463 }
464 if (min_dist == dist_2)
465 {
466 return 2;
467 }
468 if (min_dist == dist_3)
469 {
470 return 3;
471 }
472 if (min_dist == dist_4)
473 {
474 return 4;
475 }
476 if (min_dist == dist_5)
477 {
478 return 5;
479 }
480 if (min_dist == dist_6)
481 {
482 return 6;
483 }
484 return 7;
485}
486
487//////////////////////////////////////////////////////////////////////////////////////////////
488template <typename PointInT> void
490 DistanceMap & output) const
491{
492 const std::size_t width = input.getWidth ();
493 const std::size_t height = input.getHeight ();
494
495 output.resize (width, height);
496
497 // compute distance map
498 //float *distance_map = new float[input_->size ()];
499 const unsigned char * mask_map = input.getData ();
500 float * distance_map = output.getData ();
501 for (std::size_t index = 0; index < width*height; ++index)
502 {
503 if (mask_map[index] == 0)
504 distance_map[index] = 0.0f;
505 else
506 distance_map[index] = static_cast<float> (width + height);
507 }
508
509 // first pass
510 float * previous_row = distance_map;
511 float * current_row = previous_row + width;
512 for (std::size_t ri = 1; ri < height; ++ri)
513 {
514 for (std::size_t ci = 1; ci < width; ++ci)
515 {
516 const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f;
517 const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f;
518 const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f;
519 const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f;
520 const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
521
522 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
523
524 if (min_value < center)
525 current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value;
526 }
527 previous_row = current_row;
528 current_row += width;
529 }
530
531 // second pass
532 float * next_row = distance_map + width * (height - 1);
533 current_row = next_row - width;
534 for (int ri = static_cast<int> (height)-2; ri >= 0; --ri)
535 {
536 for (int ci = static_cast<int> (width)-2; ci >= 0; --ci)
537 {
538 const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f;
539 const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f;
540 const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f;
541 const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f;
542 const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
543
544 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
545
546 if (min_value < center)
547 current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value;
548 }
549 next_row = current_row;
550 current_row -= width;
551 }
552}
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internally computed spread quantized map.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
static int quantizeColorOnRGBExtrema(const float r, const float g, const float b)
virtual void processInputData()
void extractFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modalityIndex, std::vector< QuantizedMultiModFeature > &features) const
Extracts features from this modality within the specified mask.
QuantizedMap & getQuantizedMap()
Returns a reference to the internally computed quantized map.
Represents a distance map obtained from a distance transformation.
void resize(const std::size_t width, const std::size_t height)
Resizes the map to the specified size.
float * getData()
Returns a pointer to the beginning of map.
std::size_t getWidth() const
Definition mask_map.h:57
std::size_t getHeight() const
Definition mask_map.h:63
unsigned char * getData()
Definition mask_map.h:69
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
Interface for a quantizable modality.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, std::size_t spreading_size)
Defines all the PCL implemented PointT point type structures.
bool operator<(const Candidate &rhs)
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.