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]	betas	Internal member variable
[in]	ut	Internal 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]	R	Rotation Matrix
[out]	t	Translation 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]	R	Rotation Matrix
[out]	t	Translation 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]	R	Rotaiton matrix
[out]	t	Translation 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]	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

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] cameraMatrix Camera 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]	opoints	Object Points
[in]	ipoints	Image 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]	A	Matrix 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]	R	Rotation matrix
[in]	t	Trnaslation 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().

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