Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_FEATURES_COLOR_GRADIENT_DOT_MODALITY
00039 #define PCL_FEATURES_COLOR_GRADIENT_DOT_MODALITY
00040
00041 #include <pcl/pcl_base.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044
00045 #include <pcl/recognition/dot_modality.h>
00046 #include <pcl/recognition/quantized_map.h>
00047
00048
00049 namespace pcl
00050 {
00051
00055 struct EIGEN_ALIGN16 PointRGB
00056 {
00057 union
00058 {
00059 union
00060 {
00061 struct
00062 {
00063 uint8_t b;
00064 uint8_t g;
00065 uint8_t r;
00066 uint8_t _unused;
00067 };
00068 float rgb;
00069 };
00070 uint32_t rgba;
00071 };
00072
00073 inline PointRGB ()
00074 {}
00075
00076 inline PointRGB (const uint8_t b, const uint8_t g, const uint8_t r)
00077 : b (b), g (g), r (r), _unused (0)
00078 {}
00079
00080 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00081 };
00082
00083
00087 struct EIGEN_ALIGN16 GradientXY
00088 {
00089 union
00090 {
00091 struct
00092 {
00093 float x;
00094 float y;
00095 float angle;
00096 float magnitude;
00097 };
00098 float data[4];
00099 };
00100 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00101
00102 inline bool operator< (const GradientXY & rhs)
00103 {
00104 return (magnitude > rhs.magnitude);
00105 }
00106 };
00107 inline std::ostream & operator << (std::ostream & os, const GradientXY & p)
00108 {
00109 os << "(" << p.x << "," << p.y << " - " << p.magnitude << ")";
00110 return (os);
00111 }
00112
00113
00114
00115 template <typename PointInT>
00116 class ColorGradientDOTModality
00117 : public DOTModality, public PCLBase<PointInT>
00118 {
00119 protected:
00120 using PCLBase<PointInT>::input_;
00121
00122 struct Candidate
00123 {
00124 GradientXY gradient;
00125
00126 int x;
00127 int y;
00128
00129 bool operator< (const Candidate & rhs)
00130 {
00131 return (gradient.magnitude > rhs.gradient.magnitude);
00132 }
00133 };
00134
00135 public:
00136 typedef typename pcl::PointCloud<PointInT> PointCloudIn;
00137
00138 ColorGradientDOTModality (size_t bin_size);
00139
00140 virtual ~ColorGradientDOTModality ();
00141
00142 inline void
00143 setGradientMagnitudeThreshold (const float threshold)
00144 {
00145 gradient_magnitude_threshold_ = threshold;
00146 }
00147
00148
00149
00150
00151
00152
00153
00154 inline QuantizedMap &
00155 getDominantQuantizedMap ()
00156 {
00157 return (dominant_quantized_color_gradients_);
00158 }
00159
00160 QuantizedMap
00161 computeInvariantQuantizedMap (const MaskMap & mask,
00162 const RegionXY & region);
00163
00167 virtual void
00168 setInputCloud (const typename PointCloudIn::ConstPtr & cloud)
00169 {
00170 input_ = cloud;
00171
00172 }
00173
00174 virtual void
00175 processInputData ();
00176
00177 protected:
00178
00179 void
00180 computeMaxColorGradients ();
00181
00182 void
00183 computeDominantQuantizedGradients ();
00184
00185
00186
00187
00188 private:
00189 size_t bin_size_;
00190
00191 float gradient_magnitude_threshold_;
00192 pcl::PointCloud<pcl::GradientXY> color_gradients_;
00193
00194 pcl::QuantizedMap dominant_quantized_color_gradients_;
00195
00196
00197 };
00198
00199 }
00200
00202 template <typename PointInT>
00203 pcl::ColorGradientDOTModality<PointInT>::
00204 ColorGradientDOTModality (const size_t bin_size)
00205 : bin_size_ (bin_size), gradient_magnitude_threshold_ (80.0f), color_gradients_ (), dominant_quantized_color_gradients_ ()
00206 {
00207 }
00208
00210 template <typename PointInT>
00211 pcl::ColorGradientDOTModality<PointInT>::
00212 ~ColorGradientDOTModality ()
00213 {
00214 }
00215
00217 template <typename PointInT>
00218 void
00219 pcl::ColorGradientDOTModality<PointInT>::
00220 processInputData ()
00221 {
00222
00223 computeMaxColorGradients ();
00224
00225
00226 computeDominantQuantizedGradients ();
00227
00228
00229
00230 }
00231
00233 template <typename PointInT>
00234 void
00235 pcl::ColorGradientDOTModality<PointInT>::
00236 computeMaxColorGradients ()
00237 {
00238 const int width = input_->width;
00239 const int height = input_->height;
00240
00241 color_gradients_.points.resize (width*height);
00242 color_gradients_.width = width;
00243 color_gradients_.height = height;
00244
00245 const float pi = tan(1.0f)*4;
00246 for (int row_index = 0; row_index < height-2; ++row_index)
00247 {
00248 for (int col_index = 0; col_index < width-2; ++col_index)
00249 {
00250 const int index0 = row_index*width+col_index;
00251 const int index_c = row_index*width+col_index+2;
00252 const int index_r = (row_index+2)*width+col_index;
00253
00254
00255
00256 const unsigned char r0 = input_->points[index0].r;
00257 const unsigned char g0 = input_->points[index0].g;
00258 const unsigned char b0 = input_->points[index0].b;
00259
00260 const unsigned char r_c = input_->points[index_c].r;
00261 const unsigned char g_c = input_->points[index_c].g;
00262 const unsigned char b_c = input_->points[index_c].b;
00263
00264 const unsigned char r_r = input_->points[index_r].r;
00265 const unsigned char g_r = input_->points[index_r].g;
00266 const unsigned char b_r = input_->points[index_r].b;
00267
00268 const float r_dx = static_cast<float> (r_c) - static_cast<float> (r0);
00269 const float g_dx = static_cast<float> (g_c) - static_cast<float> (g0);
00270 const float b_dx = static_cast<float> (b_c) - static_cast<float> (b0);
00271
00272 const float r_dy = static_cast<float> (r_r) - static_cast<float> (r0);
00273 const float g_dy = static_cast<float> (g_r) - static_cast<float> (g0);
00274 const float b_dy = static_cast<float> (b_r) - static_cast<float> (b0);
00275
00276 const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
00277 const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
00278 const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
00279
00280 GradientXY gradient;
00281 gradient.x = col_index;
00282 gradient.y = row_index;
00283 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
00284 {
00285 gradient.magnitude = sqrt (sqr_mag_r);
00286 gradient.angle = atan2 (r_dy, r_dx) * 180.0f / pi;
00287 }
00288 else if (sqr_mag_g > sqr_mag_b)
00289 {
00290
00291 gradient.magnitude = sqrt (sqr_mag_g);
00292 gradient.angle = atan2 (g_dy, g_dx) * 180.0f / pi;
00293
00294
00295
00296
00297 }
00298 else
00299 {
00300
00301 gradient.magnitude = sqrt (sqr_mag_b);
00302 gradient.angle = atan2 (b_dy, b_dx) * 180.0f / pi;
00303
00304
00305
00306
00307 }
00308
00309 assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
00310 color_gradients_ (col_index+1, row_index+1).angle <= 180);
00311
00312 color_gradients_ (col_index+1, row_index+1) = gradient;
00313 }
00314 }
00315
00316 return;
00317 }
00318
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00482 template <typename PointInT>
00483 void
00484 pcl::ColorGradientDOTModality<PointInT>::
00485 computeDominantQuantizedGradients ()
00486 {
00487 const size_t input_width = input_->width;
00488 const size_t input_height = input_->height;
00489
00490 const size_t output_width = input_width / bin_size_;
00491 const size_t output_height = input_height / bin_size_;
00492
00493 dominant_quantized_color_gradients_.resize (output_width, output_height);
00494
00495
00496
00497
00498 const size_t num_gradient_bins = 7;
00499 const size_t max_num_of_gradients = 1;
00500
00501 const float divisor = 180.0f / (num_gradient_bins - 1.0f);
00502
00503 float global_max_gradient = 0.0f;
00504 float local_max_gradient = 0.0f;
00505
00506 unsigned char * peak_pointer = dominant_quantized_color_gradients_.getData ();
00507 memset (peak_pointer, 0, output_width*output_height);
00508
00509
00510 for (size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index)
00511 {
00512 for (size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index)
00513 {
00514 const size_t x_position = col_bin_index * bin_size_;
00515 const size_t y_position = row_bin_index * bin_size_;
00516
00517
00518
00519
00520
00521
00522
00523 {
00524 float max_gradient;
00525 size_t max_gradient_pos_x;
00526 size_t max_gradient_pos_y;
00527
00528
00529 {
00530 max_gradient = 0.0f;
00531 for (size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
00532 {
00533 for (size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
00534 {
00535 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
00536
00537 if (magnitude > max_gradient)
00538 {
00539 max_gradient = magnitude;
00540 max_gradient_pos_x = col_sub_index;
00541 max_gradient_pos_y = row_sub_index;
00542 }
00543 }
00544 }
00545 }
00546
00547 if (max_gradient >= gradient_magnitude_threshold_)
00548 {
00549 const size_t angle = static_cast<size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f);
00550 const size_t bin_index = static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
00551
00552 *peak_pointer |= 1 << bin_index;
00553 }
00554
00555
00556
00557
00558
00559
00560
00561
00562 }
00563
00565
00566
00567
00568
00569
00570
00571 if (*peak_pointer == 0)
00572 {
00573 *peak_pointer |= 1 << 7;
00574 }
00575
00576
00577
00578
00579
00580
00581
00582 ++peak_pointer;
00583
00584
00585 }
00586
00587
00588
00589 }
00590 }
00591
00593 template <typename PointInT>
00594 pcl::QuantizedMap
00595 pcl::ColorGradientDOTModality<PointInT>::
00596 computeInvariantQuantizedMap (const MaskMap & mask,
00597 const RegionXY & region)
00598 {
00599 const size_t input_width = input_->width;
00600 const size_t input_height = input_->height;
00601
00602 const size_t output_width = input_width / bin_size_;
00603 const size_t output_height = input_height / bin_size_;
00604
00605 const size_t sub_start_x = region.x / bin_size_;
00606 const size_t sub_start_y = region.y / bin_size_;
00607 const size_t sub_width = region.width / bin_size_;
00608 const size_t sub_height = region.height / bin_size_;
00609
00610 QuantizedMap map;
00611 map.resize (sub_width, sub_height);
00612
00613
00614
00615
00616 const size_t num_gradient_bins = 7;
00617 const size_t max_num_of_gradients = 7;
00618
00619 const float divisor = 180.0f / (num_gradient_bins - 1.0f);
00620
00621 float global_max_gradient = 0.0f;
00622 float local_max_gradient = 0.0f;
00623
00624 unsigned char * peak_pointer = map.getData ();
00625
00626
00627 for (size_t row_bin_index = 0; row_bin_index < sub_height; ++row_bin_index)
00628 {
00629 for (size_t col_bin_index = 0; col_bin_index < sub_width; ++col_bin_index)
00630 {
00631 std::vector<size_t> x_coordinates;
00632 std::vector<size_t> y_coordinates;
00633 std::vector<float> values;
00634
00635 for (int row_pixel_index = -static_cast<int> (bin_size_)/2;
00636 row_pixel_index <= static_cast<int> (bin_size_)/2;
00637 row_pixel_index += static_cast<int> (bin_size_)/2)
00638 {
00639 const size_t y_position = row_pixel_index + (sub_start_y + row_bin_index)*bin_size_;
00640
00641 if (y_position < 0 || y_position >= input_height)
00642 continue;
00643
00644 for (int col_pixel_index = -static_cast<int> (bin_size_)/2;
00645 col_pixel_index <= static_cast<int> (bin_size_)/2;
00646 col_pixel_index += static_cast<int> (bin_size_)/2)
00647 {
00648 const size_t x_position = col_pixel_index + (sub_start_x + col_bin_index)*bin_size_;
00649 size_t counter = 0;
00650
00651 if (x_position < 0 || x_position >= input_width)
00652 continue;
00653
00654
00655 {
00656 local_max_gradient = 0.0f;
00657 for (size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
00658 {
00659 for (size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
00660 {
00661 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
00662
00663 if (magnitude > local_max_gradient)
00664 local_max_gradient = magnitude;
00665 }
00666 }
00667 }
00668
00669
00670
00671 if (local_max_gradient > global_max_gradient)
00672 {
00673 global_max_gradient = local_max_gradient;
00674 }
00675
00676
00677 while (true)
00678 {
00679 float max_gradient;
00680 size_t max_gradient_pos_x;
00681 size_t max_gradient_pos_y;
00682
00683
00684 {
00685 max_gradient = 0.0f;
00686 for (size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index)
00687 {
00688 for (size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index)
00689 {
00690 const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude;
00691
00692 if (magnitude > max_gradient)
00693 {
00694 max_gradient = magnitude;
00695 max_gradient_pos_x = col_sub_index;
00696 max_gradient_pos_y = row_sub_index;
00697 }
00698 }
00699 }
00700 }
00701
00702
00703 if (local_max_gradient < gradient_magnitude_threshold_)
00704 {
00705
00706 break;
00707 }
00708
00709
00710 if (
00711 counter >= max_num_of_gradients)
00712 {
00713 break;
00714 }
00715
00716 ++counter;
00717
00718 const size_t angle = static_cast<size_t> (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f);
00719 const size_t bin_index = static_cast<size_t> ((angle >= 180 ? angle-180 : angle)/divisor);
00720
00721 *peak_pointer |= 1 << bin_index;
00722
00723 x_coordinates.push_back (max_gradient_pos_x + x_position);
00724 y_coordinates.push_back (max_gradient_pos_y + y_position);
00725 values.push_back (max_gradient);
00726
00727 color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f;
00728 }
00729
00730
00731 for (size_t value_index = 0; value_index < values.size (); ++value_index)
00732 {
00733 color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index];
00734 }
00735
00736 x_coordinates.clear ();
00737 y_coordinates.clear ();
00738 values.clear ();
00739 }
00740 }
00741
00742 if (*peak_pointer == 0)
00743 {
00744 *peak_pointer |= 1 << 7;
00745 }
00746
00747
00748
00749
00750
00751
00752
00753 ++peak_pointer;
00754
00755
00756 }
00757
00758
00759
00760 }
00761
00762 return map;
00763 }
00764
00765 #endif