[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [39587] branches/soc-2011-tomato/extern/ libmv/libmv/numeric: Camera tracking integration

[Sergey Sharybin]

Revision: 39587
          http://projects.blender.org/scm/viewvc.php?view=rev&root=bf-blender&revision=39587
Author:   nazgul
Date:     2011-08-21 17:08:05 +0000 (Sun, 21 Aug 2011)
Log Message:
-----------
Camera tracking integration
===========================

Reverting commit 81613ee0cc94b315f333c9632b18b95d426aad05 from Matthias branch.
It destroyed lens distortion code.

Modified Paths:
--------------
    branches/soc-2011-tomato/extern/libmv/libmv/numeric/function_derivative.h
    branches/soc-2011-tomato/extern/libmv/libmv/numeric/levenberg_marquardt.h

Modified: branches/soc-2011-tomato/extern/libmv/libmv/numeric/function_derivative.h
===================================================================
--- branches/soc-2011-tomato/extern/libmv/libmv/numeric/function_derivative.h	2011-08-21 15:47:21 UTC (rev 39586)
+++ branches/soc-2011-tomato/extern/libmv/libmv/numeric/function_derivative.h	2011-08-21 17:08:05 UTC (rev 39587)
@@ -24,6 +24,7 @@
 #include <cmath>
 
 #include "libmv/numeric/numeric.h"
+#include "libmv/logging/logging.h"
 
 namespace libmv {
 
@@ -97,7 +98,7 @@
 
   typename NumericJacobian<Function>::JMatrixType J_numeric = j_numeric(x);
   typename NumericJacobian<Function>::JMatrixType J_analytic = j_analytic(x);
-  //LG << J_numeric - J_analytic;
+  LG << J_numeric - J_analytic;
   return true;
 }
 

Modified: branches/soc-2011-tomato/extern/libmv/libmv/numeric/levenberg_marquardt.h
===================================================================
--- branches/soc-2011-tomato/extern/libmv/libmv/numeric/levenberg_marquardt.h	2011-08-21 15:47:21 UTC (rev 39586)
+++ branches/soc-2011-tomato/extern/libmv/libmv/numeric/levenberg_marquardt.h	2011-08-21 17:08:05 UTC (rev 39587)
@@ -33,6 +33,7 @@
 
 #include "libmv/numeric/numeric.h"
 #include "libmv/numeric/function_derivative.h"
+#include "libmv/logging/logging.h"
 
 namespace libmv {
 
@@ -123,26 +124,40 @@
     Parameters dx, x_new;
     int i;
     for (i = 0; results.status == RUNNING && i < params.max_iterations; ++i) {
-      if (dx.norm() <= params.relative_step_threshold * x.norm()) {
+      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;
+
+      AMatrixType A_augmented = A + u*AMatrixType::Identity(J.cols(), J.cols());
+      Solver solver(A_augmented);
+      dx = solver.solve(g);
+      bool solved = (A_augmented * dx).isApprox(g);
+      if (!solved) {
+        LOG(ERROR) << "Failed to solve";
+      }
+      if (solved && dx.norm() <= params.relative_step_threshold * x.norm()) {
         results.status = RELATIVE_STEP_SIZE_TOO_SMALL;
         break;
-      }
-      x_new = x + dx;
-      // Rho is the ratio of the actual reduction in error to the reduction
-      // in error that would be obtained if the problem was linear.
-      // See [1] for details.
-      Scalar rho((error.squaredNorm() - f_(x_new).squaredNorm())
-                 / dx.dot(u*dx + g));
-      if (rho > 0) {
-        // Accept the Gauss-Newton step because the linear model fits well.
-        x = x_new;
-        results.status = Update(x, params, &J, &A, &error, &g);
-        Scalar tmp = Scalar(2*rho-1);
-        u = u*std::max(1/3., 1 - (tmp*tmp*tmp));
-        v = 2;
-        continue;
-      }
-
+      } 
+      if (solved) {
+        x_new = x + dx;
+        // Rho is the ratio of the actual reduction in error to the reduction
+        // in error that would be obtained if the problem was linear.
+        // See [1] for details.
+        Scalar rho((error.squaredNorm() - f_(x_new).squaredNorm())
+                   / dx.dot(u*dx + g));
+        if (rho > 0) {
+          // Accept the Gauss-Newton step because the linear model fits well.
+          x = x_new;
+          results.status = Update(x, params, &J, &A, &error, &g);
+          Scalar tmp = Scalar(2*rho-1);
+          u = u*std::max(1/3., 1 - (tmp*tmp*tmp));
+          v = 2;
+          continue;
+        } 
+      } 
       // Reject the update because either the normal equations failed to solve
       // or the local linear model was not good (rho < 0). Instead, increase u
       // to move closer to gradient descent.