diff options
Diffstat (limited to 'extern/libmv/libmv/simple_pipeline/detect.cc')
| -rw-r--r-- | extern/libmv/libmv/simple_pipeline/detect.cc | 272 |
1 files changed, 202 insertions, 70 deletions
diff --git a/extern/libmv/libmv/simple_pipeline/detect.cc b/extern/libmv/libmv/simple_pipeline/detect.cc index 27639e958a0..1ddb23dcd28 100644 --- a/extern/libmv/libmv/simple_pipeline/detect.cc +++ b/extern/libmv/libmv/simple_pipeline/detect.cc @@ -22,96 +22,133 @@ ** ****************************************************************************/ -#include "libmv/simple_pipeline/detect.h" -#include <third_party/fast/fast.h> #include <stdlib.h> #include <memory.h> +#include <queue> +#include <third_party/fast/fast.h> + +#include "libmv/base/scoped_ptr.h" +#include "libmv/image/array_nd.h" +#include "libmv/image/image_converter.h" +#include "libmv/image/convolve.h" +#include "libmv/logging/logging.h" +#include "libmv/simple_pipeline/detect.h" #ifdef __SSE2__ -#include <emmintrin.h> +# include <emmintrin.h> #endif namespace libmv { -typedef unsigned int uint; +namespace { + +class FeatureComparison { + public: + bool operator() (const Feature &left, const Feature &right) const { + return right.score > left.score; + } +}; + +// Filter the features so there are no features closer than +// minimal distance to each other. +// This is a naive implementation with O(n^2) asymptotic. +void FilterFeaturesByDistance(const vector<Feature> &all_features, + int min_distance, + vector<Feature> *detected_features) { + const int min_distance_squared = min_distance * min_distance; + + // Use priority queue to sort the features by their score. + // + // Do this on copy of the input features to prevent possible + // distortion in callee function behavior. + std::priority_queue<Feature, + std::vector<Feature>, + FeatureComparison> priority_features; + + for (int i = 0; i < all_features.size(); i++) { + priority_features.push(all_features.at(i)); + } + + while (!priority_features.empty()) { + bool ok = true; + Feature a = priority_features.top(); + + for (int i = 0; i < detected_features->size(); i++) { + Feature &b = detected_features->at(i); + if (Square(a.x - b.x) + Square(a.y - b.y) < min_distance_squared) { + ok = false; + break; + } + } -static int featurecmp(const void *a_v, const void *b_v) { - Feature *a = (Feature*)a_v; - Feature *b = (Feature*)b_v; + if (ok) { + detected_features->push_back(a); + } - return b->score - a->score; + priority_features.pop(); + } } -std::vector<Feature> DetectFAST(const unsigned char* data, - int width, int height, - int stride, - int min_trackness, - int min_distance) { - std::vector<Feature> features; +void DetectFAST(const FloatImage &grayscale_image, + const DetectOptions &options, + vector<Feature> *detected_features) { + const int min_distance = options.min_distance; + const int min_trackness = options.fast_min_trackness; + const int margin = options.margin; + const int width = grayscale_image.Width() - 2 * margin; + const int height = grayscale_image.Width() - 2 * margin; + const int stride = grayscale_image.Width(); + + // TODO(sergey): Use scoped_array to guard de-allocation of the array. + // Same goes to `scores` and `all` arrays here. + unsigned char *byte_image = FloatImageToUCharArray(grayscale_image); + const int byte_image_offset = margin * stride + margin; + // TODO(MatthiasF): Support targetting a feature count (binary search trackness) int num_features; - xy* all = fast9_detect(data, width, height, - stride, min_trackness, &num_features); + xy *all = fast9_detect(byte_image + byte_image_offset, + width, + height, + stride, + min_trackness, + &num_features); if (num_features == 0) { free(all); - return features; + delete [] byte_image; + return; } - int* scores = fast9_score(data, stride, all, num_features, min_trackness); + int *scores = fast9_score(byte_image + byte_image_offset, + stride, + all, + num_features, + min_trackness); // TODO(MatthiasF): merge with close feature suppression - xy* nonmax = nonmax_suppression(all, scores, num_features, &num_features); + xy *nonmax = nonmax_suppression(all, scores, num_features, &num_features); free(all); + delete [] byte_image; // Remove too close features // TODO(MatthiasF): A resolution independent parameter would be better than // distance e.g. a coefficient going from 0 (no minimal distance) to 1 // (optimal circle packing) // FIXME(MatthiasF): this method will not necessarily give all maximum markers if (num_features) { - Feature *all_features = new Feature[num_features]; - + vector<Feature> all_features; for (int i = 0; i < num_features; ++i) { - Feature a = { (float)nonmax[i].x, (float)nonmax[i].y, (float)scores[i], 0 }; - all_features[i] = a; - } - - qsort((void *)all_features, num_features, sizeof(Feature), featurecmp); - - features.reserve(num_features); - - int prev_score = all_features[0].score; - const int min_distance_squared = min_distance * min_distance; - for (int i = 0; i < num_features; ++i) { - bool ok = true; - Feature a = all_features[i]; - if (a.score>prev_score) - abort(); - prev_score = a.score; - - // compare each feature against filtered set - for (int j = 0; j < features.size(); j++) { - Feature& b = features[j]; - if ((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y) < min_distance_squared) { - // already a nearby feature - ok = false; - break; - } - } - - if (ok) { - // add the new feature - features.push_back(a); - } + Feature new_feature = {(float)nonmax[i].x + margin, + (float)nonmax[i].y + margin, + (float)scores[i], + 0}; + all_features.push_back(new_feature); } - - delete [] all_features; + FilterFeaturesByDistance(all_features, min_distance, detected_features); } free(scores); free(nonmax); - return features; } #ifdef __SSE2__ -static uint SAD(const ubyte* imageA, const ubyte* imageB, - int strideA, int strideB) { +static unsigned int SAD(const ubyte* imageA, const ubyte* imageB, + int strideA, int strideB) { __m128i a = _mm_setzero_si128(); for (int i = 0; i < 16; i++) { a = _mm_adds_epu16(a, @@ -121,9 +158,9 @@ static uint SAD(const ubyte* imageA, const ubyte* imageB, return _mm_extract_epi16(a, 0) + _mm_extract_epi16(a, 4); } #else -static uint SAD(const ubyte* imageA, const ubyte* imageB, - int strideA, int strideB) { - uint sad = 0; +static unsigned int SAD(const ubyte* imageA, const ubyte* imageB, + int strideA, int strideB) { + unsigned int sad = 0; for (int i = 0; i < 16; i++) { for (int j = 0; j < 16; j++) { sad += abs((int)imageA[i*strideA+j] - imageB[i*strideB+j]); @@ -133,11 +170,20 @@ static uint SAD(const ubyte* imageA, const ubyte* imageB, } #endif -void DetectMORAVEC(const ubyte* image, - int stride, int width, int height, - Feature* detected, int* count, - int distance, - ubyte* pattern) { +void DetectMORAVEC(const FloatImage &grayscale_image, + const DetectOptions &options, + vector<Feature> *detected_features) { + const int distance = options.min_distance; + const int margin = options.margin; + const unsigned char *pattern = options.moravec_pattern; + const int count = options.moravec_max_count; + const int width = grayscale_image.Width() - 2 * margin; + const int height = grayscale_image.Width() - 2 * margin; + const int stride = grayscale_image.Width(); + + // TODO(sergey): Use scoped_array to guard de-allocation of the array. + unsigned char *byte_image = FloatImageToUCharArray(grayscale_image); + unsigned short histogram[256]; memset(histogram, 0, sizeof(histogram)); ubyte* scores = new ubyte[width*height]; @@ -145,7 +191,7 @@ void DetectMORAVEC(const ubyte* image, const int r = 1; // radius for self similarity comparison for (int y = distance; y < height-distance; y++) { for (int x = distance; x < width-distance; x++) { - const ubyte* s = &image[y*stride+x]; + const ubyte* s = &byte_image[y*stride+x]; int score = // low self-similarity with overlapping patterns // OPTI: load pattern once SAD(s, s-r*stride-r, stride, stride)+SAD(s, s-r*stride, stride, stride)+SAD(s, s-r*stride+r, stride, stride)+ @@ -182,19 +228,105 @@ void DetectMORAVEC(const ubyte* image, int min = 255, total = 0; for (; min > 0; min--) { int h = histogram[min]; - if (total+h > *count) break; + if (total+h > count) break; total += h; } - int i = 0; for (int y = 16; y < height-16; y++) { for (int x = 16; x < width-16; x++) { int s = scores[y*width+x]; Feature f = { (float)x+8.0f, (float)y+8.0f, (float)s, 16 }; - if (s > min) detected[i++] = f; + if (s > min) { + detected_features->push_back(f); + } } } - *count = i; delete[] scores; + delete[] byte_image; +} + +void DetectHarris(const FloatImage &grayscale_image, + const DetectOptions &options, + vector<Feature> *detected_features) { + const double alpha = 0.06; + const double sigma = 0.9; + + const int min_distance = options.min_distance; + const int margin = options.margin; + const double threshold = options.harris_threshold; + + FloatImage gradient_x, gradient_y; + ImageDerivatives(grayscale_image, sigma, &gradient_x, &gradient_y); + + FloatImage gradient_xx, gradient_yy, gradient_xy; + MultiplyElements(gradient_x, gradient_x, &gradient_xx); + MultiplyElements(gradient_y, gradient_y, &gradient_yy); + MultiplyElements(gradient_x, gradient_y, &gradient_xy); + + FloatImage gradient_xx_blurred, + gradient_yy_blurred, + gradient_xy_blurred; + ConvolveGaussian(gradient_xx, sigma, &gradient_xx_blurred); + ConvolveGaussian(gradient_yy, sigma, &gradient_yy_blurred); + ConvolveGaussian(gradient_xy, sigma, &gradient_xy_blurred); + + vector<Feature> all_features; + for (int y = margin; y < gradient_xx_blurred.Height() - margin; ++y) { + for (int x = margin; x < gradient_xx_blurred.Width() - margin; ++x) { + // Construct matrix + // + // A = [ Ix^2 Ix*Iy ] + // [ Ix*Iy Iy^2 ] + Mat2 A; + A(0, 0) = gradient_xx_blurred(y, x); + A(1, 1) = gradient_yy_blurred(y, x); + A(0, 1) = A(1, 0) = gradient_xy_blurred(y, x); + + double detA = A.determinant(); + double traceA = A.trace(); + double harris_function = detA - alpha * traceA * traceA; + if (harris_function > threshold) { + Feature new_feature = {(float)x, (float)y, (float)harris_function, 0.0f}; + all_features.push_back(new_feature); + } + } + } + + FilterFeaturesByDistance(all_features, min_distance, detected_features); +} + +} // namespace + +DetectOptions::DetectOptions() + : type(DetectOptions::HARRIS), + margin(0), + min_distance(120), + fast_min_trackness(128), + moravec_max_count(0), + moravec_pattern(NULL), + harris_threshold(0.0) {} + +void Detect(const FloatImage &image, + const DetectOptions &options, + vector<Feature> *detected_features) { + // Currently all the detectors requires image to be grayscale. + // Do it here to avoid code duplication. + FloatImage grayscale_image; + if (image.Depth() != 1) { + Rgb2Gray(image, &grayscale_image); + } else { + // TODO(sergey): Find a way to avoid such image duplication/ + grayscale_image = image; + } + + if (options.type == DetectOptions::FAST) { + DetectFAST(grayscale_image, options, detected_features); + } else if (options.type == DetectOptions::MORAVEC) { + DetectMORAVEC(grayscale_image, options, detected_features); + } else if (options.type == DetectOptions::HARRIS) { + DetectHarris(grayscale_image, options, detected_features); + } else { + LOG(FATAL) << "Unknown detector has been passed to featur detection"; + } } } // namespace libmv |