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
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();
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];
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],
}
}
-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])
{
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)
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);