Point Cloud Library (PCL) 1.13.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 () = default;
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>
152void
154{
155 // quantize gradients
156 quantizeColors ();
157
158 // filter quantized gradients to get only dominants one + thresholding
159 filterQuantizedColors ();
160
161 // spread filtered quantized gradients
162 //spreadFilteredQunatizedColorGradients ();
163 const int spreading_size = 8;
164 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_colors_,
165 spreaded_filtered_quantized_colors_, spreading_size);
166}
167
168//////////////////////////////////////////////////////////////////////////////////////////////
169template <typename PointInT>
171 const std::size_t nr_features,
172 const std::size_t modality_index,
173 std::vector<QuantizedMultiModFeature> & features) const
174{
175 const std::size_t width = mask.getWidth ();
176 const std::size_t height = mask.getHeight ();
177
178 MaskMap mask_maps[8];
179 for (std::size_t map_index = 0; map_index < 8; ++map_index)
180 mask_maps[map_index].resize (width, height);
181
182 unsigned char map[255]{};
183
184 map[0x1<<0] = 0;
185 map[0x1<<1] = 1;
186 map[0x1<<2] = 2;
187 map[0x1<<3] = 3;
188 map[0x1<<4] = 4;
189 map[0x1<<5] = 5;
190 map[0x1<<6] = 6;
191 map[0x1<<7] = 7;
192
193 QuantizedMap distance_map_indices (width, height);
194 //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
195
196 for (std::size_t row_index = 0; row_index < height; ++row_index)
197 {
198 for (std::size_t col_index = 0; col_index < width; ++col_index)
199 {
200 if (mask (col_index, row_index) != 0)
201 {
202 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
203 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
204
205 if (quantized_value == 0)
206 continue;
207 const int dist_map_index = map[quantized_value];
208
209 distance_map_indices (col_index, row_index) = dist_map_index;
210 //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
211 mask_maps[dist_map_index] (col_index, row_index) = 255;
212 }
213 }
214 }
215
216 DistanceMap distance_maps[8];
217 for (int map_index = 0; map_index < 8; ++map_index)
218 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
219
220 std::list<Candidate> list1;
221 std::list<Candidate> list2;
222
223 float weights[8] = {0,0,0,0,0,0,0,0};
224
225 const std::size_t off = 4;
226 for (std::size_t row_index = off; row_index < height-off; ++row_index)
227 {
228 for (std::size_t col_index = off; col_index < width-off; ++col_index)
229 {
230 if (mask (col_index, row_index) != 0)
231 {
232 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
233 const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index);
234
235 //const float nx = surface_normals_ (col_index, row_index).normal_x;
236 //const float ny = surface_normals_ (col_index, row_index).normal_y;
237 //const float nz = surface_normals_ (col_index, row_index).normal_z;
238
239 if (quantized_value != 0)
240 {
241 const int distance_map_index = map[quantized_value];
242
243 //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
244 const float distance = distance_maps[distance_map_index] (col_index, row_index);
245
246 if (distance >= feature_distance_threshold_)
247 {
248 Candidate candidate;
249
250 candidate.distance = distance;
251 candidate.x = col_index;
252 candidate.y = row_index;
253 candidate.bin_index = distance_map_index;
254
255 list1.push_back (candidate);
256
257 ++weights[distance_map_index];
258 }
259 }
260 }
261 }
262 }
263
264 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
265 iter->distance *= 1.0f / weights[iter->bin_index];
266
267 list1.sort ();
268
269 if (list1.size () <= nr_features)
270 {
271 features.reserve (list1.size ());
272 for (typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
273 {
275
276 feature.x = static_cast<int> (iter->x);
277 feature.y = static_cast<int> (iter->y);
278 feature.modality_index = modality_index;
279 feature.quantized_value = filtered_quantized_colors_ (iter->x, iter->y);
280
281 features.push_back (feature);
282 }
283
284 return;
285 }
286
287 int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:!
288 while (list2.size () != nr_features)
289 {
290 const int sqr_distance = distance*distance;
291 for (typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
292 {
293 bool candidate_accepted = true;
294
295 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
296 {
297 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
298 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
299 const int tmp_distance = dx*dx + dy*dy;
300
301 if (tmp_distance < sqr_distance)
302 {
303 candidate_accepted = false;
304 break;
305 }
306 }
307
308 if (candidate_accepted)
309 list2.push_back (*iter1);
310
311 if (list2.size () == nr_features) break;
312 }
313 --distance;
314 }
315
316 for (typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
317 {
319
320 feature.x = static_cast<int> (iter2->x);
321 feature.y = static_cast<int> (iter2->y);
322 feature.modality_index = modality_index;
323 feature.quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y);
324
325 features.push_back (feature);
326 }
327}
328
329//////////////////////////////////////////////////////////////////////////////////////////////
330template <typename PointInT>
331void
333{
334 const std::size_t width = input_->width;
335 const std::size_t height = input_->height;
336
337 quantized_colors_.resize (width, height);
338
339 for (std::size_t row_index = 0; row_index < height; ++row_index)
340 {
341 for (std::size_t col_index = 0; col_index < width; ++col_index)
342 {
343 const float r = static_cast<float> ((*input_) (col_index, row_index).r);
344 const float g = static_cast<float> ((*input_) (col_index, row_index).g);
345 const float b = static_cast<float> ((*input_) (col_index, row_index).b);
346
347 quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b);
348 }
349 }
350}
351
352//////////////////////////////////////////////////////////////////////////////////////////////
353template <typename PointInT>
354void
356{
357 const std::size_t width = input_->width;
358 const std::size_t height = input_->height;
359
360 filtered_quantized_colors_.resize (width, height);
361
362 // filter data
363 for (std::size_t row_index = 1; row_index < height-1; ++row_index)
364 {
365 for (std::size_t col_index = 1; col_index < width-1; ++col_index)
366 {
367 unsigned char histogram[8] = {0,0,0,0,0,0,0,0};
368
369 {
370 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index-1)*width+col_index-1;
371 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
372 0 <= data_ptr[1] && data_ptr[1] < 9 &&
373 0 <= data_ptr[2] && data_ptr[2] < 9);
374 ++histogram[data_ptr[0]];
375 ++histogram[data_ptr[1]];
376 ++histogram[data_ptr[2]];
377 }
378 {
379 const unsigned char * data_ptr = quantized_colors_.getData () + row_index*width+col_index-1;
380 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
381 0 <= data_ptr[1] && data_ptr[1] < 9 &&
382 0 <= data_ptr[2] && data_ptr[2] < 9);
383 ++histogram[data_ptr[0]];
384 ++histogram[data_ptr[1]];
385 ++histogram[data_ptr[2]];
386 }
387 {
388 const unsigned char * data_ptr = quantized_colors_.getData () + (row_index+1)*width+col_index-1;
389 assert (0 <= data_ptr[0] && data_ptr[0] < 9 &&
390 0 <= data_ptr[1] && data_ptr[1] < 9 &&
391 0 <= data_ptr[2] && data_ptr[2] < 9);
392 ++histogram[data_ptr[0]];
393 ++histogram[data_ptr[1]];
394 ++histogram[data_ptr[2]];
395 }
396
397 unsigned char max_hist_value = 0;
398 int max_hist_index = -1;
399
400 // for (int i = 0; i < 8; ++i)
401 // {
402 // if (max_hist_value < histogram[i+1])
403 // {
404 // max_hist_index = i;
405 // max_hist_value = histogram[i+1]
406 // }
407 // }
408 // Unrolled for performance optimization:
409 if (max_hist_value < histogram[0]) {max_hist_index = 0; max_hist_value = histogram[0];}
410 if (max_hist_value < histogram[1]) {max_hist_index = 1; max_hist_value = histogram[1];}
411 if (max_hist_value < histogram[2]) {max_hist_index = 2; max_hist_value = histogram[2];}
412 if (max_hist_value < histogram[3]) {max_hist_index = 3; max_hist_value = histogram[3];}
413 if (max_hist_value < histogram[4]) {max_hist_index = 4; max_hist_value = histogram[4];}
414 if (max_hist_value < histogram[5]) {max_hist_index = 5; max_hist_value = histogram[5];}
415 if (max_hist_value < histogram[6]) {max_hist_index = 6; max_hist_value = histogram[6];}
416 if (max_hist_value < histogram[7]) {max_hist_index = 7; max_hist_value = histogram[7];}
417
418 //if (max_hist_index != -1 && max_hist_value >= 5)
419 filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index;
420 //else
421 // filtered_quantized_color_gradients_ (col_index, row_index) = 0;
422
423 }
424 }
425}
426
427//////////////////////////////////////////////////////////////////////////////////////////////
428template <typename PointInT>
429int
431 const float g,
432 const float b)
433{
434 const float r_inv = 255.0f-r;
435 const float g_inv = 255.0f-g;
436 const float b_inv = 255.0f-b;
437
438 const float dist_0 = (r*r + g*g + b*b)*2.0f;
439 const float dist_1 = r*r + g*g + b_inv*b_inv;
440 const float dist_2 = r*r + g_inv*g_inv+ b*b;
441 const float dist_3 = r*r + g_inv*g_inv + b_inv*b_inv;
442 const float dist_4 = r_inv*r_inv + g*g + b*b;
443 const float dist_5 = r_inv*r_inv + g*g + b_inv*b_inv;
444 const float dist_6 = r_inv*r_inv + g_inv*g_inv+ b*b;
445 const float dist_7 = (r_inv*r_inv + g_inv*g_inv + b_inv*b_inv)*1.5f;
446
447 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)));
448
449 if (min_dist == dist_0)
450 {
451 return 0;
452 }
453 if (min_dist == dist_1)
454 {
455 return 1;
456 }
457 if (min_dist == dist_2)
458 {
459 return 2;
460 }
461 if (min_dist == dist_3)
462 {
463 return 3;
464 }
465 if (min_dist == dist_4)
466 {
467 return 4;
468 }
469 if (min_dist == dist_5)
470 {
471 return 5;
472 }
473 if (min_dist == dist_6)
474 {
475 return 6;
476 }
477 return 7;
478}
479
480//////////////////////////////////////////////////////////////////////////////////////////////
481template <typename PointInT> void
483 DistanceMap & output) const
484{
485 const std::size_t width = input.getWidth ();
486 const std::size_t height = input.getHeight ();
487
488 output.resize (width, height);
489
490 // compute distance map
491 //float *distance_map = new float[input_->size ()];
492 const unsigned char * mask_map = input.getData ();
493 float * distance_map = output.getData ();
494 for (std::size_t index = 0; index < width*height; ++index)
495 {
496 if (mask_map[index] == 0)
497 distance_map[index] = 0.0f;
498 else
499 distance_map[index] = static_cast<float> (width + height);
500 }
501
502 // first pass
503 float * previous_row = distance_map;
504 float * current_row = previous_row + width;
505 for (std::size_t ri = 1; ri < height; ++ri)
506 {
507 for (std::size_t ci = 1; ci < width; ++ci)
508 {
509 const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f;
510 const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f;
511 const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f;
512 const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f;
513 const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
514
515 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
516
517 if (min_value < center)
518 current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value;
519 }
520 previous_row = current_row;
521 current_row += width;
522 }
523
524 // second pass
525 float * next_row = distance_map + width * (height - 1);
526 current_row = next_row - width;
527 for (int ri = static_cast<int> (height)-2; ri >= 0; --ri)
528 {
529 for (int ci = static_cast<int> (width)-2; ci >= 0; --ci)
530 {
531 const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f;
532 const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f;
533 const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f;
534 const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f;
535 const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
536
537 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
538
539 if (min_value < center)
540 current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value;
541 }
542 next_row = current_row;
543 current_row -= width;
544 }
545}
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)
virtual ~ColorModality()=default
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.