MRPT  2.0.4
List of all members | Public Member Functions | Private Member Functions | Private Attributes
mrpt::vision::pnp::upnp Class Reference

Detailed Description

Author
Chandra Mangipudi
Date
12/08/16

Definition at line 85 of file upnp.h.

#include <pnp/upnp.h>

Public Member Functions

 upnp (const cv::Mat &cameraMatrix, const cv::Mat &opoints, const cv::Mat &ipoints)
 Constructor for UPnP class. More...
 
 ~upnp ()
 Destructor for UPnP class. More...
 
double compute_pose (cv::Mat &R, cv::Mat &t)
 Function to compute pose. More...
 

Private Member Functions

template<typename T >
void init_camera_parameters (const cv::Mat &cameraMatrix)
 Initialize camera variables using camera intrinsic matrix. More...
 
template<typename OpointType , typename IpointType >
void init_points (const cv::Mat &opoints, const cv::Mat &ipoints)
 Iniialize Object points and image points from OpenCV Matrix. More...
 
double reprojection_error (const double R[3][3], const double t[3])
 Compute the reprojection error using the estimated Rotation matrix and Translation Vector. More...
 
void choose_control_points ()
 Function to select 4 control points. More...
 
void compute_alphas ()
 Function to comput . More...
 
void fill_M (cv::Mat *M, const int row, const double *alphas, const double u, const double v)
 Function to compute Maucaulay matrix M. More...
 
void compute_ccs (const double *betas, const double *ut)
 Compute the control points. More...
 
void compute_pcs ()
 Compute object points based on control points. More...
 
void solve_for_sign ()
 Internal member function. More...
 
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. More...
 
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) More...
 
void qr_solve (cv::Mat *A, cv::Mat *b, cv::Mat *X)
 Function to do a QR decomposition. More...
 
cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk (const cv::Mat &M1)
 Internal function. More...
 
cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk (const cv::Mat &M1, const cv::Mat &M2)
 Internal function. More...
 
void generate_all_possible_solutions_for_f_unk (const double betas[5], double solutions[18][3])
 Get all possible solutions. More...
 
double sign (const double v)
 Return the sign of the scalar. More...
 
double dot (const double *v1, const double *v2)
 Compute the dot product between two vectors. More...
 
double dotXY (const double *v1, const double *v2)
 Compute dot product in 2D with only x and y components. More...
 
double dotZ (const double *v1, const double *v2)
 Compute the dot product using only z component. More...
 
double dist2 (const double *p1, const double *p2)
 Compute the euclidean distance squared between two points in 3D. More...
 
void compute_rho (double *rho)
 Internal fucntion. More...
 
void compute_L_6x12 (const double *ut, double *l_6x12)
 Internal function. More...
 
void gauss_newton (const cv::Mat *L_6x12, const cv::Mat *Rho, double current_betas[4], double *efs)
 Gauss Newton Iterative optimization. More...
 
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. More...
 
double compute_R_and_t (const double *ut, const double *betas, double R[3][3], double t[3])
 Function to compute the pose. More...
 
void estimate_R_and_t (double R[3][3], double t[3])
 Helper function to function compute_R_and_t() More...
 
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. More...
 

Private Attributes

double uc
 
double vc
 Image center in x-direction. More...
 
double fu
 Image center in y-direction. More...
 
double fv
 Focal length in x-direction. More...
 
std::vector< double > pws
 Focal length in y-direction. More...
 
std::vector< double > us
 Object points. More...
 
std::vector< double > alphas
 Image points. More...
 
std::vector< double > pcs
 
int number_of_correspondences
 Internal variable. More...
 
double cws [4][3]
 Number of 2d/3d correspondences. More...
 
double ccs [4][3]
 
int max_nr
 Control point variables. More...
 
double * A1
 Internal variable. More...
 
double * A2
 

Constructor & Destructor Documentation

◆ upnp()

upnp::upnp ( const cv::Mat &  cameraMatrix,
const cv::Mat &  opoints,
const cv::Mat &  ipoints 
)

Constructor for UPnP class.

Definition at line 77 of file upnp.cpp.

References mrpt::obs::gnss::A1.

◆ ~upnp()

upnp::~upnp ( )

Destructor for UPnP class.

Definition at line 110 of file upnp.cpp.

References mrpt::obs::gnss::A1.

Member Function Documentation

◆ choose_control_points()

void upnp::choose_control_points ( )
private

Function to select 4 control points.

Definition at line 295 of file upnp.cpp.

◆ compute_A_and_b_gauss_newton()

void upnp::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 
)
private

Compute matrix A and vector b.

Parameters
[in]l_6x12
[in]rho
[in]cb
[out]A
[out]b
[in]f

Definition at line 675 of file upnp.cpp.

◆ compute_alphas()

void upnp::compute_alphas ( )
private

Function to comput .

Definition at line 301 of file upnp.cpp.

◆ compute_ccs()

void upnp::compute_ccs ( const double *  betas,
const double *  ut 
)
private

Compute the control points.

Parameters
[in]betasInternal member variable
[in]utInternal member variable

Definition at line 337 of file upnp.cpp.

◆ compute_constraint_distance_2param_6eq_2unk_f_unk()

Mat upnp::compute_constraint_distance_2param_6eq_2unk_f_unk ( const cv::Mat &  M1)
private

Internal function.

Parameters
[in]M1
Returns
Distance

Definition at line 437 of file upnp.cpp.

◆ compute_constraint_distance_3param_6eq_6unk_f_unk()

Mat upnp::compute_constraint_distance_3param_6eq_6unk_f_unk ( const cv::Mat &  M1,
const cv::Mat &  M2 
)
private

Internal function.

Parameters
[in]M1
[in]M2
Returns
Distance

Definition at line 479 of file upnp.cpp.

◆ compute_L_6x12()

void upnp::compute_L_6x12 ( const double *  ut,
double *  l_6x12 
)
private

Internal function.

Parameters
[in]ut
[out]l_6x12

Definition at line 719 of file upnp.cpp.

◆ compute_pcs()

void upnp::compute_pcs ( )
private

Compute object points based on control points.

Definition at line 352 of file upnp.cpp.

◆ compute_pose()

double upnp::compute_pose ( cv::Mat &  R,
cv::Mat &  t 
)

Function to compute pose.

Parameters
[out]RRotation Matrix
[out]tTranslation Vector
Returns

Definition at line 116 of file upnp.cpp.

References R.

Referenced by mrpt::vision::pnp::CPnP::upnp().

Here is the caller graph for this function:

◆ compute_R_and_t()

double upnp::compute_R_and_t ( const double *  ut,
const double *  betas,
double  R[3][3],
double  t[3] 
)
private

Function to compute the pose.

Parameters
[in]ut
[in]betas
[out]RRotation Matrix
[out]tTranslation VectorS
Returns

Definition at line 262 of file upnp.cpp.

References R.

◆ compute_rho()

void upnp::compute_rho ( double *  rho)
private

Internal fucntion.

Parameters
[out]rho

Definition at line 767 of file upnp.cpp.

◆ copy_R_and_t()

void upnp::copy_R_and_t ( const double  R_dst[3][3],
const double  t_dst[3],
double  R_src[3][3],
double  t_src[3] 
)
private

Function to copy the pose.

Parameters
[in]R_dst
[in]t_dst
[out]R_src
[out]t_src

Definition at line 168 of file upnp.cpp.

◆ dist2()

double upnp::dist2 ( const double *  p1,
const double *  p2 
)
private

Compute the euclidean distance squared between two points in 3D.

Parameters
[in]p1
[in]p2
Returns
Distance squared

Definition at line 777 of file upnp.cpp.

◆ dot()

double upnp::dot ( const double *  v1,
const double *  v2 
)
private

Compute the dot product between two vectors.

Parameters
[in]v1
[in]v2
Returns
Dot product

Definition at line 784 of file upnp.cpp.

◆ dotXY()

double upnp::dotXY ( const double *  v1,
const double *  v2 
)
private

Compute dot product in 2D with only x and y components.

Parameters
[in]v1
[in]v2
Returns
Dot product

Definition at line 789 of file upnp.cpp.

◆ dotZ()

double upnp::dotZ ( const double *  v1,
const double *  v2 
)
private

Compute the dot product using only z component.

Parameters
[in]v1
[in]v2
Returns
Dot product

Definition at line 794 of file upnp.cpp.

◆ estimate_R_and_t()

void upnp::estimate_R_and_t ( double  R[3][3],
double  t[3] 
)
private

Helper function to function compute_R_and_t()

Parameters
[out]RRotaiton matrix
[out]tTranslation vector

Definition at line 179 of file upnp.cpp.

References R.

◆ fill_M()

void upnp::fill_M ( cv::Mat *  M,
const int  row,
const double *  alphas,
const double  u,
const double  v 
)
private

Function to compute Maucaulay matrix M.

Parameters
[out]MMaucaulay matrix
[in]rowInternal member
[in]alphasInternal member
[in]uImage pixel x co-ordinate
[in]vImage pixel y co-ordinate

Definition at line 319 of file upnp.cpp.

◆ find_betas_and_focal_approx_1()

void upnp::find_betas_and_focal_approx_1 ( cv::Mat *  Ut,
cv::Mat *  Rho,
double *  betas,
double *  efs 
)
private

Function to approximately calculate betas and focal length.

Parameters
[in]Ut
[in]Rho
[out]betas
[out]efs

Definition at line 365 of file upnp.cpp.

◆ find_betas_and_focal_approx_2()

void upnp::find_betas_and_focal_approx_2 ( cv::Mat *  Ut,
cv::Mat *  Rho,
double *  betas,
double *  efs 
)
private

Function to calculate betas and focal length (more accurate)

Parameters
[in]Ut
[in]Rho
[out]betas
[out]efs

Definition at line 386 of file upnp.cpp.

◆ gauss_newton()

void upnp::gauss_newton ( const cv::Mat *  L_6x12,
const cv::Mat *  Rho,
double  current_betas[4],
double *  efs 
)
private

Gauss Newton Iterative optimization.

Parameters
[in]L_6x12
[in]Rho
[in,out]current_betas
[out]efs

Definition at line 643 of file upnp.cpp.

◆ generate_all_possible_solutions_for_f_unk()

void upnp::generate_all_possible_solutions_for_f_unk ( const double  betas[5],
double  solutions[18][3] 
)
private

Get all possible solutions.

Parameters
[in]betas
[out]solutions

Definition at line 603 of file upnp.cpp.

References mrpt::sign().

Here is the call graph for this function:

◆ init_camera_parameters()

template<typename T >
void mrpt::vision::pnp::upnp::init_camera_parameters ( const cv::Mat &  cameraMatrix)
inlineprivate

Initialize camera variables using camera intrinsic matrix.

Parameters
[in]cameraMatrixCamera Intrinsic Matrix

Definition at line 110 of file upnp.h.

References fu, fv, uc, and vc.

◆ init_points()

template<typename OpointType , typename IpointType >
void mrpt::vision::pnp::upnp::init_points ( const cv::Mat &  opoints,
const cv::Mat &  ipoints 
)
inlineprivate

Iniialize Object points and image points from OpenCV Matrix.

Parameters
[in]opointsObject Points
[in]ipointsImage Points

Definition at line 124 of file upnp.h.

References number_of_correspondences, pws, and us.

◆ qr_solve()

void upnp::qr_solve ( cv::Mat *  A,
cv::Mat *  b,
cv::Mat *  X 
)
private

Function to do a QR decomposition.

Parameters
[in]AMatrix to be decomposed
[out]b
[out]X

Definition at line 800 of file upnp.cpp.

References mrpt::obs::gnss::A1, and mrpt::math::sum().

Here is the call graph for this function:

◆ reprojection_error()

double upnp::reprojection_error ( const double  R[3][3],
const double  t[3] 
)
private

Compute the reprojection error using the estimated Rotation matrix and Translation Vector.

Parameters
[in]RRotation matrix
[in]tTrnaslation Vector
Returns
Linear least squares error in pixels

Definition at line 275 of file upnp.cpp.

References R.

◆ sign()

double upnp::sign ( const double  v)
private

Return the sign of the scalar.

Parameters
[in]v
Returns
True if positive else Flase

Definition at line 795 of file upnp.cpp.

◆ solve_for_sign()

void upnp::solve_for_sign ( )
private

Internal member function.

Definition at line 246 of file upnp.cpp.

Member Data Documentation

◆ A1

double* mrpt::vision::pnp::upnp::A1
private

Internal variable.

Definition at line 355 of file upnp.h.

◆ A2

double * mrpt::vision::pnp::upnp::A2
private

Definition at line 355 of file upnp.h.

◆ alphas

std::vector<double> mrpt::vision::pnp::upnp::alphas
private

Image points.

Definition at line 350 of file upnp.h.

◆ ccs

double mrpt::vision::pnp::upnp::ccs[4][3]
private

Definition at line 353 of file upnp.h.

◆ cws

double mrpt::vision::pnp::upnp::cws[4][3]
private

Number of 2d/3d correspondences.

Definition at line 353 of file upnp.h.

◆ fu

double mrpt::vision::pnp::upnp::fu
private

Image center in y-direction.

Definition at line 345 of file upnp.h.

Referenced by init_camera_parameters().

◆ fv

double mrpt::vision::pnp::upnp::fv
private

Focal length in x-direction.

Definition at line 346 of file upnp.h.

Referenced by init_camera_parameters().

◆ max_nr

int mrpt::vision::pnp::upnp::max_nr
private

Control point variables.

Definition at line 354 of file upnp.h.

◆ number_of_correspondences

int mrpt::vision::pnp::upnp::number_of_correspondences
private

Internal variable.

Definition at line 351 of file upnp.h.

Referenced by init_points().

◆ pcs

std::vector<double> mrpt::vision::pnp::upnp::pcs
private

Definition at line 350 of file upnp.h.

◆ pws

std::vector<double> mrpt::vision::pnp::upnp::pws
private

Focal length in y-direction.

Definition at line 348 of file upnp.h.

Referenced by init_points().

◆ uc

double mrpt::vision::pnp::upnp::uc
private

Definition at line 343 of file upnp.h.

Referenced by init_camera_parameters().

◆ us

std::vector<double> mrpt::vision::pnp::upnp::us
private

Object points.

Definition at line 349 of file upnp.h.

Referenced by init_points().

◆ vc

double mrpt::vision::pnp::upnp::vc
private

Image center in x-direction.

Definition at line 344 of file upnp.h.

Referenced by init_camera_parameters().




Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020