38 epnp(
const cv::Mat& cameraMatrix,
const cv::Mat& opoints,
const cv::Mat& ipoints);
51 void add_correspondence(
const double X,
const double Y,
const double Z,
52 const double u,
const double v);
59 void compute_pose(cv::Mat& R, cv::Mat&
t);
66 void init_camera_parameters(
const cv::Mat& cameraMatrix)
68 uc = cameraMatrix.at<T> (0, 2);
69 vc = cameraMatrix.at<T> (1, 2);
70 fu = cameraMatrix.at<T> (0, 0);
71 fv = cameraMatrix.at<T> (1, 1);
79 template <
typename Opo
intType,
typename Ipo
intType>
80 void init_points(
const cv::Mat& opoints,
const cv::Mat& ipoints)
82 for(
int i = 0; i < number_of_correspondences; i++)
84 pws[3 * i ] = opoints.at<OpointType>(0,i).
x;
85 pws[3 * i + 1] = opoints.at<OpointType>(0,i).y;
86 pws[3 * i + 2] = opoints.at<OpointType>(0,i).
z;
88 us[2 * i ] = ipoints.at<IpointType>(0,i).
x*fu + uc;
89 us[2 * i + 1] = ipoints.at<IpointType>(0,i).y*fv + vc;
99 double reprojection_error(
const double R[3][3],
const double t[3]);
104 void choose_control_points(
void);
109 void compute_barycentric_coordinates(
void);
119 void fill_M(CvMat * M,
const int row,
const double * alphas,
const double u,
const double v);
126 void compute_ccs(
const double * betas,
const double * ut);
131 void compute_pcs(
void);
137 void solve_for_sign(
void);
145 void find_betas_approx_1(
const CvMat * L_6x10,
const CvMat * Rho,
double * betas);
153 void find_betas_approx_2(
const CvMat * L_6x10,
const CvMat * Rho,
double * betas);
161 void find_betas_approx_3(
const CvMat * L_6x10,
const CvMat * Rho,
double * betas);
169 void qr_solve(CvMat * A, CvMat * b, CvMat * X);
177 double dot(
const double * v1,
const double * v2);
185 double dist2(
const double * p1,
const double * p2);
191 void compute_rho(
double * rho);
198 void compute_L_6x10(
const double * ut,
double * l_6x10);
206 void gauss_newton(
const CvMat * L_6x10,
const CvMat * Rho,
double current_betas[4]);
216 void compute_A_and_b_gauss_newton(
const double * l_6x10,
const double * rho,
217 const double cb[4], CvMat * A, CvMat * b);
227 double compute_R_and_t(
const double * ut,
const double * betas,
228 double R[3][3],
double t[3]);
235 void estimate_R_and_t(
double R[3][3],
double t[3]);
244 void copy_R_and_t(
const double R_dst[3][3],
const double t_dst[3],
245 double R_src[3][3],
double t_src[3]);
253 std::vector<double> pws, us, alphas, pcs;
254 int number_of_correspondences;
256 double cws[4][3], ccs[4][3];
257 double cws_determinant;
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
double A1
UTC constant and 1st order terms.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)