Return the estimated focal length

modules/calib3d/src/solvepnp.cpp

@@ -113,8 +113,9 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
         upnp PnP(cameraMatrix, opoints, ipoints);
 
         cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
-		PnP.compute_pose(R, tvec);
+		double f = PnP.compute_pose(R, tvec);
 		cv::Rodrigues(R, rvec);
+		cameraMatrix.at<double>(0,0) = cameraMatrix.at<double>(1,1) = f;
 		return true;
     }
     else

modules/calib3d/src/upnp.cpp

@@ -53,7 +53,7 @@ upnp::~upnp()
         delete[] A2;
 }
 
-void upnp::compute_pose(cv::Mat& R, cv::Mat& t)
+double upnp::compute_pose(cv::Mat& R, cv::Mat& t)
 {
 	choose_control_points();
 	compute_alphas();
@@ -72,7 +72,6 @@ void upnp::compute_pose(cv::Mat& R, cv::Mat& t)
 
 	cvMulTransposed(M, &MtM, 1);
 	cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
-	//check_positive_eigenvectors(ut); // same result
 	cvReleaseMat(&M);
 
     double l_6x12[6 * 12], rho[6];
@@ -99,6 +98,8 @@ void upnp::compute_pose(cv::Mat& R, cv::Mat& t)
 
 	cv::Mat(3, 1, CV_64F, ts[N]).copyTo(t);
 	cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
+
+	return Efs[N][0];
 }
 
 void upnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
@@ -186,14 +187,6 @@ void upnp::solve_for_sign(void)
   }
 }
 
-void upnp::check_positive_eigenvectors(double * ut)
-{
-	for (int i = 0; i < 12; ++i)
-		if (ut[12 * i] < 0.0) {
-			for (int j = 0; j < 12; ++j)ut[12 * i + j] = -ut[12 * i + j];
-		}
-}
-
 double upnp::compute_R_and_t(const double * ut, const double * betas, const double * efs,
            double R[3][3], double t[3])
 {

modules/calib3d/src/upnp.h

@@ -13,7 +13,7 @@ public:
     upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
     ~upnp();
 
-    void compute_pose(cv::Mat& R, cv::Mat& t);
+    double compute_pose(cv::Mat& R, cv::Mat& t);
 private:
     template <typename T>
       void init_camera_parameters(const cv::Mat& cameraMatrix)
@@ -45,7 +45,6 @@ private:
       void compute_pcs(void);
 
       void solve_for_sign(void);
-      void check_positive_eigenvectors(double * ut);
 
       void find_betas_and_focal_approx_1(const CvMat * Ut, const CvMat * Rho, double * betas, double * efs);
       void find_betas_and_focal_approx_2(const CvMat * Ut, const CvMat * Rho, double * betas, double * efs);