detect.cc

Go to the documentation of this file.

/****************************************************************************
**
** Copyright (c) 2011 libmv authors.
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to
** deal in the Software without restriction, including without limitation the
** rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of the Software, and to permit persons to whom the Software is
** furnished to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in
** all copies or substantial portions of the Software.
**
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
** FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
** IN THE SOFTWARE.
**
****************************************************************************/
 
#include <memory.h>
#include <stdlib.h>
#include <queue>
 
#include "libmv/base/scoped_ptr.h"
#include "libmv/image/array_nd.h"
#include "libmv/image/convolve.h"
#include "libmv/image/image_converter.h"
#include "libmv/logging/logging.h"
#include "libmv/simple_pipeline/detect.h"
 
#ifndef LIBMV_NO_FAST_DETECTOR
#  include <third_party/fast/fast.h>
#endif
 
#ifdef __SSE2__
#  include <emmintrin.h>
#endif
 
namespace libmv {
 
namespace {
 
// Default value for FAST minimal trackness in the DetectOptions structure.
// TODO(sergey): Think of a better default value here.
int kDefaultFastMinTrackness = 128;
 
// Default value for Harris threshold in the DetectOptions structure.
// TODO(sergey): Think of a better default value here.
double kDefaultHarrisThreshold = 1e-5;
 
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;
      }
    }
 
    if (ok) {
      detected_features->push_back(a);
    }
 
    priority_features.pop();
  }
}
 
void DetectFAST(const FloatImage& grayscale_image,
                const DetectOptions& options,
                vector<Feature>* detected_features) {
#ifndef LIBMV_NO_FAST_DETECTOR
  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();
 
  scoped_array<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(byte_image.get() + byte_image_offset,
                         width,
                         height,
                         stride,
                         min_trackness,
                         &num_features);
  if (num_features == 0) {
    free(all);
    return;
  }
  int* scores = fast9_score(byte_image.get() + 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);
  free(all);
  // 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) {
    vector<Feature> all_features;
    for (int i = 0; i < num_features; ++i) {
      all_features.push_back(Feature((float)nonmax[i].x + margin,
                                     (float)nonmax[i].y + margin,
                                     (float)scores[i],
                                     9.0));
    }
    FilterFeaturesByDistance(all_features, min_distance, detected_features);
  }
  free(scores);
  free(nonmax);
#else
  (void)grayscale_image;    // Ignored.
  (void)options;            // Ignored.
  (void)detected_features;  // Ignored.
  LOG(FATAL) << "FAST detector is disabled in this build.";
#endif
}
 
#ifdef __SSE2__
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,
        _mm_sad_epu8(_mm_loadu_si128((__m128i*)(imageA + i * strideA)),
                     _mm_loadu_si128((__m128i*)(imageB + i * strideB))));
  }
  return _mm_extract_epi16(a, 0) + _mm_extract_epi16(a, 4);
}
#else
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]);
    }
  }
  return sad;
}
#endif
 
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();
 
  scoped_array<unsigned char> byte_image(
      FloatImageToUCharArray(grayscale_image));
 
  unsigned short histogram[256];
  memset(histogram, 0, sizeof(histogram));
  scoped_array<ubyte> scores(new ubyte[width * height]);
  memset(scores.get(), 0, width * height);
  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 = &byte_image[y * stride + x];
      // low self-similarity with overlapping patterns
      // OPTI: load pattern once
      // clang-format off
      int score =  
          SAD(s, s-r*stride-r, stride, stride)+SAD(s, s-r*stride, stride, stride)+SAD(s, s-r*stride+r, stride, stride)+
          SAD(s, s         -r, stride, stride)+                                   SAD(s, s         +r, stride, stride)+
          SAD(s, s+r*stride-r, stride, stride)+SAD(s, s+r*stride, stride, stride)+SAD(s, s+r*stride+r, stride, stride);
      // clang-format on
 
      score /= 256;   // normalize
      if (pattern) {  // find only features similar to pattern
        score -= SAD(s, pattern, stride, 16);
      }
      if (score <= 16) {
        continue;  // filter very self-similar features
      }
      score -= 16;  // translate to score/histogram values
      if (score > 255) {
        score = 255;  // clip
      }
      ubyte* c = &scores[y * width + x];
      for (int i = -distance; i < 0; i++) {
        for (int j = -distance; j < distance; j++) {
          int s = c[i * width + j];
          if (s == 0) {
            continue;
          }
          if (s >= score) {
            goto nonmax;
          }
          c[i * width + j] = 0;
          histogram[s]--;
        }
      }
      for (int i = 0, j = -distance; j < 0; j++) {
        int s = c[i * width + j];
        if (s == 0) {
          continue;
        }
        if (s >= score) {
          goto nonmax;
        }
        c[i * width + j] = 0;
        histogram[s]--;
      }
      c[0] = score, histogram[score]++;
    nonmax: {}  // Do nothing.
    }
  }
  int min = 255, total = 0;
  for (; min > 0; min--) {
    int h = histogram[min];
    if (total + h > count) {
      break;
    }
    total += h;
  }
  for (int y = 16; y < height - 16; y++) {
    for (int x = 16; x < width - 16; x++) {
      int s = scores[y * width + x];
      if (s > min) {
        // Currently SAD works with the patterns of 16x16 pixels.
        //
        // Score calculation above uses top left corner of the
        // patch as the origin, here we need to convert this value
        // to a pattrn center by adding 8 pixels.
        detected_features->push_back(
            Feature((float)x + 8.0f, (float)y + 8.0f, (float)s, 16.0f));
      }
    }
  }
}
 
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) {
        all_features.push_back(
            Feature((float)x, (float)y, (float)harris_function, 5.0f));
      }
    }
  }
 
  FilterFeaturesByDistance(all_features, min_distance, detected_features);
}
 
}  // namespace
 
DetectOptions::DetectOptions()
    : type(DetectOptions::HARRIS),
      margin(0),
      min_distance(120),
      fast_min_trackness(kDefaultFastMinTrackness),
      moravec_max_count(0),
      moravec_pattern(NULL),
      harris_threshold(kDefaultHarrisThreshold) {
}
 
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";
  }
}
 
std::ostream& operator<<(std::ostream& os, const Feature& feature) {
  os << "x: " << feature.x << ", y: " << feature.y;
  os << ", score: " << feature.score;
  os << ", size: " << feature.size;
  return os;
}
 
}  // namespace libmv