diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 2fb8a8dcbe..5a7a2412e6 100644 --- 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(0,0) = cameraMatrix.at(1,1) = f; return true; } else diff --git a/modules/calib3d/src/upnp.cpp b/modules/calib3d/src/upnp.cpp index 66836812c0..440471a1e8 100644 --- 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 fce22bf2f8..39d21bf8ea 100644 --- 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 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);