Return the estimated focal length

diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index 2fb8a8d..5a7a241 100644 (file)
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/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

diff --git a/modules/calib3d/src/upnp.cpp b/modules/calib3d/src/upnp.cpp
index 6683681..440471a 100644 (file)
--- a/modules/calib3d/src/upnp.cpp
+++ b/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])
 {

diff --git a/modules/calib3d/src/upnp.h b/modules/calib3d/src/upnp.h
index fce22bf..39d21bf 100644 (file)
--- a/modules/calib3d/src/upnp.h
+++ b/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);