Detailed Description

Author: Chandra Mangipudi

Date: 11/08/16

Definition at line 34 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 (void)
	Function to select 4 control points from n points. More...

void	compute_barycentric_coordinates (void)
	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 (void)
	Internal function. More...

void	solve_for_sign (void)
	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 @func 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.

◆ ~epnp()

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

Destructor for EPnP class.

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]	X	X coordinate in Camera coordinate system
[in]	Y	Y coordinate in Camera coordinate system
[in]	Z	Z coordinate in Camera coordinate system
[in]	u	Image pixel coordinate u in x axis
[in]	v	Image pixel coordinate v in y axis

◆ choose_control_points()

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

private

Function to select 4 control points from n points.

◆ 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

◆ compute_barycentric_coordinates()

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

private

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

◆ compute_ccs()

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

private

Internal function.

Parameters

[in]	betas
[in]	ut

◆ 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

◆ compute_pcs()

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

private

Internal function.

◆ compute_pose()

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

OpenCV wrapper to compute pose.

Parameters

[out]	R	Rotation Matrix
[out]	t	Translation Vector

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

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

◆ estimate_R_and_t()

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

private

Helper function to @func compute_R_and_t()

Parameters

R
t

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

◆ init_camera_parameters()

template<typename T >

void mrpt::vision::pnp::epnp::init_camera_parameters ( const cv::Mat & cameraMatrix )

inlineprivate

Initialize Camera Matrix.

Parameters

[in] cameraMatrix Camera Intrinsic matrix as a OpenCV Matrix

Definition at line 70 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

opoints	Object points in Camera coordinate system
ipoints	Imate points in pixel coordinates

Definition at line 85 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

◆ 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

R	Rotation Matrix
t	Translation Vector

Returns

◆ solve_for_sign()

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

private

Internal function.

Member Data Documentation

◆ A1

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

private

Internal member variable.

Definition at line 271 of file epnp.h.

◆ A2

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

private

Definition at line 271 of file epnp.h.

◆ alphas

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

private

Definition at line 265 of file epnp.h.

◆ ccs

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

private

Definition at line 268 of file epnp.h.

◆ cws

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

private

Number of 2d/3d correspondences.

Definition at line 268 of file epnp.h.

◆ cws_determinant

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

private

Internal member variables.

Definition at line 269 of file epnp.h.

◆ fu

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

private

Image center in y-direction.

Definition at line 262 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 263 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 270 of file epnp.h.

◆ number_of_correspondences

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

private

Internal member variables.

Definition at line 266 of file epnp.h.

Referenced by init_points().

◆ pcs

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

private

Definition at line 265 of file epnp.h.

◆ pws

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

private

Focal length in y-direction.

Definition at line 265 of file epnp.h.

Referenced by init_points().

◆ uc

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

private

Definition at line 260 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 265 of file epnp.h.

Referenced by init_points().

◆ vc

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

private

Image center in x-direction.

Definition at line 261 of file epnp.h.

Referenced by init_camera_parameters(), and init_points().

Detailed Description

Public Member Functions

Private Member Functions

Private Attributes

Constructor & Destructor Documentation

◆ epnp()

◆ ~epnp()

Member Function Documentation

◆ add_correspondence()

◆ choose_control_points()

◆ compute_A_and_b_gauss_newton()

◆ compute_barycentric_coordinates()

◆ compute_ccs()

◆ compute_L_6x10()

◆ compute_pcs()

◆ compute_pose()

◆ compute_R_and_t()

◆ compute_rho()

◆ copy_R_and_t()

◆ dist2()

◆ dot()

◆ estimate_R_and_t()

◆ fill_M()

◆ find_betas_approx_1()

◆ find_betas_approx_2()

◆ find_betas_approx_3()

◆ gauss_newton()

◆ init_camera_parameters()

◆ init_points()

◆ qr_solve()

◆ reprojection_error()

◆ solve_for_sign()

Member Data Documentation

◆ A1

◆ A2

◆ alphas

◆ ccs

◆ cws

◆ cws_determinant

◆ fu

◆ fv

◆ max_nr

◆ number_of_correspondences

◆ pcs

◆ pws

◆ uc

◆ us

◆ vc