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

Detailed Description

Author
Chandra Mangipudi
Date
11/08/16

Definition at line 29 of file epnp.h.

#include <pnp/epnp.h>

Public Member Functions

 epnp (const cv::Mat &cameraMatrix, const cv::Mat &opoints, const cv::Mat &ipoints)
 Constructor for EPnP class. More...
 
 ~epnp ()
 Destructor for EPnP class. More...
 
void add_correspondence (const double X, const double Y, const double Z, const double u, const double v)
 Add a 2d/3d correspondence. More...
 
void compute_pose (cv::Mat &R, cv::Mat &t)
 OpenCV wrapper to compute pose. More...
 

Private Member Functions

template<typename T >
void init_camera_parameters (const cv::Mat &cameraMatrix)
 Initialize Camera Matrix. More...
 
template<typename OpointType , typename IpointType >
void init_points (const cv::Mat &opoints, const cv::Mat &ipoints)
 Convert object points and image points from OpenCV format to STL matrices. More...
 
double reprojection_error (const double R[3][3], const double t[3])
 Function to compute reprojection error. More...
 
void choose_control_points ()
 Function to select 4 control points from n points. More...
 
void compute_barycentric_coordinates ()
 Convert from object space to relative object space (Barycentric coordinates) More...
 
void fill_M (CvMat *M, const int row, const double *alphas, const double u, const double v)
 Generate the Matrix M. More...
 
void compute_ccs (const double *betas, const double *ut)
 Internal function. More...
 
void compute_pcs ()
 Internal function. More...
 
void solve_for_sign ()
 Internal function. More...
 
void find_betas_approx_1 (const CvMat *L_6x10, const CvMat *Rho, double *betas)
 Internal function. More...
 
void find_betas_approx_2 (const CvMat *L_6x10, const CvMat *Rho, double *betas)
 Internal function. More...
 
void find_betas_approx_3 (const CvMat *L_6x10, const CvMat *Rho, double *betas)
 Internal function. More...
 
void qr_solve (CvMat *A, CvMat *b, CvMat *X)
 QR optimization algorithm. More...
 
double dot (const double *v1, const double *v2)
 Dot product of two OpenCV vectors. More...
 
double dist2 (const double *p1, const double *p2)
 Squared distance between two vectors. More...
 
void compute_rho (double *rho)
 Get distances between all object points taken 2 at a time(nC2) More...
 
void compute_L_6x10 (const double *ut, double *l_6x10)
 Internal function. More...
 
void gauss_newton (const CvMat *L_6x10, const CvMat *Rho, double current_betas[4])
 Gauss Newton iterative algorithm. More...
 
void compute_A_and_b_gauss_newton (const double *l_6x10, const double *rho, const double cb[4], CvMat *A, CvMat *b)
 Internal function. More...
 
double compute_R_and_t (const double *ut, const double *betas, double R[3][3], double t[3])
 Function to compute pose. More...
 
void estimate_R_and_t (double R[3][3], double t[3])
 Helper function to 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])
 Copy function of output result. 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
 
std::vector< double > alphas
 
std::vector< double > pcs
 
int number_of_correspondences
 Internal member variables. More...
 
double cws [4][3]
 Number of 2d/3d correspondences. More...
 
double ccs [4][3]
 
double cws_determinant
 Internal member variables. More...
 
int max_nr
 Internal member variable. More...
 
double * A1
 Internal member variable. More...
 
double * A2
 

Constructor & Destructor Documentation

◆ epnp()

mrpt::vision::pnp::epnp::epnp ( const cv::Mat &  cameraMatrix,
const cv::Mat &  opoints,
const cv::Mat &  ipoints 
)

Constructor for EPnP class.

Definition at line 20 of file epnp.cpp.

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

◆ ~epnp()

mrpt::vision::pnp::epnp::~epnp ( )

Destructor for EPnP class.

Definition at line 54 of file epnp.cpp.

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

Member Function Documentation

◆ add_correspondence()

void mrpt::vision::pnp::epnp::add_correspondence ( const double  X,
const double  Y,
const double  Z,
const double  u,
const double  v 
)

Add a 2d/3d correspondence.

Parameters
[in]XX coordinate in Camera coordinate system
[in]YY coordinate in Camera coordinate system
[in]ZZ coordinate in Camera coordinate system
[in]uImage pixel coordinate u in x axis
[in]vImage pixel coordinate v in y axis

◆ choose_control_points()

void mrpt::vision::pnp::epnp::choose_control_points ( )
private

Function to select 4 control points from n points.

Definition at line 60 of file epnp.cpp.

◆ compute_A_and_b_gauss_newton()

void mrpt::vision::pnp::epnp::compute_A_and_b_gauss_newton ( const double *  l_6x10,
const double *  rho,
const double  cb[4],
CvMat *  A,
CvMat *  b 
)
private

Internal function.

Parameters
[in]l_6x10
[in]rho
[in]cb
[out]A
[out]b

Definition at line 513 of file epnp.cpp.

◆ compute_barycentric_coordinates()

void mrpt::vision::pnp::epnp::compute_barycentric_coordinates ( )
private

Convert from object space to relative object space (Barycentric coordinates)

Definition at line 94 of file epnp.cpp.

◆ compute_ccs()

void mrpt::vision::pnp::epnp::compute_ccs ( const double *  betas,
const double *  ut 
)
private

Internal function.

Parameters
[in]betas
[in]ut

Definition at line 136 of file epnp.cpp.

◆ compute_L_6x10()

void mrpt::vision::pnp::epnp::compute_L_6x10 ( const double *  ut,
double *  l_6x10 
)
private

Internal function.

Parameters
[in]ut
[out]l_6x10

Definition at line 457 of file epnp.cpp.

◆ compute_pcs()

void mrpt::vision::pnp::epnp::compute_pcs ( )
private

Internal function.

Definition at line 148 of file epnp.cpp.

◆ compute_pose()

void mrpt::vision::pnp::epnp::compute_pose ( cv::Mat &  R,
cv::Mat &  t 
)

OpenCV wrapper to compute pose.

Parameters
[out]RRotation Matrix
[out]tTranslation Vector

Definition at line 161 of file epnp.cpp.

References R.

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

Here is the caller graph for this function:

◆ compute_R_and_t()

double mrpt::vision::pnp::epnp::compute_R_and_t ( const double *  ut,
const double *  betas,
double  R[3][3],
double  t[3] 
)
private

Function to compute pose.

Parameters
[in]ut
[in]betas
[out]R
[out]t
Returns

Definition at line 315 of file epnp.cpp.

References R.

◆ compute_rho()

void mrpt::vision::pnp::epnp::compute_rho ( double *  rho)
private

Get distances between all object points taken 2 at a time(nC2)

Parameters
rho

Definition at line 503 of file epnp.cpp.

◆ copy_R_and_t()

void mrpt::vision::pnp::epnp::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

Copy function of output result.

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

Definition at line 210 of file epnp.cpp.

◆ dist2()

double mrpt::vision::pnp::epnp::dist2 ( const double *  p1,
const double *  p2 
)
private

Squared distance between two vectors.

Parameters
[in]p1
[in]p2
Returns

Definition at line 221 of file epnp.cpp.

◆ dot()

double mrpt::vision::pnp::epnp::dot ( const double *  v1,
const double *  v2 
)
private

Dot product of two OpenCV vectors.

Parameters
[in]v1
[in]v2
Returns

Definition at line 228 of file epnp.cpp.

◆ estimate_R_and_t()

void mrpt::vision::pnp::epnp::estimate_R_and_t ( double  R[3][3],
double  t[3] 
)
private

Helper function to compute_R_and_t()

Parameters
R
t

Definition at line 233 of file epnp.cpp.

References R.

◆ fill_M()

void mrpt::vision::pnp::epnp::fill_M ( CvMat *  M,
const int  row,
const double *  alphas,
const double  u,
const double  v 
)
private

Generate the Matrix M.

Parameters
[out]M
[in]row
[in]alphas
[in]u
[in]v

Definition at line 118 of file epnp.cpp.

◆ find_betas_approx_1()

void mrpt::vision::pnp::epnp::find_betas_approx_1 ( const CvMat *  L_6x10,
const CvMat *  Rho,
double *  betas 
)
private

Internal function.

Parameters
[out]L_6x10
[in]Rho
[in]betas

Definition at line 352 of file epnp.cpp.

◆ find_betas_approx_2()

void mrpt::vision::pnp::epnp::find_betas_approx_2 ( const CvMat *  L_6x10,
const CvMat *  Rho,
double *  betas 
)
private

Internal function.

Parameters
[out]L_6x10
[in]Rho
[in]betas

Definition at line 388 of file epnp.cpp.

References mrpt::obs::gnss::b3.

◆ find_betas_approx_3()

void mrpt::vision::pnp::epnp::find_betas_approx_3 ( const CvMat *  L_6x10,
const CvMat *  Rho,
double *  betas 
)
private

Internal function.

Parameters
[out]L_6x10
[in]Rho
[in]betas

Definition at line 424 of file epnp.cpp.

◆ gauss_newton()

void mrpt::vision::pnp::epnp::gauss_newton ( const CvMat *  L_6x10,
const CvMat *  Rho,
double  current_betas[4] 
)
private

Gauss Newton iterative algorithm.

Parameters
[in]L_6x10
[in]Rho
[in,out]current_betas

Definition at line 543 of file epnp.cpp.

◆ init_camera_parameters()

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

Initialize Camera Matrix.

Parameters
[in]cameraMatrixCamera Intrinsic matrix as a OpenCV Matrix

Definition at line 65 of file epnp.h.

References fu, fv, uc, and vc.

◆ init_points()

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

Convert object points and image points from OpenCV format to STL matrices.

Parameters
opointsObject points in Camera coordinate system
ipointsImate points in pixel coordinates

Definition at line 80 of file epnp.h.

References fu, fv, number_of_correspondences, pws, uc, us, and vc.

◆ qr_solve()

void mrpt::vision::pnp::epnp::qr_solve ( CvMat *  A,
CvMat *  b,
CvMat *  X 
)
private

QR optimization algorithm.

Parameters
[in]A
[out]b
[out]X

Definition at line 562 of file epnp.cpp.

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

Here is the call graph for this function:

◆ reprojection_error()

double mrpt::vision::pnp::epnp::reprojection_error ( const double  R[3][3],
const double  t[3] 
)
private

Function to compute reprojection error.

Parameters
RRotation Matrix
tTranslation Vector
Returns

Definition at line 328 of file epnp.cpp.

References R.

◆ solve_for_sign()

void mrpt::vision::pnp::epnp::solve_for_sign ( )
private

Internal function.

Definition at line 299 of file epnp.cpp.

Member Data Documentation

◆ A1

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

Internal member variable.

Definition at line 266 of file epnp.h.

◆ A2

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

Definition at line 266 of file epnp.h.

◆ alphas

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

Definition at line 260 of file epnp.h.

◆ ccs

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

Definition at line 263 of file epnp.h.

◆ cws

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

Number of 2d/3d correspondences.

Definition at line 263 of file epnp.h.

◆ cws_determinant

double mrpt::vision::pnp::epnp::cws_determinant
private

Internal member variables.

Definition at line 264 of file epnp.h.

◆ fu

double mrpt::vision::pnp::epnp::fu
private

Image center in y-direction.

Definition at line 257 of file epnp.h.

Referenced by init_camera_parameters(), and init_points().

◆ fv

double mrpt::vision::pnp::epnp::fv
private

Focal length in x-direction.

Definition at line 258 of file epnp.h.

Referenced by init_camera_parameters(), and init_points().

◆ max_nr

int mrpt::vision::pnp::epnp::max_nr
private

Internal member variable.

Definition at line 265 of file epnp.h.

◆ number_of_correspondences

int mrpt::vision::pnp::epnp::number_of_correspondences
private

Internal member variables.

Definition at line 261 of file epnp.h.

Referenced by init_points().

◆ pcs

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

Definition at line 260 of file epnp.h.

◆ pws

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

Focal length in y-direction.

Definition at line 260 of file epnp.h.

Referenced by init_points().

◆ uc

double mrpt::vision::pnp::epnp::uc
private

Definition at line 255 of file epnp.h.

Referenced by init_camera_parameters(), and init_points().

◆ us

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

Definition at line 260 of file epnp.h.

Referenced by init_points().

◆ vc

double mrpt::vision::pnp::epnp::vc
private

Image center in x-direction.

Definition at line 256 of file epnp.h.

Referenced by init_camera_parameters(), and init_points().




Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020