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]	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 ( )

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]	R	Rotation Matrix
[out]	t	Translation 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] cameraMatrix Camera 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

opoints	Object points in Camera coordinate system
ipoints	Imate 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

R	Rotation Matrix
t	Translation 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().

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