Detailed Description

Author: Chandra Mangipudi

Date: 12/08/16

Definition at line 90 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 @alphas. 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 (void)
	Compute object points based on control points. More...

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

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

Constructor for UPnP class.

◆ ~upnp()

mrpt::vision::pnp::upnp::~upnp ( )

Destructor for UPnP class.

Member Function Documentation

◆ choose_control_points()

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

private

Function to select 4 control points.

◆ compute_A_and_b_gauss_newton()

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

◆ compute_alphas()

void mrpt::vision::pnp::upnp::compute_alphas ( )

private

Function to comput @alphas.

◆ compute_ccs()

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

private

Compute the control points.

Parameters

[in]	betas	Internal member variable
[in]	ut	Internal member variable

◆ compute_constraint_distance_2param_6eq_2unk_f_unk()

cv::Mat mrpt::vision::pnp::upnp::compute_constraint_distance_2param_6eq_2unk_f_unk ( const cv::Mat & M1 )

private

Internal function.

Parameters

[in] M1

Returns: Distance

◆ compute_constraint_distance_3param_6eq_6unk_f_unk()

cv::Mat mrpt::vision::pnp::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

◆ compute_L_6x12()

void mrpt::vision::pnp::upnp::compute_L_6x12	(	const double *	ut,
		double *	l_6x12
	)

private

Internal function.

Parameters

[in]	ut
[out]	l_6x12

◆ compute_pcs()

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

private

Compute object points based on control points.

◆ compute_pose()

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

Function to compute pose.

Parameters

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

Returns

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

◆ compute_R_and_t()

double mrpt::vision::pnp::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]	R	Rotation Matrix
[out]	t	Translation VectorS

Returns

◆ compute_rho()

void mrpt::vision::pnp::upnp::compute_rho ( double * rho )

private

Internal fucntion.

Parameters

[out] rho

◆ copy_R_and_t()

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

◆ dist2()

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

◆ dot()

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

private

Compute the dot product between two vectors.

Parameters

[in]	v1
[in]	v2

Returns: Dot product

◆ dotXY()

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

◆ dotZ()

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

◆ estimate_R_and_t()

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

private

Helper function to function compute_R_and_t()

Parameters

[out]	R	Rotaiton matrix
[out]	t	Translation vector

◆ fill_M()

void mrpt::vision::pnp::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]	M	Maucaulay matrix
[in]	row	Internal member
[in]	alphas	Internal member
[in]	u	Image pixel x co-ordinate
[in]	v	Image pixel y co-ordinate

◆ find_betas_and_focal_approx_1()

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

◆ find_betas_and_focal_approx_2()

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

◆ gauss_newton()

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

◆ generate_all_possible_solutions_for_f_unk()

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

◆ 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] cameraMatrix Camera Intrinsic Matrix

Definition at line 115 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]	opoints	Object Points
[in]	ipoints	Image Points

Definition at line 129 of file upnp.h.

References number_of_correspondences, pws, and us.

◆ qr_solve()

void mrpt::vision::pnp::upnp::qr_solve	(	cv::Mat *	A,
		cv::Mat *	b,
		cv::Mat *	X
	)

private

Function to do a QR decomposition.

Parameters

[in]	A	Matrix to be decomposed
[out]	b
[out]	X

◆ reprojection_error()

double mrpt::vision::pnp::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]	R	Rotation matrix
[in]	t	Trnaslation Vector

Returns: Linear least squares error in pixels

◆ sign()

double mrpt::vision::pnp::upnp::sign ( const double v )

private

Return the sign of the scalar.

Parameters

[in] v

Returns: True if positive else Flase

◆ solve_for_sign()

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

private

Internal member function.

Member Data Documentation

◆ A1

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

private

Internal variable.

Definition at line 360 of file upnp.h.

◆ A2

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

private

Definition at line 360 of file upnp.h.

◆ alphas

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

private

Image points.

Definition at line 355 of file upnp.h.

◆ ccs

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

private

Definition at line 358 of file upnp.h.

◆ cws

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

private

Number of 2d/3d correspondences.

Definition at line 358 of file upnp.h.

◆ fu

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

private

Image center in y-direction.

Definition at line 350 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 351 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 359 of file upnp.h.

◆ number_of_correspondences

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

private

Internal variable.

Definition at line 356 of file upnp.h.

Referenced by init_points().

◆ pcs

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

private

Definition at line 355 of file upnp.h.

◆ pws

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

private

Focal length in y-direction.

Definition at line 353 of file upnp.h.

Referenced by init_points().

◆ uc

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

private

Definition at line 348 of file upnp.h.

Referenced by init_camera_parameters().

◆ us

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

private

Object points.

Definition at line 354 of file upnp.h.

Referenced by init_points().

◆ vc

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

private

Image center in x-direction.

Definition at line 349 of file upnp.h.

Referenced by init_camera_parameters().

Detailed Description

Public Member Functions

Private Member Functions

Private Attributes

Constructor & Destructor Documentation

◆ upnp()

◆ ~upnp()

Member Function Documentation

◆ choose_control_points()

◆ compute_A_and_b_gauss_newton()

◆ compute_alphas()

◆ compute_ccs()

◆ compute_constraint_distance_2param_6eq_2unk_f_unk()

◆ compute_constraint_distance_3param_6eq_6unk_f_unk()

◆ compute_L_6x12()

◆ compute_pcs()

◆ compute_pose()

◆ compute_R_and_t()

◆ compute_rho()

◆ copy_R_and_t()

◆ dist2()

◆ dot()

◆ dotXY()

◆ dotZ()

◆ estimate_R_and_t()

◆ fill_M()

◆ find_betas_and_focal_approx_1()

◆ find_betas_and_focal_approx_2()

◆ gauss_newton()

◆ generate_all_possible_solutions_for_f_unk()

◆ init_camera_parameters()

◆ init_points()

◆ qr_solve()

◆ reprojection_error()

◆ sign()

◆ solve_for_sign()

Member Data Documentation

◆ A1

◆ A2

◆ alphas

◆ ccs

◆ cws

◆ fu

◆ fv

◆ max_nr

◆ number_of_correspondences

◆ pcs

◆ pws

◆ uc

◆ us

◆ vc