48 #ifndef OPENCV_CALIB3D_UPNP_H_ 49 #define OPENCV_CALIB3D_UPNP_H_ 51 #include <mrpt/config.h> 80 upnp(
const cv::Mat& cameraMatrix,
const cv::Mat& opoints,
const cv::Mat& ipoints);
91 double compute_pose(cv::Mat& R, cv::Mat&
t);
99 void init_camera_parameters(
const cv::Mat& cameraMatrix)
101 uc = cameraMatrix.at<T> (0, 2);
102 vc = cameraMatrix.at<T> (1, 2);
112 template <
typename Opo
intType,
typename Ipo
intType>
113 void init_points(
const cv::Mat& opoints,
const cv::Mat& ipoints)
115 for(
int i = 0; i < number_of_correspondences; i++)
117 pws[3 * i ] = opoints.at<OpointType>(i).
x;
118 pws[3 * i + 1] = opoints.at<OpointType>(i).y;
119 pws[3 * i + 2] = opoints.at<OpointType>(i).
z;
121 us[2 * i ] = ipoints.at<IpointType>(i).
x;
122 us[2 * i + 1] = ipoints.at<IpointType>(i).y;
132 double reprojection_error(
const double R[3][3],
const double t[3]);
137 void choose_control_points();
142 void compute_alphas();
152 void fill_M(cv::Mat * M,
const int row,
const double * alphas,
const double u,
const double v);
159 void compute_ccs(
const double * betas,
const double * ut);
164 void compute_pcs(
void);
169 void solve_for_sign(
void);
178 void find_betas_and_focal_approx_1(cv::Mat * Ut, cv::Mat * Rho,
double * betas,
double * efs);
187 void find_betas_and_focal_approx_2(cv::Mat * Ut, cv::Mat * Rho,
double * betas,
double * efs);
195 void qr_solve(cv::Mat * A, cv::Mat * b, cv::Mat * X);
202 cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(
const cv::Mat& M1);
210 cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(
const cv::Mat& M1,
const cv::Mat& M2);
217 void generate_all_possible_solutions_for_f_unk(
const double betas[5],
double solutions[18][3]);
224 double sign(
const double v);
232 double dot(
const double * v1,
const double * v2);
240 double dotXY(
const double * v1,
const double * v2);
248 double dotZ(
const double * v1,
const double * v2);
256 double dist2(
const double * p1,
const double * p2);
262 void compute_rho(
double * rho);
269 void compute_L_6x12(
const double * ut,
double * l_6x12);
279 void gauss_newton(
const cv::Mat * L_6x12,
const cv::Mat * Rho,
double current_betas[4],
double * efs);
290 void compute_A_and_b_gauss_newton(
const double * l_6x12,
const double * rho,
291 const double cb[4], cv::Mat * A, cv::Mat * b,
double const f);
301 double compute_R_and_t(
const double * ut,
const double * betas,
302 double R[3][3],
double t[3]);
309 void estimate_R_and_t(
double R[3][3],
double t[3]);
318 void copy_R_and_t(
const double R_dst[3][3],
const double t_dst[3],
319 double R_src[3][3],
double t_src[3]);
327 std::vector<double> pws;
328 std::vector<double> us;
329 std::vector<double> alphas, pcs;
330 int number_of_correspondences;
332 double cws[4][3], ccs[4][3];
342 #endif // Check for OPENCV_LIB 343 #endif // OPENCV_CALIB3D_UPNP_H_ EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
int sign(T x)
Returns the sign of X as "1" or "-1".
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)