Enhance logging in libmv's trackers.

Cleanups in brute_region_tracker.cc.

4 files changed, 74 insertions, 60 deletions

diff --git a/extern/libmv/libmv/image/correlation.h b/extern/libmv/libmv/image/correlation.h
index 9d6aceecceb..ba64951a167 100644
--- a/extern/libmv/libmv/image/correlation.h
+++ b/extern/libmv/libmv/image/correlation.h
@@ -21,6 +21,7 @@
 #ifndef LIBMV_IMAGE_CORRELATION_H
 #define LIBMV_IMAGE_CORRELATION_H
 
+#include "libmv/logging/logging.h"
 #include "libmv/image/image.h"
 
 namespace libmv {
@@ -28,7 +29,8 @@ namespace libmv {
 inline double PearsonProductMomentCorrelation(Array3Df image_and_gradient1_sampled,
                                               Array3Df image_and_gradient2_sampled,
                                               int width) {
-  double sX=0,sY=0,sXX=0,sYY=0,sXY=0;
+  double sX = 0, sY = 0, sXX = 0, sYY = 0, sXY = 0;
+
   for (int r = 0; r < width; ++r) {
     for (int c = 0; c < width; ++c) {
       double x = image_and_gradient1_sampled(r, c, 0);
@@ -40,9 +42,23 @@ inline double PearsonProductMomentCorrelation(Array3Df image_and_gradient1_sampl
       sXY += x*y;
     }
   }
-  double N = width*width;
-  sX /= N, sY /= N, sXX /= N, sYY /= N, sXY /= N;
-  double correlation = (sXY-sX*sY)/sqrt(double((sXX-sX*sX)*(sYY-sY*sY)));
+
+  // Normalize.
+  double N = width * width;
+  sX /= N;
+  sY /= N;
+  sXX /= N;
+  sYY /= N;
+  sXY /= N;
+
+  double var_x = sXX - sX*sX;
+  double var_y = sYY - sY*sY;
+  double covariance_xy = sXY - sX*sY;
+
+  double correlation = covariance_xy / sqrt(var_x * var_y);
+  LG << "Covariance xy: " << covariance_xy
+     << ", var 1: " << var_x << ", var 2: " << var_y
+     << ", correlation: " << correlation;
   return correlation;
 }
 
diff --git a/extern/libmv/libmv/image/sample.h b/extern/libmv/libmv/image/sample.h
index ea86edf117d..e842747e6d4 100644
--- a/extern/libmv/libmv/image/sample.h
+++ b/extern/libmv/libmv/image/sample.h
@@ -95,7 +95,6 @@ inline void DownsampleChannelsBy2(const Array3Df &in, Array3Df *out) {
       }
     }
   }
-
 }
 
 // Sample a region centered at x,y in image with size extending by half_width
diff --git a/extern/libmv/libmv/tracking/brute_region_tracker.cc b/extern/libmv/libmv/tracking/brute_region_tracker.cc
index af7f673472e..ef36dffb56d 100644
--- a/extern/libmv/libmv/tracking/brute_region_tracker.cc
+++ b/extern/libmv/libmv/tracking/brute_region_tracker.cc
@@ -310,7 +310,7 @@ bool BruteRegionTracker::Track(const FloatImage &image1,
   FloatArrayToByteArrayWithPadding(image_and_gradient2, &search_area, &search_area_stride);
 
   // Try all possible locations inside the search area. Yes, everywhere.
-  int best_i, best_j, best_sad = INT_MAX;
+  int best_i = -1, best_j = -1, best_sad = INT_MAX;
   for (int i = 0; i < image2.Height() - pattern_width; ++i) {
     for (int j = 0; j < image2.Width() - pattern_width; ++j) {
       int sad = SumOfAbsoluteDifferencesContiguousImage(pattern,
@@ -327,38 +327,51 @@ bool BruteRegionTracker::Track(const FloatImage &image1,
     }
   }
 
+  CHECK_NE(best_i, -1);
+  CHECK_NE(best_j, -1);
+
   aligned_free(pattern);
   aligned_free(search_area);
 
-  if (best_sad != INT_MAX) {
-    *x2 = best_j + half_window_size;
-    *y2 = best_i + half_window_size;
-
-    if (minimum_correlation > 0) {
-      Array3Df image_and_gradient1_sampled, image_and_gradient2_sampled;
-
-      SamplePattern(image_and_gradient1, x1, y1, half_window_size, 3,
-                    &image_and_gradient1_sampled);
-      SamplePattern(image_and_gradient2, *x2, *y2, half_window_size, 3,
-                    &image_and_gradient2_sampled);
-
-      // Compute the Pearson product-moment correlation coefficient to check
-      // for sanity.
-      double correlation = PearsonProductMomentCorrelation(image_and_gradient1_sampled,
-                                                           image_and_gradient2_sampled,
-                                                           pattern_width);
-      LG << "Final correlation: " << correlation;
-
-      if (correlation < minimum_correlation) {
-        LG << "Correlation " << correlation << " greater than "
-           << minimum_correlation << "; bailing.";
-        return false;
-      }
-    }
+  if (best_sad == INT_MAX) {
+    LG << "Hit INT_MAX in SAD; failing.";
+    return false;
+  }
+
+  *x2 = best_j + half_window_size;
+  *y2 = best_i + half_window_size;
 
+  // Calculate the shift done by the fine tracker.
+  double dx2 = *x2 - x1;
+  double dy2 = *y2 - y1;
+  double fine_shift = sqrt(dx2 * dx2 + dy2 * dy2);
+  LG << "Brute shift: dx=" << dx2 << " dy=" << dy2 << ", d=" << fine_shift;
+
+  if (minimum_correlation <= 0) {
+    // No correlation checking requested; nothing else to do.
+    LG << "No correlation checking; returning success. best_sad: " << best_sad;
     return true;
   }
-  return false;
+
+  Array3Df image_and_gradient1_sampled, image_and_gradient2_sampled;
+  SamplePattern(image_and_gradient1, x1, y1, half_window_size, 3,
+                &image_and_gradient1_sampled);
+  SamplePattern(image_and_gradient2, *x2, *y2, half_window_size, 3,
+                &image_and_gradient2_sampled);
+
+  // Compute the Pearson product-moment correlation coefficient to check
+  // for sanity.
+  double correlation = PearsonProductMomentCorrelation(image_and_gradient1_sampled,
+                                                       image_and_gradient2_sampled,
+                                                       pattern_width);
+  LG << "Final correlation: " << correlation;
+
+  if (correlation < minimum_correlation) {
+    LG << "Correlation " << correlation << " greater than "
+       << minimum_correlation << "; bailing.";
+    return false;
+  }
+  return true;
 }
 
 }  // namespace libmv
diff --git a/extern/libmv/libmv/tracking/esm_region_tracker.cc b/extern/libmv/libmv/tracking/esm_region_tracker.cc
index e0b85f19943..221fa4d081b 100644
--- a/extern/libmv/libmv/tracking/esm_region_tracker.cc
+++ b/extern/libmv/libmv/tracking/esm_region_tracker.cc
@@ -41,6 +41,7 @@ static bool RegionIsInBounds(const FloatImage &image1,
   int min_y = floor(y) - half_window_size - 1;
   if (min_x < 0.0 ||
       min_y < 0.0) {
+    LG << "Out of bounds; min_x: " << min_x << ", min_y: " << min_y;
     return false;
   }
 
@@ -49,6 +50,9 @@ static bool RegionIsInBounds(const FloatImage &image1,
   int max_y = ceil(y) + half_window_size + 1;
   if (max_x > image1.cols() ||
       max_y > image1.rows()) {
+    LG << "Out of bounds; max_x: " << max_x << ", max_y: " << max_y
+       << ", image1.cols(): " << image1.cols()
+       << ", image1.rows(): " << image1.rows();
     return false;
   }
 
@@ -56,24 +60,6 @@ static bool RegionIsInBounds(const FloatImage &image1,
   return true;
 }
 
-// Estimate "reasonable" error by computing autocorrelation for a small shift.
-// TODO(keir): Add a facility for 
-static double EstimateReasonableError(const FloatImage &image,
-                               double x, double y,
-                               int half_width) {
-  double error = 0.0;
-  for (int r = -half_width; r <= half_width; ++r) {
-    for (int c = -half_width; c <= half_width; ++c) {
-      double s = SampleLinear(image, y + r, x + c, 0);
-      double e1 = SampleLinear(image, y + r + 0.5, x + c, 0) - s;
-      double e2 = SampleLinear(image, y + r, x + c + 0.5, 0) - s;
-      error += e1*e1 + e2*e2;
-    }
-  }
-  // XXX hack
-  return error / 2.0 * 16.0;
-}
-
 // This is implemented from "Lukas and Kanade 20 years on: Part 1. Page 42,
 // figure 14: the Levenberg-Marquardt-Inverse Compositional Algorithm".
 bool EsmRegionTracker::Track(const FloatImage &image1,
@@ -107,9 +93,6 @@ bool EsmRegionTracker::Track(const FloatImage &image1,
   // 
   // Ignored for my "normal" LM loop.
 
-  double reasonable_error =
-      EstimateReasonableError(image1, x1, y1, half_window_size);
-
   // Step 1: Warp I with W(x, p) to compute I(W(x; p).
   //
   // Use two images for accepting / rejecting updates.
@@ -228,7 +211,7 @@ bool EsmRegionTracker::Track(const FloatImage &image1,
         new_error += e*e;
       }
     }
-    //LG << "Old error: " << error << ", new error: " << new_error;
+    LG << "Old error: " << error << ", new error: " << new_error;
 
     double rho = (error - new_error) / (d.transpose() * (mu * d + z));
 
@@ -253,6 +236,7 @@ bool EsmRegionTracker::Track(const FloatImage &image1,
 
       mu *= std::max(1/3., 1 - pow(2*rho - 1, 3));
       nu = M_E;  // See above for why to use e.
+      LG << "Error decreased, so accept update.";
     }
 
     // If the step was accepted, then check for termination.
@@ -264,13 +248,15 @@ bool EsmRegionTracker::Track(const FloatImage &image1,
                                                            width);
       LG << "Final correlation: " << correlation;
 
-      if (correlation < minimum_correlation) {
-        LG << "Correlation " << correlation << " greater than "
-           << minimum_correlation << "; bailing.";
-        return false;
+      // Note: Do the comparison here to handle nan's correctly (since all
+      // comparisons with nan are false).
+      if (minimum_correlation < correlation) {
+        LG << "Successful track in " << (i + 1) << " iterations.";
+        return true;
       }
-      LG << "Successful track in " << (i + 1) << " iterations.";
-      return true;
+      LG << "Correlation " << correlation << " greater than "
+         << minimum_correlation << " or is nan; bailing.";
+      return false;
     }
   }
   // Getting here means we hit max iterations, so tracking failed.