Add Procrustes PNP ("PPnP") resection algorithm to libmv
authorSergey Sharybin <sergey.vfx@gmail.com>
Fri, 16 Aug 2013 08:26:34 +0000 (08:26 +0000)
committerSergey Sharybin <sergey.vfx@gmail.com>
Fri, 16 Aug 2013 08:26:34 +0000 (08:26 +0000)
This adds a new Euclidean resection method, used to create the
initial reconstruction in the motion tracker, to libmv. The method
is based on the Procrustes PNP algorithm (aka "PPnP"). Currently
the algorithm is not connected with the motion tracker, but it
will be eventually since it supports initialization.

Having an initial guess when doing resection is important for
ambiguous cases where potentially the user could offer extra
guidance to the solver, in the form of "this point is in front of
that point".

--
svn merge -r58821:58822 ^/branches/soc-2011-tomato

extern/libmv/libmv/multiview/euclidean_resection.cc
extern/libmv/libmv/multiview/euclidean_resection.h

index 10c7330770f6480bb4d5a5243eb6ad670ccf6b77..d5421b9691ee0b4c9e8478bd9fbb866f49fc0b35 100644 (file)
@@ -34,6 +34,11 @@ namespace libmv {
 namespace euclidean_resection {
 
 typedef unsigned int uint;
+  
+bool EuclideanResectionPPnP(const Mat2X &x_camera,
+                            const Mat3X &X_world,
+                            Mat3 *R, Vec3 *t,
+                            double tolerance);
 
 bool EuclideanResection(const Mat2X &x_camera,
                         const Mat3X &X_world,
@@ -47,6 +52,9 @@ bool EuclideanResection(const Mat2X &x_camera,
     case RESECTION_EPNP:
       return EuclideanResectionEPnP(x_camera, X_world, R, t, success_threshold);
       break;
+    case RESECTION_PPNP:
+      return EuclideanResectionPPnP(x_camera, X_world, R, t, success_threshold);
+      break;
     default:
       LOG(FATAL) << "Unknown resection method.";
   }
@@ -674,6 +682,107 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
   // TODO(julien): Improve the solutions with non-linear refinement.
   return true;
 }
+  
+/*
+ Straight from the paper:
+ http://www.diegm.uniud.it/fusiello/papers/3dimpvt12-b.pdf
+ function [R T] = ppnp(P,S,tol)
+ % input
+ % P  : matrix (nx3) image coordinates in camera reference [u v 1]
+ % S  : matrix (nx3) coordinates in world reference [X Y Z]
+ % tol: exit threshold
+ %
+ % output
+ % R : matrix (3x3) rotation (world-to-camera)
+ % T : vector (3x1) translation (world-to-camera)
+ %
+ n = size(P,1);
+ Z = zeros(n);
+ e = ones(n,1);
+ A = eye(n)-((e*e’)./n);
+ II = e./n;
+ err = +Inf;
+ E_old = 1000*ones(n,3);
+ while err>tol
+   [U,˜,V] = svd(P’*Z*A*S);
+   VT = V’;
+   R=U*[1 0 0; 0 1 0; 0 0 det(U*VT)]*VT;
+   PR = P*R;
+   c = (S-Z*PR)’*II;
+   Y = S-e*c’;
+   Zmindiag = diag(PR*Y’)./(sum(P.*P,2));
+   Zmindiag(Zmindiag<0)=0;
+   Z = diag(Zmindiag);
+   E = Y-Z*PR;
+   err = norm(E-E_old,’fro’);
+   E_old = E;
+ end
+ T = -R*c;
+ end
+ */
+// TODO(keir): Re-do all the variable names and add comments matching the paper.
+// This implementation has too much of the terseness of the original. On the
+// other hand, it did work on the first try.
+bool EuclideanResectionPPnP(const Mat2X &x_camera,
+                            const Mat3X &X_world,
+                            Mat3 *R, Vec3 *t,
+                            double tolerance) {
+  int n = x_camera.cols();
+  Mat Z = Mat::Zero(n, n);
+  Vec e = Vec::Ones(n);
+  Mat A = Mat::Identity(n, n) - (e * e.transpose() / n);
+  Vec II = e / n;
+  
+  Mat P(n, 3);
+  P.col(0) = x_camera.row(0);
+  P.col(1) = x_camera.row(1);
+  P.col(2).setConstant(1.0);
+  
+  Mat S = X_world.transpose();
+  
+  double error = std::numeric_limits<double>::infinity();
+  Mat E_old = 1000 * Mat::Ones(n, 3);
+  
+  Vec3 c;
+  Mat E(n, 3);
+  
+  int iteration = 0;
+  tolerance = 1e-5;
+  // TODO(keir): The limit of 100 can probably be reduced, but this will require
+  // some investigation.
+  while (error > tolerance && iteration < 100) {
+    Mat3 tmp = P.transpose() * Z * A * S;
+    Eigen::JacobiSVD<Mat3> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    Mat3 U = svd.matrixU();
+    Mat3 VT = svd.matrixV().transpose();
+    Vec3 s;
+    s << 1, 1, (U * VT).determinant();
+    *R = U * s.asDiagonal() * VT;
+    Mat PR = P * *R;  // n x 3
+    c = (S - Z*PR).transpose() * II;
+    Mat Y = S - e*c.transpose();  // n x 3
+    Vec Zmindiag = (PR * Y.transpose()).diagonal()
+        .cwiseQuotient(P.rowwise().squaredNorm());
+    for (int i = 0; i < n; ++i) {
+      Zmindiag[i] = std::max(Zmindiag[i], 0.0);
+    }
+    Z = Zmindiag.asDiagonal();
+    E = Y - Z*PR;
+    error = (E - E_old).norm();
+    LOG(INFO) << "PPnP error(" << (iteration++) << "): " << error;
+    E_old = E;
+  }
+  *t = -*R*c;
+
+  // TODO(keir): Figure out what the failure cases are. Is it too many
+  // iterations? Spend some time going through the math figuring out if there
+  // is some way to detect that the algorithm is going crazy, and return false.
+  return true;
+}
+
 
 }  // namespace resection
 }  // namespace libmv
index 1a329702c2a6e5aff465140ffc9cc5729e7772bb..ff9bccdd5c9fb51333d6b9f66fc266b7af14670f 100644 (file)
@@ -33,6 +33,10 @@ enum ResectionMethod {
   // The "EPnP" algorithm by Lepetit et al.
   // http://cvlab.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf
   RESECTION_EPNP,
+  
+  // The Procrustes PNP algorithm ("PPnP")
+  // http://www.diegm.uniud.it/fusiello/papers/3dimpvt12-b.pdf
+  RESECTION_PPNP
 };
 
 /**