62 #ifndef OPENCV_CALIB3D_UPNP_H_ 63 #define OPENCV_CALIB3D_UPNP_H_ 65 #include <mrpt/config.h> 66 #include <mrpt/otherlibs/do_opencv_includes.h> 91 const cv::Mat& cameraMatrix,
const cv::Mat& opoints,
92 const cv::Mat& ipoints);
110 template <
typename T>
113 uc = cameraMatrix.at<T>(0, 2);
114 vc = cameraMatrix.at<T>(1, 2);
124 template <
typename Opo
intType,
typename Ipo
intType>
129 pws[3 * i] = opoints.at<OpointType>(i).
x;
130 pws[3 * i + 1] = opoints.at<OpointType>(i).
y;
131 pws[3 * i + 2] = opoints.at<OpointType>(i).
z;
133 us[2 * i] = ipoints.at<IpointType>(i).
x;
134 us[2 * i + 1] = ipoints.at<IpointType>(i).
y;
166 cv::Mat* M,
const int row,
const double*
alphas,
const double u,
174 void compute_ccs(
const double* betas,
const double* ut);
194 cv::Mat* Ut, cv::Mat* Rho,
double* betas,
double* efs);
204 cv::Mat* Ut, cv::Mat* Rho,
double* betas,
double* efs);
212 void qr_solve(cv::Mat* A, cv::Mat*
b, cv::Mat* X);
229 const cv::Mat& M1,
const cv::Mat& M2);
237 const double betas[5],
double solutions[18][3]);
244 double sign(
const double v);
252 double dot(
const double*
v1,
const double*
v2);
260 double dotXY(
const double*
v1,
const double*
v2);
268 double dotZ(
const double*
v1,
const double*
v2);
276 double dist2(
const double* p1,
const double* p2);
299 const cv::Mat* L_6x12,
const cv::Mat* Rho,
double current_betas[4],
312 const double* l_6x12,
const double* rho,
const double cb[4], cv::Mat* A,
313 cv::Mat*
b,
double const f);
324 const double* ut,
const double* betas,
double R[3][3],
double t[3]);
341 const double R_dst[3][3],
const double t_dst[3],
double R_src[3][3],
350 std::vector<double>
us;
361 #endif // Check for OPENCV_LIB 362 #endif // OPENCV_CALIB3D_UPNP_H_ double compute_R_and_t(const double *ut, const double *betas, double R[3][3], double t[3])
Function to compute the pose.
void fill_M(cv::Mat *M, const int row, const double *alphas, const double u, const double v)
Function to compute Maucaulay matrix M.
void init_points(const cv::Mat &opoints, const cv::Mat &ipoints)
Iniialize Object points and image points from OpenCV Matrix.
void estimate_R_and_t(double R[3][3], double t[3])
Helper function to function compute_R_and_t()
void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], double R_src[3][3], double t_src[3])
Function to copy the pose.
Perspective n Point (PnP) Algorithms toolkit for MRPT mrpt_vision_grp.
std::vector< double > pws
Focal length in y-direction.
double cws[4][3]
Number of 2d/3d correspondences.
void compute_pcs(void)
Compute object points based on control points.
void compute_ccs(const double *betas, const double *ut)
Compute the control points.
void compute_alphas()
Function to comput .
std::vector< double > pcs
int number_of_correspondences
Internal variable.
double dotXY(const double *v1, const double *v2)
Compute dot product in 2D with only x and y components.
void find_betas_and_focal_approx_1(cv::Mat *Ut, cv::Mat *Rho, double *betas, double *efs)
Function to approximately calculate betas and focal length.
double compute_pose(cv::Mat &R, cv::Mat &t)
Function to compute pose.
std::vector< double > alphas
Image points.
void qr_solve(cv::Mat *A, cv::Mat *b, cv::Mat *X)
Function to do a QR decomposition.
double dot(const double *v1, const double *v2)
Compute the dot product between two vectors.
double dotZ(const double *v1, const double *v2)
Compute the dot product using only z component.
upnp(const cv::Mat &cameraMatrix, const cv::Mat &opoints, const cv::Mat &ipoints)
Constructor for UPnP class.
void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])
Get all possible solutions.
double dist2(const double *p1, const double *p2)
Compute the euclidean distance squared between two points in 3D.
cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat &M1, const cv::Mat &M2)
Internal function.
double sign(const double v)
Return the sign of the scalar.
void compute_A_and_b_gauss_newton(const double *l_6x12, const double *rho, const double cb[4], cv::Mat *A, cv::Mat *b, double const f)
Compute matrix A and vector b.
void init_camera_parameters(const cv::Mat &cameraMatrix)
Initialize camera variables using camera intrinsic matrix.
std::vector< double > us
Object points.
GLenum GLenum GLvoid * row
void find_betas_and_focal_approx_2(cv::Mat *Ut, cv::Mat *Rho, double *betas, double *efs)
Function to calculate betas and focal length (more accurate)
int max_nr
Control point variables.
double vc
Image center in x-direction.
~upnp()
Destructor for UPnP class.
GLfloat GLfloat GLfloat v2
cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat &M1)
Internal function.
void compute_rho(double *rho)
Internal fucntion.
void compute_L_6x12(const double *ut, double *l_6x12)
Internal function.
double reprojection_error(const double R[3][3], const double t[3])
Compute the reprojection error using the estimated Rotation matrix and Translation Vector...
void choose_control_points()
Function to select 4 control points.
void solve_for_sign(void)
Internal member function.
double * A1
Internal variable.
double fu
Image center in y-direction.
void gauss_newton(const cv::Mat *L_6x12, const cv::Mat *Rho, double current_betas[4], double *efs)
Gauss Newton Iterative optimization.
double fv
Focal length in x-direction.