Camera tracking integration

===========================

Added option to choose which method to use for feature detector: FAST or Moravec

Moravec was supposed to be more "stable", but it collected worse feedback.
before returning back to FAST detector added option to easier toggle between
different detector to compare it's result and find out if we can leave only FAST.

6 files changed, 285 insertions, 25 deletions

diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp
index 8bf27ac0656..863e16515df 100644
--- a/extern/libmv/libmv-capi.cpp
+++ b/extern/libmv/libmv-capi.cpp
@@ -502,11 +502,13 @@ void libmv_destroyReconstruction(libmv_Reconstruction *libmv_reconstruction)
 
 /* ************ feature detector ************ */
 
-struct libmv_Features *libmv_detectFeatures(unsigned char *data, int width, int height, int stride,
-			int margin, int count, int min_distance)
+struct libmv_Features *libmv_detectFeaturesFAST(unsigned char *data, int width, int height, int stride,
+			int margin, int min_trackness, int min_distance)
 {
-	libmv::Feature *features = new libmv::Feature[count];
-	libmv_Features *libmv_features = new libmv_Features;
+	libmv::Feature *features = NULL;
+	std::vector<libmv::Feature> v;
+	libmv_Features *libmv_features = new libmv_Features();
+	int i= 0, count;
 
 	if(margin) {
 		data += margin*stride+margin;
@@ -514,13 +516,47 @@ struct libmv_Features *libmv_detectFeatures(unsigned char *data, int width, int
 		height -= 2*margin;
 	}
 
-	libmv::Detect(data, stride, width, height, features, &count, min_distance, NULL);
+	v = libmv::DetectFAST(data, width, height, stride, min_trackness, min_distance);
+
+	count = v.size();
+
+	if(count) {
+		features= new libmv::Feature[count];
+
+		for(std::vector<libmv::Feature>::iterator it = v.begin(); it != v.end(); it++) {
+			features[i++]= *it;
+		}
+	}
 
-	libmv_features->count= count;
-	libmv_features->margin= margin;
-	libmv_features->features= features;
+	libmv_features->features = features;
+	libmv_features->count = count;
+	libmv_features->margin = margin;
 
-	return libmv_features ;
+	return (libmv_Features *)libmv_features;
+}
+
+struct libmv_Features *libmv_detectFeaturesMORAVEC(unsigned char *data, int width, int height, int stride,
+			int margin, int count, int min_distance)
+{
+	libmv::Feature *features = NULL;
+	libmv_Features *libmv_features = new libmv_Features;
+
+	if(count) {
+		if(margin) {
+			data += margin*stride+margin;
+			width -= 2*margin;
+			height -= 2*margin;
+		}
+
+		features = new libmv::Feature[count];
+		libmv::DetectMORAVEC(data, stride, width, height, features, &count, min_distance, NULL);
+	}
+
+	libmv_features->count = count;
+	libmv_features->margin = margin;
+	libmv_features->features = features;
+
+	return libmv_features;
 }
 
 int libmv_countFeatures(struct libmv_Features *libmv_features)
@@ -530,7 +566,7 @@ int libmv_countFeatures(struct libmv_Features *libmv_features)
 
 void libmv_getFeature(struct libmv_Features *libmv_features, int number, double *x, double *y, double *score, double *size)
 {
-	libmv::Feature feature = libmv_features->features[number];
+	libmv::Feature feature= libmv_features->features[number];
 
 	*x = feature.x + libmv_features->margin;
 	*y = feature.y + libmv_features->margin;
@@ -540,7 +576,10 @@ void libmv_getFeature(struct libmv_Features *libmv_features, int number, double
 
 void libmv_destroyFeatures(struct libmv_Features *libmv_features)
 {
-	delete (libmv::Feature *)libmv_features;
+	if(libmv_features->features)
+		delete [] libmv_features->features;
+
+	delete libmv_features;
 }
 
 /* ************ camera intrinsics ************ */
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index 8f2a57748b2..d77e9ac62fa 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -72,7 +72,9 @@ double libmv_reprojectionError(struct libmv_Reconstruction *libmv_reconstruction
 void libmv_destroyReconstruction(struct libmv_Reconstruction *libmv_reconstruction);
 
 /* feature detector */
-struct libmv_Features *libmv_detectFeatures(unsigned char *data, int width, int height, int stride,
+struct libmv_Features *libmv_detectFeaturesFAST(unsigned char *data, int width, int height, int stride,
+			int margin, int min_trackness, int min_distance);
+struct libmv_Features *libmv_detectFeaturesMORAVEC(unsigned char *data, int width, int height, int stride,
 			int margin, int count, int min_distance);
 int libmv_countFeatures(struct libmv_Features *libmv_features);
 void libmv_getFeature(struct libmv_Features *libmv_features, int number, double *x, double *y, double *score, double *size);
diff --git a/extern/libmv/libmv/simple_pipeline/detect.cc b/extern/libmv/libmv/simple_pipeline/detect.cc
index 6fc0cdd120a..b316f427649 100644
--- a/extern/libmv/libmv/simple_pipeline/detect.cc
+++ b/extern/libmv/libmv/simple_pipeline/detect.cc
@@ -23,15 +23,59 @@
 ****************************************************************************/
 
 #include "libmv/simple_pipeline/detect.h"
+#include <third_party/fast/fast.h>
 #include <stdlib.h>
-#include <string.h>
+#include <memory.h>
+
+#ifdef __SSE2__
+#include <emmintrin.h>
+#endif
 
 namespace libmv {
 
 typedef unsigned int uint;
 
+std::vector<Feature> DetectFAST(const unsigned char* data, int width, int height, int stride,
+                           int min_trackness, int min_distance) {
+  std::vector<Feature> features;
+  // 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);
+  if(num_features == 0) {
+    free(all);
+    return features;
+  }
+  int* scores = fast9_score(data, stride, all, num_features, min_trackness);
+  // TODO: 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) features.reserve(num_features);
+  for(int i = 0; i < num_features; ++i) {
+    xy xy = nonmax[i];
+    Feature a = { xy.x, xy.y, scores[i], 7 };
+    // 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*min_distance ) {
+        // already a nearby feature
+        goto skip;
+      }
+    }
+    // otherwise add the new feature
+    features.push_back(a);
+    skip: ;
+  }
+  free(scores);
+  free(nonmax);
+  return features;
+}
+
 #ifdef __SSE2__
-#include <emmintrin.h>
 static uint SAD(const ubyte* imageA, const ubyte* imageB, int strideA, int strideB) {
   __m128i a = _mm_setzero_si128();
   for(int i = 0; i < 16; i++) {
@@ -52,7 +96,7 @@ static uint SAD(const ubyte* imageA, const ubyte* imageB, int strideA, int strid
 }
 #endif
 
-void Detect(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance, ubyte* pattern) {
+void DetectMORAVEC(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance, ubyte* pattern) {
   unsigned short histogram[256];
   memset(histogram,0,sizeof(histogram));
   ubyte* scores = new ubyte[width*height];
diff --git a/extern/libmv/libmv/simple_pipeline/detect.h b/extern/libmv/libmv/simple_pipeline/detect.h
index 23b239b81d6..bbe7aed784c 100644
--- a/extern/libmv/libmv/simple_pipeline/detect.h
+++ b/extern/libmv/libmv/simple_pipeline/detect.h
@@ -25,27 +25,52 @@
 #ifndef LIBMV_SIMPLE_PIPELINE_DETECT_H_
 #define LIBMV_SIMPLE_PIPELINE_DETECT_H_
 
-#ifdef __cplusplus
+#include <vector>
+
 namespace libmv {
-#endif
 
 typedef unsigned char ubyte;
 
 /*!
-    \a Feature is the 2D location of a detected feature in an image.
+    A Feature is the 2D location of a detected feature in an image.
 
-    \a x, \a y is the position of the center in pixels (from image top-left).
-    \a score is an estimate of how well the pattern will be tracked.
-    \a size can be used as an initial size to track the pattern.
+    \a x, \a y is the position of the feature in pixels from the top left corner.
+    \a score is an estimate of how well the feature will be tracked.
+    \a size can be used as an initial pattern size to track the feature.
 
     \sa Detect
 */
 struct Feature {
+  /// Position in pixels (from top-left corner)
+  /// \note libmv might eventually support subpixel precision.
   float x, y;
+  /// Trackness of the feature
   float score;
+  /// Size of the feature in pixels
   float size;
 };
- //radius for non maximal suppression
+
+/*!
+    Detect features in an image.
+
+    You need to input a single channel 8-bit image using pointer to image \a data,
+    \a width, \a height and \a stride (i.e bytes per line).
+
+    You can tweak the count of detected features using \a min_trackness, which is
+    the minimum score to add a feature, and \a min_distance which is the minimal
+    distance accepted between two featuress.
+
+    \note You can binary search over \a min_trackness to get a given feature count.
+
+    \note a way to get an uniform distribution of a given feature count is:
+          \a min_distance = \a width * \a height / desired_feature_count ^ 2
+
+    \return All detected feartures matching given parameters
+*/
+std::vector<Feature> DetectFAST(const unsigned char* data, int width, int height,
+                           int stride, int min_trackness = 128,
+                           int min_distance = 120);
+
 /*!
     Detect features in an image.
 
@@ -63,10 +88,8 @@ struct Feature {
     \note \a You can crop the image (to avoid detecting markers near the borders) without copying:
              image += marginY*stride+marginX, width -= 2*marginX, height -= 2*marginY;
 */
-void Detect(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance /*=32*/, ubyte* pattern /*=0*/);
+void DetectMORAVEC(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance /*=32*/, ubyte* pattern /*=0*/);
 
-#ifdef __cplusplus
 }
-#endif
 
 #endif
diff --git a/extern/libmv/patches/detect.patch b/extern/libmv/patches/detect.patch
new file mode 100644
index 00000000000..0d9b88faa96
--- /dev/null
+++ b/extern/libmv/patches/detect.patch
@@ -0,0 +1,151 @@
+diff --git a/src/libmv/simple_pipeline/detect.cc b/src/libmv/simple_pipeline/detect.cc
+index 6fc0cdd..b316f42 100644
+--- a/src/libmv/simple_pipeline/detect.cc
++++ b/src/libmv/simple_pipeline/detect.cc
+@@ -23,15 +23,59 @@
+ ****************************************************************************/
+ 
+ #include "libmv/simple_pipeline/detect.h"
++#include <third_party/fast/fast.h>
+ #include <stdlib.h>
+-#include <string.h>
++#include <memory.h>
++
++#ifdef __SSE2__
++#include <emmintrin.h>
++#endif
+ 
+ namespace libmv {
+ 
+ typedef unsigned int uint;
+ 
++std::vector<Feature> DetectFAST(const unsigned char* data, int width, int height, int stride,
++                           int min_trackness, int min_distance) {
++  std::vector<Feature> features;
++  // 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);
++  if(num_features == 0) {
++    free(all);
++    return features;
++  }
++  int* scores = fast9_score(data, stride, all, num_features, min_trackness);
++  // TODO: 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) features.reserve(num_features);
++  for(int i = 0; i < num_features; ++i) {
++    xy xy = nonmax[i];
++    Feature a = { xy.x, xy.y, scores[i], 7 };
++    // 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*min_distance ) {
++        // already a nearby feature
++        goto skip;
++      }
++    }
++    // otherwise add the new feature
++    features.push_back(a);
++    skip: ;
++  }
++  free(scores);
++  free(nonmax);
++  return features;
++}
++
+ #ifdef __SSE2__
+-#include <emmintrin.h>
+ static uint SAD(const ubyte* imageA, const ubyte* imageB, int strideA, int strideB) {
+   __m128i a = _mm_setzero_si128();
+   for(int i = 0; i < 16; i++) {
+@@ -52,7 +96,7 @@ static uint SAD(const ubyte* imageA, const ubyte* imageB, int strideA, int strid
+ }
+ #endif
+ 
+-void Detect(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance, ubyte* pattern) {
++void DetectMORAVEC(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance, ubyte* pattern) {
+   unsigned short histogram[256];
+   memset(histogram,0,sizeof(histogram));
+   ubyte* scores = new ubyte[width*height];
+diff --git a/src/libmv/simple_pipeline/detect.h b/src/libmv/simple_pipeline/detect.h
+index 23b239b..bbe7aed 100644
+--- a/src/libmv/simple_pipeline/detect.h
++++ b/src/libmv/simple_pipeline/detect.h
+@@ -25,27 +25,52 @@
+ #ifndef LIBMV_SIMPLE_PIPELINE_DETECT_H_
+ #define LIBMV_SIMPLE_PIPELINE_DETECT_H_
+ 
+-#ifdef __cplusplus
++#include <vector>
++
+ namespace libmv {
+-#endif
+ 
+ typedef unsigned char ubyte;
+ 
+ /*!
+-    \a Feature is the 2D location of a detected feature in an image.
++    A Feature is the 2D location of a detected feature in an image.
+ 
+-    \a x, \a y is the position of the center in pixels (from image top-left).
+-    \a score is an estimate of how well the pattern will be tracked.
+-    \a size can be used as an initial size to track the pattern.
++    \a x, \a y is the position of the feature in pixels from the top left corner.
++    \a score is an estimate of how well the feature will be tracked.
++    \a size can be used as an initial pattern size to track the feature.
+ 
+     \sa Detect
+ */
+ struct Feature {
++  /// Position in pixels (from top-left corner)
++  /// \note libmv might eventually support subpixel precision.
+   float x, y;
++  /// Trackness of the feature
+   float score;
++  /// Size of the feature in pixels
+   float size;
+ };
+- //radius for non maximal suppression
++
++/*!
++    Detect features in an image.
++
++    You need to input a single channel 8-bit image using pointer to image \a data,
++    \a width, \a height and \a stride (i.e bytes per line).
++
++    You can tweak the count of detected features using \a min_trackness, which is
++    the minimum score to add a feature, and \a min_distance which is the minimal
++    distance accepted between two featuress.
++
++    \note You can binary search over \a min_trackness to get a given feature count.
++
++    \note a way to get an uniform distribution of a given feature count is:
++          \a min_distance = \a width * \a height / desired_feature_count ^ 2
++
++    \return All detected feartures matching given parameters
++*/
++std::vector<Feature> DetectFAST(const unsigned char* data, int width, int height,
++                           int stride, int min_trackness = 128,
++                           int min_distance = 120);
++
+ /*!
+     Detect features in an image.
+ 
+@@ -63,10 +88,8 @@ struct Feature {
+     \note \a You can crop the image (to avoid detecting markers near the borders) without copying:
+              image += marginY*stride+marginX, width -= 2*marginX, height -= 2*marginY;
+ */
+-void Detect(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance /*=32*/, ubyte* pattern /*=0*/);
++void DetectMORAVEC(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance /*=32*/, ubyte* pattern /*=0*/);
+ 
+-#ifdef __cplusplus
+ }
+-#endif
+ 
+ #endif
diff --git a/extern/libmv/patches/series b/extern/libmv/patches/series
index 410a14ca135..00a52c1cfaa 100644
--- a/extern/libmv/patches/series
+++ b/extern/libmv/patches/series
@@ -10,3 +10,4 @@ mingw.patch
 msvc2010.patch
 scaled_distortion.patch
 overscan.patch
+detect.patch