Make Euclidean resection "always" succeed.

The Euclidean resection code had a magical constant, 1e-3, used to
compare the results of solving an equation. This failure detection
was well-intended, trying to prevent poor solutions from getting
made without notifying the caller. Unfortunately in practice, this
threshold is too conservative. Furthermore, it is not clear the
threshold should exist at all; the purpose of the Euclidean
resection is to come up with the best solution it can; other
methods (e.g. reprojection error) should be used to compare
whether the method succeeded.

This commit changes the Euclidean EPnP code to always succeed,
causing the previous fallback to projective resection to never
run. In most cases, this will result in better reconstructions.

This should, in most cases, fix the dreaded "flipping" problem.

5 files changed, 27 insertions, 11 deletions

diff --git a/extern/libmv/libmv/multiview/euclidean_resection.cc b/extern/libmv/libmv/multiview/euclidean_resection.cc
index bcde6de5cab..6cad4cac049 100644
--- a/extern/libmv/libmv/multiview/euclidean_resection.cc
+++ b/extern/libmv/libmv/multiview/euclidean_resection.cc
@@ -535,7 +535,16 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
   vector<Vec3> ts(3);
   Vec rmse(3);
 
-  // TODO(julien): Document where the "1e-3" magical constant comes from below.
+  // At one point this threshold was 1e-3, and caused no end of problems in
+  // Blender by causing frames to not resect when they would have worked fine.
+  // When the resect failed, the projective followup is run leading to worse
+  // results, and often the dreaded "flipping" where objects get flipped
+  // between frames. Instead, disable the check for now, always succeeding. The
+  // ultimate check is always reprojection error anyway.
+  //
+  // TODO(keir): Decide if setting this to infinity, effectively disabling the
+  // check, is the right approach. So far this seems the case.
+  double kSuccessThreshold = std::numeric_limits<double>::max();
 
   // Find the first possible solution for R, t corresponding to:
   // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
@@ -548,7 +557,7 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
   Eigen::JacobiSVD<Mat> svd_of_l4(l_6x4, 
                                   Eigen::ComputeFullU | Eigen::ComputeFullV);
   Vec4 b4 = svd_of_l4.solve(rho);
-  if ((l_6x4 * b4).isApprox(rho, 1e-3)) {
+  if ((l_6x4 * b4).isApprox(rho, kSuccessThreshold)) {
     if (b4(0) < 0) {
       b4 = -b4;
     } 
@@ -574,7 +583,7 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
   Vec3 b3 = svdOfL3.solve(rho);
   VLOG(2) << " rho = " << rho;
   VLOG(2) << " l_6x3 * b3 = " << l_6x3 * b3;
-  if ((l_6x3 * b3).isApprox(rho, 1e-3)) {
+  if ((l_6x3 * b3).isApprox(rho, kSuccessThreshold)) {
     if (b3(0) < 0) {
       betas(0) = std::sqrt(-b3(0));
       betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0;
@@ -605,7 +614,7 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
   Eigen::JacobiSVD<Mat> svdOfL5(l_6x5, 
                                 Eigen::ComputeFullU | Eigen::ComputeFullV);
   Vec5 b5 = svdOfL5.solve(rho);
-  if ((l_6x5 * b5).isApprox(rho, 1e-3)) {
+  if ((l_6x5 * b5).isApprox(rho, kSuccessThreshold)) {
     if (b5(0) < 0) {
       betas(0) = std::sqrt(-b5(0));
       if (b5(2) < 0) {
diff --git a/extern/libmv/libmv/multiview/euclidean_resection.h b/extern/libmv/libmv/multiview/euclidean_resection.h
index 08fa3d90bd3..f20513d3fbe 100644
--- a/extern/libmv/libmv/multiview/euclidean_resection.h
+++ b/extern/libmv/libmv/multiview/euclidean_resection.h
@@ -29,6 +29,9 @@ namespace euclidean_resection {
   
 enum ResectionMethod {
   RESECTION_ANSAR_DANIILIDIS,
+
+  // The "EPnP" algorithm by Lepetit et al.
+  // http://cvlab.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf
   RESECTION_EPNP,
 };
 
diff --git a/extern/libmv/libmv/numeric/levenberg_marquardt.h b/extern/libmv/libmv/numeric/levenberg_marquardt.h
index 4473b72f156..a7877e0270b 100644
--- a/extern/libmv/libmv/numeric/levenberg_marquardt.h
+++ b/extern/libmv/libmv/numeric/levenberg_marquardt.h
@@ -124,11 +124,11 @@ class LevenbergMarquardt {
     Parameters dx, x_new;
     int i;
     for (i = 0; results.status == RUNNING && i < params.max_iterations; ++i) {
-      VLOG(1) << "iteration: " << i;
-      VLOG(1) << "||f(x)||: " << f_(x).norm();
-      VLOG(1) << "max(g): " << g.array().abs().maxCoeff();
-      VLOG(1) << "u: " << u;
-      VLOG(1) << "v: " << v;
+      VLOG(3) << "iteration: " << i;
+      VLOG(3) << "||f(x)||: " << f_(x).norm();
+      VLOG(3) << "max(g): " << g.array().abs().maxCoeff();
+      VLOG(3) << "u: " << u;
+      VLOG(3) << "v: " << v;
 
       AMatrixType A_augmented = A + u*AMatrixType::Identity(J.cols(), J.cols());
       Solver solver(A_augmented);
diff --git a/extern/libmv/libmv/simple_pipeline/pipeline.cc b/extern/libmv/libmv/simple_pipeline/pipeline.cc
index 2459d059114..4773a70d69b 100644
--- a/extern/libmv/libmv/simple_pipeline/pipeline.cc
+++ b/extern/libmv/libmv/simple_pipeline/pipeline.cc
@@ -242,9 +242,9 @@ void InternalCompleteReconstruction(
                                        (double)tot_resects/(max_image));
       if (PipelineRoutines::Resect(reconstructed_markers, reconstruction, true)) {
         num_resects++;
-        LG << "Ran Resect() for image " << image;
+        LG << "Ran final Resect() for image " << image;
       } else {
-        LG << "Failed Resect() for image " << image;
+        LG << "Failed final Resect() for image " << image;
       }
     }
   }
diff --git a/extern/libmv/libmv/simple_pipeline/resect.cc b/extern/libmv/libmv/simple_pipeline/resect.cc
index b30238f7e39..8737182dd43 100644
--- a/extern/libmv/libmv/simple_pipeline/resect.cc
+++ b/extern/libmv/libmv/simple_pipeline/resect.cc
@@ -108,6 +108,10 @@ bool EuclideanResect(const vector<Marker> &markers,
     // printf("Resection for image %d failed\n", markers[0].image);
     LG << "Resection for image " << markers[0].image << " failed;"
        << " trying fallback projective resection.";
+
+    LG << "No fallback; failing resection for " << markers[0].image;
+    return false;
+
     if (!final_pass) return false;
     // Euclidean resection failed. Fall back to projective resection, which is
     // less reliable but better conditioned when there are many points.