Camera tracking: SAD tracker now supports patterns with any size

(rectangle patterns are getting enlarged to square like it's happening for KLT)

3 files changed, 26 insertions, 9 deletions

diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp
index f08aea9fbd1..aa05279d731 100644
--- a/extern/libmv/libmv-capi.cpp
+++ b/extern/libmv/libmv-capi.cpp
@@ -297,16 +297,16 @@ void libmv_regionTrackerDestroy(libmv_RegionTracker *libmv_tracker)
 /* ************ Tracks ************ */
 
 void libmv_SADSamplePattern(unsigned char *image, int stride,
-			float warp[3][2], unsigned char *pattern)
+			float warp[3][2], unsigned char *pattern, int pattern_size)
 {
 	libmv::mat32 mat32;
 
 	memcpy(mat32.data, warp, sizeof(float)*3*2);
 
-	libmv::SamplePattern(image, stride, mat32, pattern, 16);
+	libmv::SamplePattern(image, stride, mat32, pattern, pattern_size);
 }
 
-float libmv_SADTrackerTrack(unsigned char *pattern, unsigned char *warped, unsigned char *image, int stride,
+float libmv_SADTrackerTrack(unsigned char *pattern, unsigned char *warped, int pattern_size, unsigned char *image, int stride,
 			int width, int height, float warp[3][2])
 {
 	float result;
@@ -314,7 +314,7 @@ float libmv_SADTrackerTrack(unsigned char *pattern, unsigned char *warped, unsig
 
 	memcpy(mat32.data, warp, sizeof(float)*3*2);
 
-	result = libmv::Track(pattern, warped, 16, image, stride, width, height, &mat32, 16, 16);
+	result = libmv::Track(pattern, warped, pattern_size, image, stride, width, height, &mat32, 16, 16);
 
 	memcpy(warp, mat32.data, sizeof(float)*3*2);
 
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index 536f8a5f14c..321593520b5 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -50,8 +50,8 @@ void libmv_regionTrackerDestroy(struct libmv_RegionTracker *libmv_tracker);
 
 /* SAD Tracker */
 void libmv_SADSamplePattern(unsigned char *image, int stride,
-			float warp[3][2], unsigned char *pattern);
-float libmv_SADTrackerTrack(unsigned char *pattern, unsigned char *warped, unsigned char *image,
+			float warp[3][2], unsigned char *pattern, int pattern_size);
+float libmv_SADTrackerTrack(unsigned char *pattern, unsigned char *warped, int pattern_size, unsigned char *image,
 			int stride, int width, int height, float warp[3][2]);
 
 /* Tracks */
diff --git a/extern/libmv/libmv/tracking/sad.cc b/extern/libmv/libmv/tracking/sad.cc
index 9b446bb4c35..0876ef2fe73 100644
--- a/extern/libmv/libmv/tracking/sad.cc
+++ b/extern/libmv/libmv/tracking/sad.cc
@@ -25,6 +25,7 @@
 #include "libmv/tracking/sad.h"
 #include <stdlib.h>
 #include <math.h>
+#include <stdio.h>
 
 namespace libmv {
 
@@ -78,15 +79,31 @@ void SamplePattern(ubyte* image, int stride, mat32 warp, ubyte* pattern, int siz
 
 #ifdef __SSE2__
 #include <emmintrin.h>
-static uint SAD(const ubyte* pattern, const ubyte* image, int stride, int size) {
+  static uint SAD(/*const*/ ubyte* pattern, /*const*/ ubyte* image, int stride, int size) {
+  uint sad = 0;
   __m128i a = _mm_setzero_si128();
+
   for(int i = 0; i < size; i++) {
-    for(int j = 0; j < size/16; j++) {
+    int j = 0;
+
+    for(j = 0; j < size/16; j++) {
+      if((i*size/16+j) % 32 == 0) {
+        sad += _mm_extract_epi16(a,0) + _mm_extract_epi16(a,4);
+        a = _mm_setzero_si128();
+      }
+
       a = _mm_adds_epu16(a, _mm_sad_epu8( _mm_loadu_si128((__m128i*)(pattern+i*size+j*16)),
                                           _mm_loadu_si128((__m128i*)(image+i*stride+j*16))));
     }
+
+    for(j = j*16; j < size; j++) {
+      sad += abs((int)pattern[i*size+j] - image[i*stride+j]);
+    }
   }
-  return _mm_extract_epi16(a,0) + _mm_extract_epi16(a,4);
+
+  sad += _mm_extract_epi16(a,0) + _mm_extract_epi16(a,4);
+
+  return sad;
 }
 #else
 static uint SAD(const ubyte* pattern, const ubyte* image, int stride, int size) {