294computeGaussianKernel (
const std::size_t kernel_size,
const float sigma, std::vector <float> & kernel_values)
297 const int n = int (kernel_size);
298 const int SMALL_GAUSSIAN_SIZE = 7;
299 static const float small_gaussian_tab[][SMALL_GAUSSIAN_SIZE] =
302 {0.25f, 0.5f, 0.25f},
303 {0.0625f, 0.25f, 0.375f, 0.25f, 0.0625f},
304 {0.03125f, 0.109375f, 0.21875f, 0.28125f, 0.21875f, 0.109375f, 0.03125f}
307 const float* fixed_kernel = n % 2 == 1 && n <= SMALL_GAUSSIAN_SIZE && sigma <= 0 ?
308 small_gaussian_tab[n>>1] :
nullptr;
312 kernel_values.resize (n);
313 float* cf = &(kernel_values[0]);
316 double sigmaX = sigma > 0 ? sigma : ((n-1)*0.5 - 1)*0.3 + 0.8;
317 double scale2X = -0.5/(sigmaX*sigmaX);
320 for(
int i = 0; i < n; i++ )
322 double x = i - (n-1)*0.5;
323 double t = fixed_kernel ? double (fixed_kernel[i]) : std::exp (scale2X*x*x);
330 for (
int i = 0; i < n; i++ )
332 cf[i] = float (cf[i]*sum);
343 const std::size_t kernel_size = 7;
344 std::vector<float> kernel_values;
345 computeGaussianKernel (kernel_size, 0.0f, kernel_values);
349 Eigen::ArrayXf gaussian_kernel(kernel_size);
352 gaussian_kernel << kernel_values[0], kernel_values[1], kernel_values[2], kernel_values[3], kernel_values[4], kernel_values[5], kernel_values[6];
356 const std::uint32_t width = input_->width;
357 const std::uint32_t height = input_->height;
359 rgb_input_->
resize (width*height);
360 rgb_input_->
width = width;
361 rgb_input_->
height = height;
362 rgb_input_->
is_dense = input_->is_dense;
363 for (std::size_t row_index = 0; row_index < height; ++row_index)
365 for (std::size_t col_index = 0; col_index < width; ++col_index)
367 (*rgb_input_) (col_index, row_index).r = (*input_) (col_index, row_index).r;
368 (*rgb_input_) (col_index, row_index).g = (*input_) (col_index, row_index).g;
369 (*rgb_input_) (col_index, row_index).b = (*input_) (col_index, row_index).b;
376 convolution.
convolve (*smoothed_input_);
379 computeMaxColorGradientsSobel (smoothed_input_);
382 quantizeColorGradients ();
385 filterQuantizedColorGradients ();
390 spreaded_filtered_quantized_color_gradients_,
411 std::vector<QuantizedMultiModFeature> & features)
const
413 const std::size_t width = mask.
getWidth ();
414 const std::size_t height = mask.
getHeight ();
416 std::list<Candidate> list1;
417 std::list<Candidate> list2;
420 if (feature_selection_method_ == DISTANCE_MAGNITUDE_SCORE)
422 for (std::size_t row_index = 0; row_index < height; ++row_index)
424 for (std::size_t col_index = 0; col_index < width; ++col_index)
426 if (mask (col_index, row_index) != 0)
428 const GradientXY & gradient = color_gradients_ (col_index, row_index);
429 if (gradient.
magnitude > gradient_magnitude_threshold_feature_extraction_
430 && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
434 candidate.
x =
static_cast<int> (col_index);
435 candidate.
y =
static_cast<int> (row_index);
437 list1.push_back (candidate);
445 if (variable_feature_nr_)
447 list2.push_back (*(list1.begin ()));
449 bool feature_selection_finished =
false;
450 while (!feature_selection_finished)
452 float best_score = 0.0f;
453 auto best_iter = list1.end ();
454 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
457 float smallest_distance = std::numeric_limits<float>::max ();
458 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
460 const float dx =
static_cast<float> (iter1->x) -
static_cast<float> (iter2->x);
461 const float dy =
static_cast<float> (iter1->y) -
static_cast<float> (iter2->y);
463 const float distance = dx*dx + dy*dy;
465 if (distance < smallest_distance)
467 smallest_distance = distance;
471 const float score = smallest_distance * iter1->gradient.magnitude;
473 if (score > best_score)
481 float min_min_sqr_distance = std::numeric_limits<float>::max ();
482 float max_min_sqr_distance = 0;
483 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
485 float min_sqr_distance = std::numeric_limits<float>::max ();
486 for (
auto iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
491 const float dx =
static_cast<float> (iter2->x) -
static_cast<float> (iter3->x);
492 const float dy =
static_cast<float> (iter2->y) -
static_cast<float> (iter3->y);
494 const float sqr_distance = dx*dx + dy*dy;
496 if (sqr_distance < min_sqr_distance)
498 min_sqr_distance = sqr_distance;
507 const float dx =
static_cast<float> (iter2->x) -
static_cast<float> (best_iter->x);
508 const float dy =
static_cast<float> (iter2->y) -
static_cast<float> (best_iter->y);
510 const float sqr_distance = dx*dx + dy*dy;
512 if (sqr_distance < min_sqr_distance)
514 min_sqr_distance = sqr_distance;
518 if (min_sqr_distance < min_min_sqr_distance)
519 min_min_sqr_distance = min_sqr_distance;
520 if (min_sqr_distance > max_min_sqr_distance)
521 max_min_sqr_distance = min_sqr_distance;
526 if (best_iter != list1.end ())
532 if (min_min_sqr_distance < 50)
534 feature_selection_finished =
true;
538 list2.push_back (*best_iter);
544 if (list1.size () <= nr_features)
546 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
550 feature.
x = iter1->x;
551 feature.
y = iter1->y;
553 feature.
quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
555 features.push_back (feature);
560 list2.push_back (*(list1.begin ()));
561 while (list2.size () != nr_features)
563 float best_score = 0.0f;
564 auto best_iter = list1.end ();
565 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
568 float smallest_distance = std::numeric_limits<float>::max ();
569 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
571 const float dx =
static_cast<float> (iter1->x) -
static_cast<float> (iter2->x);
572 const float dy =
static_cast<float> (iter1->y) -
static_cast<float> (iter2->y);
574 const float distance = dx*dx + dy*dy;
576 if (distance < smallest_distance)
578 smallest_distance = distance;
582 const float score = smallest_distance * iter1->gradient.magnitude;
584 if (score > best_score)
591 if (best_iter != list1.end ())
593 list2.push_back (*best_iter);
602 else if (feature_selection_method_ == MASK_BORDER_HIGH_GRADIENTS || feature_selection_method_ == MASK_BORDER_EQUALLY)
605 erode (mask, eroded_mask);
609 for (std::size_t row_index = 0; row_index < height; ++row_index)
611 for (std::size_t col_index = 0; col_index < width; ++col_index)
613 if (diff_mask (col_index, row_index) != 0)
615 const GradientXY & gradient = color_gradients_ (col_index, row_index);
616 if ((feature_selection_method_ == MASK_BORDER_EQUALLY || gradient.
magnitude > gradient_magnitude_threshold_feature_extraction_)
617 && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
621 candidate.
x =
static_cast<int> (col_index);
622 candidate.
y =
static_cast<int> (row_index);
624 list1.push_back (candidate);
632 if (list1.size () <= nr_features)
634 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
638 feature.
x = iter1->x;
639 feature.
y = iter1->y;
641 feature.
quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
643 features.push_back (feature);
648 std::size_t distance = list1.size () / nr_features + 1;
649 while (list2.size () != nr_features)
651 const std::size_t sqr_distance = distance*distance;
652 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
654 bool candidate_accepted =
true;
656 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
658 const int dx = iter1->x - iter2->x;
659 const int dy = iter1->y - iter2->y;
660 const unsigned int tmp_distance = dx*dx + dy*dy;
663 if (tmp_distance < sqr_distance)
665 candidate_accepted =
false;
670 if (candidate_accepted)
671 list2.push_back (*iter1);
673 if (list2.size () == nr_features)
680 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
684 feature.
x = iter2->x;
685 feature.
y = iter2->y;
687 feature.
quantized_value = filtered_quantized_color_gradients_ (iter2->x, iter2->y);
689 features.push_back (feature);
697 std::vector<QuantizedMultiModFeature> & features)
const
699 const std::size_t width = mask.
getWidth ();
700 const std::size_t height = mask.
getHeight ();
702 std::list<Candidate> list1;
703 std::list<Candidate> list2;
706 for (std::size_t row_index = 0; row_index < height; ++row_index)
708 for (std::size_t col_index = 0; col_index < width; ++col_index)
710 if (mask (col_index, row_index) != 0)
712 const GradientXY & gradient = color_gradients_ (col_index, row_index);
713 if (gradient.
magnitude > gradient_magnitude_threshold_feature_extraction_
714 && filtered_quantized_color_gradients_ (col_index, row_index) != 0)
718 candidate.
x =
static_cast<int> (col_index);
719 candidate.
y =
static_cast<int> (row_index);
721 list1.push_back (candidate);
729 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
733 feature.
x = iter1->x;
734 feature.
y = iter1->y;
736 feature.
quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y);
738 features.push_back (feature);
748 const int width = cloud->
width;
749 const int height = cloud->
height;
751 color_gradients_.resize (width*height);
752 color_gradients_.width = width;
753 color_gradients_.height = height;
755 const float pi = std::tan (1.0f) * 2;
756 for (
int row_index = 0; row_index < height-2; ++row_index)
758 for (
int col_index = 0; col_index < width-2; ++col_index)
760 const int index0 = row_index*width+col_index;
761 const int index_c = row_index*width+col_index+2;
762 const int index_r = (row_index+2)*width+col_index;
766 const unsigned char r0 = (*cloud)[index0].r;
767 const unsigned char g0 = (*cloud)[index0].g;
768 const unsigned char b0 = (*cloud)[index0].b;
770 const unsigned char r_c = (*cloud)[index_c].r;
771 const unsigned char g_c = (*cloud)[index_c].g;
772 const unsigned char b_c = (*cloud)[index_c].b;
774 const unsigned char r_r = (*cloud)[index_r].r;
775 const unsigned char g_r = (*cloud)[index_r].g;
776 const unsigned char b_r = (*cloud)[index_r].b;
778 const float r_dx =
static_cast<float> (r_c) -
static_cast<float> (r0);
779 const float g_dx =
static_cast<float> (g_c) -
static_cast<float> (g0);
780 const float b_dx =
static_cast<float> (b_c) -
static_cast<float> (b0);
782 const float r_dy =
static_cast<float> (r_r) -
static_cast<float> (r0);
783 const float g_dy =
static_cast<float> (g_r) -
static_cast<float> (g0);
784 const float b_dy =
static_cast<float> (b_r) -
static_cast<float> (b0);
786 const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
787 const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
788 const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
790 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
793 gradient.
magnitude = std::sqrt (sqr_mag_r);
794 gradient.
angle = std::atan2 (r_dy, r_dx) * 180.0f / pi;
795 gradient.
x =
static_cast<float> (col_index);
796 gradient.
y =
static_cast<float> (row_index);
798 color_gradients_ (col_index+1, row_index+1) = gradient;
800 else if (sqr_mag_g > sqr_mag_b)
803 gradient.
magnitude = std::sqrt (sqr_mag_g);
804 gradient.
angle = std::atan2 (g_dy, g_dx) * 180.0f / pi;
805 gradient.
x =
static_cast<float> (col_index);
806 gradient.
y =
static_cast<float> (row_index);
808 color_gradients_ (col_index+1, row_index+1) = gradient;
813 gradient.
magnitude = std::sqrt (sqr_mag_b);
814 gradient.
angle = std::atan2 (b_dy, b_dx) * 180.0f / pi;
815 gradient.
x =
static_cast<float> (col_index);
816 gradient.
y =
static_cast<float> (row_index);
818 color_gradients_ (col_index+1, row_index+1) = gradient;
821 assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 &&
822 color_gradients_ (col_index+1, row_index+1).angle <= 180);
835 const int width = cloud->
width;
836 const int height = cloud->
height;
838 color_gradients_.resize (width*height);
839 color_gradients_.width = width;
840 color_gradients_.height = height;
842 const float pi = tanf (1.0f) * 2.0f;
843 for (
int row_index = 1; row_index < height-1; ++row_index)
845 for (
int col_index = 1; col_index < width-1; ++col_index)
847 const int r7 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index-1)].r);
848 const int g7 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index-1)].g);
849 const int b7 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index-1)].b);
850 const int r8 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index)].r);
851 const int g8 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index)].g);
852 const int b8 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index)].b);
853 const int r9 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index+1)].r);
854 const int g9 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index+1)].g);
855 const int b9 =
static_cast<int> ((*cloud)[(row_index-1)*width + (col_index+1)].b);
856 const int r4 =
static_cast<int> ((*cloud)[(row_index)*width + (col_index-1)].r);
857 const int g4 =
static_cast<int> ((*cloud)[(row_index)*width + (col_index-1)].g);
858 const int b4 =
static_cast<int> ((*cloud)[(row_index)*width + (col_index-1)].b);
859 const int r6 =
static_cast<int> ((*cloud)[(row_index)*width + (col_index+1)].r);
860 const int g6 =
static_cast<int> ((*cloud)[(row_index)*width + (col_index+1)].g);
861 const int b6 =
static_cast<int> ((*cloud)[(row_index)*width + (col_index+1)].b);
862 const int r1 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index-1)].r);
863 const int g1 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index-1)].g);
864 const int b1 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index-1)].b);
865 const int r2 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index)].r);
866 const int g2 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index)].g);
867 const int b2 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index)].b);
868 const int r3 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index+1)].r);
869 const int g3 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index+1)].g);
870 const int b3 =
static_cast<int> ((*cloud)[(row_index+1)*width + (col_index+1)].b);
895 const int r_dx = r9 + 2*r6 + r3 - (r7 + 2*r4 + r1);
896 const int r_dy = r1 + 2*r2 + r3 - (r7 + 2*r8 + r9);
897 const int g_dx = g9 + 2*g6 + g3 - (g7 + 2*g4 + g1);
898 const int g_dy = g1 + 2*g2 + g3 - (g7 + 2*g8 + g9);
899 const int b_dx = b9 + 2*b6 + b3 - (b7 + 2*b4 + b1);
900 const int b_dy = b1 + 2*b2 + b3 - (b7 + 2*b8 + b9);
902 const int sqr_mag_r = r_dx*r_dx + r_dy*r_dy;
903 const int sqr_mag_g = g_dx*g_dx + g_dy*g_dy;
904 const int sqr_mag_b = b_dx*b_dx + b_dy*b_dy;
906 if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b)
909 gradient.
magnitude = std::sqrt (
static_cast<float> (sqr_mag_r));
910 gradient.
angle = std::atan2 (
static_cast<float> (r_dy),
static_cast<float> (r_dx)) * 180.0f / pi;
911 if (gradient.
angle < -180.0f) gradient.
angle += 360.0f;
912 if (gradient.
angle >= 180.0f) gradient.
angle -= 360.0f;
913 gradient.
x =
static_cast<float> (col_index);
914 gradient.
y =
static_cast<float> (row_index);
916 color_gradients_ (col_index, row_index) = gradient;
918 else if (sqr_mag_g > sqr_mag_b)
921 gradient.
magnitude = std::sqrt (
static_cast<float> (sqr_mag_g));
922 gradient.
angle = std::atan2 (
static_cast<float> (g_dy),
static_cast<float> (g_dx)) * 180.0f / pi;
923 if (gradient.
angle < -180.0f) gradient.
angle += 360.0f;
924 if (gradient.
angle >= 180.0f) gradient.
angle -= 360.0f;
925 gradient.
x =
static_cast<float> (col_index);
926 gradient.
y =
static_cast<float> (row_index);
928 color_gradients_ (col_index, row_index) = gradient;
933 gradient.
magnitude = std::sqrt (
static_cast<float> (sqr_mag_b));
934 gradient.
angle = std::atan2 (
static_cast<float> (b_dy),
static_cast<float> (b_dx)) * 180.0f / pi;
935 if (gradient.
angle < -180.0f) gradient.
angle += 360.0f;
936 if (gradient.
angle >= 180.0f) gradient.
angle -= 360.0f;
937 gradient.
x =
static_cast<float> (col_index);
938 gradient.
y =
static_cast<float> (row_index);
940 color_gradients_ (col_index, row_index) = gradient;
943 assert (color_gradients_ (col_index, row_index).angle >= -180 &&
944 color_gradients_ (col_index, row_index).angle <= 180);
1020 const std::size_t width = input_->width;
1021 const std::size_t height = input_->height;
1023 filtered_quantized_color_gradients_.resize (width, height);
1026 for (std::size_t row_index = 1; row_index < height-1; ++row_index)
1028 for (std::size_t col_index = 1; col_index < width-1; ++col_index)
1030 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1033 const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index-1)*width+col_index-1;
1034 assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1035 ++histogram[data_ptr[0]];
1036 ++histogram[data_ptr[1]];
1037 ++histogram[data_ptr[2]];
1040 const unsigned char * data_ptr = quantized_color_gradients_.getData () + row_index*width+col_index-1;
1041 assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1042 ++histogram[data_ptr[0]];
1043 ++histogram[data_ptr[1]];
1044 ++histogram[data_ptr[2]];
1047 const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index+1)*width+col_index-1;
1048 assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9);
1049 ++histogram[data_ptr[0]];
1050 ++histogram[data_ptr[1]];
1051 ++histogram[data_ptr[2]];
1054 unsigned char max_hist_value = 0;
1055 int max_hist_index = -1;
1066 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1067 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1068 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1069 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1070 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1071 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1072 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1073 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1075 if (max_hist_index != -1 && max_hist_value >= 5)
1076 filtered_quantized_color_gradients_ (col_index, row_index) =
static_cast<unsigned char> (0x1 << max_hist_index);
1078 filtered_quantized_color_gradients_ (col_index, row_index) = 0;