generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Return the estimated focal length

Browse Source
This commit is contained in:
edgarriba 2014-10-02 17:05:42 +02:00
parent ea893bf9d9
commit 7520544840
3 changed files with 6 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
modules/calib3d/src/solvepnp.cpp
View File

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

13
modules/calib3d/src/upnp.cpp
View File

@ -53,7 +53,7 @@ upnp::~upnp()
delete[] A2; 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(); choose_control_points();
compute_alphas(); compute_alphas();
@ -72,7 +72,6 @@ void upnp::compute_pose(cv::Mat& R, cv::Mat& t)
cvMulTransposed(M, &MtM, 1); cvMulTransposed(M, &MtM, 1);
cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
//check_positive_eigenvectors(ut); // same result
cvReleaseMat(&M); cvReleaseMat(&M);
double l_6x12[6 * 12], rho[6]; 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, 1, CV_64F, ts[N]).copyTo(t);
cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R); 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], 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 upnp::compute_R_and_t(const double * ut, const double * betas, const double * efs,
double R[3][3], double t[3]) double R[3][3], double t[3])
{ {

3
modules/calib3d/src/upnp.h
View File

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