Updating for N=2

modules/calib3d/src/upnp.cpp

@@ -2,17 +2,6 @@
 #include "upnp.h"
 #include <limits>
 
-void printMat(cv::Mat &mat)
-{
-	cout << mat.rows << "x" << mat.cols << endl;
-	for (int i = 0; i < mat.rows; ++i) {
-		cout << "[";
-		for (int j = 0; j < mat.cols; ++j) {
-			cout << mat.at<double>(i,j) << ",";
-		}
-		cout << "];" << endl;
-	}
-}
 upnp::upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints)
 {
   if (cameraMatrix.depth() == CV_32F)
@@ -81,25 +70,25 @@ double upnp::compute_pose(cv::Mat& R, cv::Mat& t)
   compute_L_6x12(ut, l_6x12);
   compute_rho(rho);
 
-	double Betas[4][4], Efs[4][1], rep_errors[4];
-	double Rs[4][3][3], ts[4][3];
+  double Betas[3][4], Efs[3][1], rep_errors[3];
+  double Rs[3][3][3], ts[3][3];
 
   find_betas_and_focal_approx_1(&Ut, &Rho, Betas[1], Efs[1]);
   gauss_newton(&L_6x12, &Rho, Betas[1], Efs[1]);
-	rep_errors[1] = compute_R_and_t(ut, Betas[1], Efs[1], Rs[1], ts[1]);
+  rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
 
   find_betas_and_focal_approx_2(&Ut, &Rho, Betas[2], Efs[2]);
-    //gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]);
-    //rep_errors[2] = compute_R_and_t(ut, Betas[2], Efs[2], Rs[2], ts[2]);
+  gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]);
+  rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
 
   int N = 1;
-	//if (rep_errors[2] < rep_errors[1]) N = 2;
-	//if (rep_errors[3] < rep_errors[N]) N = 3;
+  if (rep_errors[2] < rep_errors[1]) N = 2;
 
   cv::Mat(3, 1, CV_64F, ts[N]).copyTo(t);
   cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
+  fu = fv = Efs[N][0];
 
-	return Efs[N][0];
+  return fu;
 }
 
 void upnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
@@ -187,10 +176,10 @@ void upnp::solve_for_sign(void)
   }
 }
 
-double upnp::compute_R_and_t(const double * ut, const double * betas, const double * efs,
+double upnp::compute_R_and_t(const double * ut, const double * betas,
          double R[3][3], double t[3])
 {
-  compute_ccs(betas, efs, ut);
+  compute_ccs(betas, ut);
   compute_pcs();
 
   solve_for_sign();
@@ -229,8 +218,8 @@ void upnp::choose_control_points()
 void upnp::compute_alphas()
 {
     cv::Mat CC = cv::Mat(4, 3, CV_64F, &cws);
-	cv::Mat PC = cv::Mat(number_of_correspondences, 3, CV_64F, &pws.front());
-	cv::Mat ALPHAS = cv::Mat(number_of_correspondences, 4, CV_64F, &alphas.front());
+    cv::Mat PC = cv::Mat(number_of_correspondences, 3, CV_64F, &pws[0]);
+    cv::Mat ALPHAS = cv::Mat(number_of_correspondences, 4, CV_64F, &alphas[0]);
 
     cv::Mat CC_ = CC.clone().t();
     cv::Mat PC_ = PC.clone().t();
@@ -260,7 +249,7 @@ void upnp::fill_M(CvMat * M, const int row, const double * as, const double u, c
   }
 }
 
-void upnp::compute_ccs(const double * betas, const double * f, const double * ut)
+void upnp::compute_ccs(const double * betas, const double * ut)
 {
     for(int i = 0; i < 4; ++i)
       ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0f;
@@ -270,10 +259,10 @@ void upnp::compute_ccs(const double * betas, const double * f, const double * ut
       const double * v = ut + 12 * (9 + i);
       for(int j = 0; j < 4; ++j)
         for(int k = 0; k < 3; ++k)
-				ccs[j][k] += betas[i] * v[3 * j + k]; // be careful with that
+          ccs[j][k] += betas[i] * v[3 * j + k];
     }
 
-	for (int i = 0; i < 4; ++i) ccs[i][2] *= f[0];
+    for (int i = 0; i < 4; ++i) ccs[i][2] *= fu;
 }
 
 void upnp::compute_pcs(void)
@@ -314,18 +303,44 @@ void upnp::find_betas_and_focal_approx_2(const CvMat * Ut, const CvMat * Rho, do
   cv::Mat Kmf2 = cv::Mat(12, 1, CV_64F, Ut->data.db + 11 * 12);
   cv::Mat dsq = cv::Mat(6, 1, CV_64F, Rho->data.db);
 
-	cv::Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk( Kmf1, Kmf2 );  // OKAY
+  cv::Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk( Kmf1, Kmf2 );
 
   cv::Mat A = D;
   cv::Mat b = dsq;
 
-	cv::Mat x = cv::Mat(6, 1, CV_64F);
+  double x[6];
+  cv::Mat X = cv::Mat(6, 1, CV_64F, x);
 
-	cv::solve(A, b, x, cv::DECOMP_QR);
+  cv::solve(A, b, X, cv::DECOMP_QR);
 
-	betas[0] = std::sqrt( std::abs( x.at<double>(0) ) );
-    betas[1] = std::sqrt( std::abs( x.at<double>(2) ) * ( x.at<double>(2) < 0 ) ? -1 : ( x.at<double>(2) > 0 ) ? 1 : 0 );
+  double solutions[18][3];
+  generate_all_possible_solutions_for_f_unk(x, solutions);
+
+  // find solution with minimum reprojection error
+  double min_error = std::numeric_limits<double>::max();
+  int min_sol = 0;
+  for (int i = 0; i < 18; ++i) {
+
+    betas[3] = solutions[i][0];
+    betas[2] = solutions[i][1];
+    betas[1] = betas[0] = 0;
+    fu = fv = solutions[i][2];
+
+    double Rs[3][3], ts[3];
+    double error_i = compute_R_and_t( Ut->data.db, betas, Rs, ts);
+
+    if( error_i < min_error)
+    {
+      min_error = error_i;
+      min_sol = i;
+    }
+}
+
+  betas[0] = solutions[min_sol][0];
+  betas[1] = solutions[min_sol][1];
   betas[2] = betas[3] = 0;
+
+  efs[0] = solutions[min_sol][2];
 }
 
 cv::Mat upnp::compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1)
@@ -457,9 +472,51 @@ cv::Mat upnp::compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M
   return P;
 }
 
+void upnp::generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])
+{
+  int matrix_to_resolve[18][9] = {
+    { 2, 0, 0, 1, 1, 0, 2, 0, 2 }, { 2, 0, 0, 1, 1, 0, 1, 1, 2 },
+    { 2, 0, 0, 1, 1, 0, 0, 2, 2 }, { 2, 0, 0, 0, 2, 0, 2, 0, 2 },
+    { 2, 0, 0, 0, 2, 0, 1, 1, 2 }, { 2, 0, 0, 0, 2, 0, 0, 2, 2 },
+    { 2, 0, 0, 2, 0, 2, 1, 1, 2 }, { 2, 0, 0, 2, 0, 2, 0, 2, 2 },
+    { 2, 0, 0, 1, 1, 2, 0, 2, 2 }, { 1, 1, 0, 0, 2, 0, 2, 0, 2 },
+    { 1, 1, 0, 0, 2, 0, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
+    { 1, 1, 0, 2, 0, 2, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
+    { 1, 1, 0, 1, 1, 2, 0, 2, 2 }, { 0, 2, 0, 2, 0, 2, 1, 1, 2 },
+    { 0, 2, 0, 2, 0, 2, 0, 2, 2 }, { 0, 2, 0, 1, 1, 2, 0, 2, 2 }
+  };
+
+  int combination[18][3] = {
+    { 1, 2, 4 }, { 1, 2, 5 }, { 1, 2, 6 }, { 1, 3, 4 },
+    { 1, 3, 5 }, { 1, 3, 6 }, { 1, 4, 5 }, { 1, 4, 6 },
+    { 1, 5, 6 }, { 2, 3, 4 }, { 2, 3, 5 }, { 2, 3, 6 },
+    { 2, 4, 5 }, { 2, 4, 6 }, { 2, 5, 6 }, { 3, 4, 5 },
+    { 3, 4, 6 }, { 3, 5, 6 }
+  };
+
+  for (int i = 0; i < 18; ++i) {
+    double matrix[9], independent_term[3];
+    cv::Mat M = cv::Mat(3, 3, CV_64F, matrix);
+    cv::Mat I = cv::Mat(3, 1, CV_64F, independent_term);
+    cv::Mat S = cv::Mat(1, 3, CV_64F);
+
+    for (int j = 0; j < 9; ++j) matrix[j] = (double)matrix_to_resolve[i][j];
+
+    independent_term[0] = std::log( std::abs( betas[ combination[i][0]-1 ] ) );
+    independent_term[1] = std::log( std::abs( betas[ combination[i][1]-1 ] ) );
+    independent_term[2] = std::log( std::abs( betas[ combination[i][2]-1 ] ) );
+
+    cv::exp( cv::Mat(M.inv() * I), S);
+
+    solutions[i][0] = S.at<double>(0);
+    solutions[i][1] = S.at<double>(1) * sign( betas[1] );
+    solutions[i][2] = std::abs( S.at<double>(2) );
+  }
+}
+
 void upnp::gauss_newton(const CvMat * L_6x12, const CvMat * Rho, double betas[4], double * f)
 {
-  const int iterations_number = 100;
+  const int iterations_number = 50;
 
   double a[6*4], b[6], x[4];
   CvMat A = cvMat(6, 4, CV_64F, a);
@@ -476,6 +533,8 @@ void upnp::gauss_newton(const CvMat * L_6x12, const CvMat * Rho, double betas[4]
   }
 
   if (f[0] < 0) f[0] = -f[0];
+    fu = fv = f[0];
+
 }
 
 void upnp::compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
@@ -587,6 +646,11 @@ double upnp::dotZ(const double * v1, const double * v2)
   return v1[2] * v2[2];
 }
 
+double upnp::sign(const double v)
+{
+  return ( v < 0 ) ? -1. : ( v > 0 ) ? 1. : 0.;
+}
+
 void upnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
 {
   const int nr = A->rows;

modules/calib3d/src/upnp.h

@@ -41,7 +41,7 @@ private:
       void choose_control_points();
       void compute_alphas();
       void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
-      void compute_ccs(const double * betas, const double * f, const double * ut);
+      void compute_ccs(const double * betas, const double * ut);
       void compute_pcs(void);
 
       void solve_for_sign(void);
@@ -52,7 +52,9 @@ private:
 
       cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1);
       cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2);
+      void generate_all_possible_solutions_for_f_unk(const double betas_[5], double solutions[18][3]);
 
+      double sign(const double v);
       double dot(const double * v1, const double * v2);
       double dotXY(const double * v1, const double * v2);
       double dotZ(const double * v1, const double * v2);
@@ -65,7 +67,7 @@ private:
       void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
                           const double cb[4], CvMat * A, CvMat * b, double const f);
 
-      double compute_R_and_t(const double * ut, const double * betas, const double * efs,
+      double compute_R_and_t(const double * ut, const double * betas,
                    double R[3][3], double t[3]);
 
       void estimate_R_and_t(double R[3][3], double t[3]);