opencv/modules/calib3d/src/epnp.h

87 lines
2.6 KiB
C
Raw Normal View History

2011-12-20 19:33:12 +08:00
#ifndef epnp_h
#define epnp_h
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
2011-12-20 19:33:12 +08:00
namespace cv
{
2011-12-20 19:33:12 +08:00
class epnp {
public:
epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
2011-12-20 19:33:12 +08:00
~epnp();
void add_correspondence(const double X, const double Y, const double Z,
2012-10-17 15:12:04 +08:00
const double u, const double v);
2011-12-20 19:33:12 +08:00
void compute_pose(cv::Mat& R, cv::Mat& t);
2011-12-20 19:33:12 +08:00
private:
template <typename T>
void init_camera_parameters(const cv::Mat& cameraMatrix)
2012-10-17 15:12:04 +08:00
{
uc = cameraMatrix.at<T> (0, 2);
vc = cameraMatrix.at<T> (1, 2);
fu = cameraMatrix.at<T> (0, 0);
fv = cameraMatrix.at<T> (1, 1);
}
template <typename OpointType, typename IpointType>
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
{
2012-10-17 15:12:04 +08:00
for(int i = 0; i < number_of_correspondences; i++)
{
2014-08-25 23:06:17 +08:00
pws[3 * i ] = opoints.at<OpointType>(i).x;
pws[3 * i + 1] = opoints.at<OpointType>(i).y;
pws[3 * i + 2] = opoints.at<OpointType>(i).z;
2012-10-17 15:12:04 +08:00
2014-08-25 23:06:17 +08:00
us[2 * i ] = ipoints.at<IpointType>(i).x*fu + uc;
us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc;
2012-10-17 15:12:04 +08:00
}
}
double reprojection_error(const double R[3][3], const double t[3]);
2011-12-20 19:33:12 +08:00
void choose_control_points(void);
void compute_barycentric_coordinates(void);
void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
void compute_ccs(const double * betas, const double * ut);
void compute_pcs(void);
void solve_for_sign(void);
void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
void qr_solve(CvMat * A, CvMat * b, CvMat * X);
double dot(const double * v1, const double * v2);
double dist2(const double * p1, const double * p2);
void compute_rho(double * rho);
void compute_L_6x10(const double * ut, double * l_6x10);
void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
2012-10-17 15:12:04 +08:00
const double cb[4], CvMat * A, CvMat * b);
2011-12-20 19:33:12 +08:00
double compute_R_and_t(const double * ut, const double * betas,
2012-10-17 15:12:04 +08:00
double R[3][3], double t[3]);
2011-12-20 19:33:12 +08:00
void estimate_R_and_t(double R[3][3], double t[3]);
void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
2012-10-17 15:12:04 +08:00
double R_src[3][3], double t_src[3]);
2011-12-20 19:33:12 +08:00
double uc, vc, fu, fv;
std::vector<double> pws, us, alphas, pcs;
2011-12-20 19:33:12 +08:00
int number_of_correspondences;
double cws[4][3], ccs[4][3];
int max_nr;
double * A1, * A2;
2011-12-20 19:33:12 +08:00
};
}
2011-12-20 19:33:12 +08:00
#endif