mrpt::vision::pnp::p3p Class Reference

Detailed Description

Author

Chandra Mangipudi

Date

11/08/16

Definition at line 32 of file p3p.h.

#include <pnp/p3p.h>

Public Member Functions
	p3p (double fx, double fy, double cx, double cy)
	Constructor for p3p class using C. More...
	p3p (Eigen::MatrixXd cam_intrinsic)
	Constructor using Eigen matrix. More...
bool	solve (Eigen::Ref< Eigen::Matrix3d > R, Eigen::Ref< Eigen::Vector3d > t, Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts)
int	solve (double R[4][3][3], double t[4][3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1, double mv1, double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2)
	Function to compute pose from 3 points using C function. More...
bool	solve (double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1, double mv1, double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2, double mu3, double mv3, double X3, double Y3, double Z3)
	Function to compute pose from 4 points using C function. More...

Private Member Functions
void	extract_points (Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts, std::vector< double > &points)
	Eigen wrapper for extracting object and image points. More...
void	init_inverse_parameters ()
	Function to compute inverse parameters of camera intrinsic matrix. More...
int	solve_for_lengths (double lengths[4][3], double distances[3], double cosines[3])
	Helper function to solve() More...
bool	align (double M_start[3][3], double X0, double Y0, double Z0, double X1, double Y1, double Z1, double X2, double Y2, double Z2, double R[3][3], double T[3])
bool	jacobi_4x4 (double *A, double *D, double *U)
	Function used to compute the SVD. More...

Private Attributes
double	fx
double	fy
	Focal length x. More...
double	cx
	Focal length y. More...
double	cy
	Image center x. More...
double	inv_fx
	Image center y. More...
double	inv_fy
	Inverse of focal length x. More...
double	cx_fx
	Inverse of focal length y. More...
double	cy_fy
	Inverse of image center point x. More...

Constructor & Destructor Documentation

p3p() [1/2]

mrpt::vision::pnp::p3p::p3p	(	double	fx,
		double	fy,
		double	cx,
		double	cy
	)

Constructor for p3p class using C.

Definition at line 43 of file p3p.cpp.

p3p() [2/2]

mrpt::vision::pnp::p3p::p3p

(

Eigen::MatrixXd

cam_intrinsic

)

Constructor using Eigen matrix.

Definition at line 34 of file p3p.cpp.

Member Function Documentation

align()

bool mrpt::vision::pnp::p3p::align ( double  M_start[3][3], double  X0, double  Y0, double  Z0, double  X1, double  Y1, double  Z1, double  X2, double  Y2, double  Z2, double  R[3][3], double  T[3]  )

private

Definition at line 342 of file p3p.cpp.

References R.

extract_points()

void mrpt::vision::pnp::p3p::extract_points ( Eigen::MatrixXd  obj_pts, Eigen::MatrixXd  img_pts, std::vector< double > &  points  )

inlineprivate

Eigen wrapper for extracting object and image points.

Parameters

[in]	opoints	Object points (Eigen Mat)
[in]	ipoints	Image points (Eigen Mat)
[out]	points	Combination of object and image points (C structure)

Definition at line 164 of file p3p.h.

References cx, cy, fx, and fy.

init_inverse_parameters()

void mrpt::vision::pnp::p3p::init_inverse_parameters ( )

private

Function to compute inverse parameters of camera intrinsic matrix.

Definition at line 26 of file p3p.cpp.

References cx, cx_fx, cy, cy_fy, fx, fy, inv_fx, and inv_fy.

jacobi_4x4()

bool mrpt::vision::pnp::p3p::jacobi_4x4 ( double *  A, double *  D, double *  U  )

private

Function used to compute the SVD.

Parameters

[in]	A	Input Matrix for which SVD is to be computed
[out]	D	Diagonal Matrix
[out]	U	Matrix of left eigen vectors

Returns

Definition at line 420 of file p3p.cpp.

References mrpt::system::os::memcpy(), and mrpt::math::sum().

solve() [1/3]

bool mrpt::vision::pnp::p3p::solve	(	Eigen::Ref< Eigen::Matrix3d >	R,
		Eigen::Ref< Eigen::Vector3d >	t,
		Eigen::MatrixXd	obj_pts,
		Eigen::MatrixXd	img_pts
	)

Definition at line 91 of file p3p.cpp.

References R.

solve() [2/3]

int mrpt::vision::pnp::p3p::solve	(	double	R[4][3][3],
		double	t[4][3],
		double	mu0,
		double	mv0,
		double	X0,
		double	Y0,
		double	Z0,
		double	mu1,
		double	mv1,
		double	X1,
		double	Y1,
		double	Z1,
		double	mu2,
		double	mv2,
		double	X2,
		double	Y2,
		double	Z2
	)

Function to compute pose from 3 points using C function.

Parameters

[out]	R	Rotation Matrix
[out]	t	Translation Vector
[in]	mu0	x- coordinate of image point 1
[in]	mv0	y- coordinate of image point 1
[in]	X0	X- coordinate of object point 1
[in]	Y0	Y- coordinate of object point 1
[in]	Z0	Z- coordinate of object point 1
[in]	mu1	x- coordinate of image point 2
[in]	mv1	y- coordinate of image point 2
[in]	X1	X- coordinate of object point 2
[in]	Y1	Y- coordinate of object point 2
[in]	Z1	Z- coordinate of object point 2
[in]	mu2	x- coordinate of image point 3
[in]	mv2	y- coordinate of image point 3
[in]	X2	X- coordinate of object point 3
[in]	Y2	Y- coordinate of object point 3
[in]	Z2	Z- coordinate of object point 3

Returns

Success flag

Definition at line 152 of file p3p.cpp.

References mrpt::math::norm(), and R.

solve() [3/3]

bool mrpt::vision::pnp::p3p::solve	(	double	R[3][3],
		double	t[3],
		double	mu0,
		double	mv0,
		double	X0,
		double	Y0,
		double	Z0,
		double	mu1,
		double	mv1,
		double	X1,
		double	Y1,
		double	Z1,
		double	mu2,
		double	mv2,
		double	X2,
		double	Y2,
		double	Z2,
		double	mu3,
		double	mv3,
		double	X3,
		double	Y3,
		double	Z3
	)

Function to compute pose from 4 points using C function.

Parameters

[out]	R	Rotation Matrix
[out]	t	Translation Vector
[in]	mu0	x- coordinate of image point 1
[in]	mv0	y- coordinate of image point 1
[in]	X0	X- coordinate of object point 1
[in]	Y0	Y- coordinate of object point 1
[in]	Z0	Z- coordinate of object point 1
[in]	mu1	x- coordinate of image point 2
[in]	mv1	y- coordinate of image point 2
[in]	X1	X- coordinate of object point 2
[in]	Y1	Y- coordinate of object point 2
[in]	Z1	Z- coordinate of object point 2
[in]	mu2	x- coordinate of image point 3
[in]	mv2	y- coordinate of image point 3
[in]	X2	X- coordinate of object point 3
[in]	Y2	Y- coordinate of object point 3
[in]	Z2	Z- coordinate of object point 3
[in]	mu3	x- coordinate of image point 4
[in]	mv3	y- coordinate of image point 4
[in]	X3	X- coordinate of object point 4
[in]	Y3	Y- coordinate of object point 4
[in]	Z3	Z- coordinate of object point 4

Returns

Definition at line 108 of file p3p.cpp.

References R.

solve_for_lengths()

int mrpt::vision::pnp::p3p::solve_for_lengths ( double  lengths[4][3], double  distances[3], double  cosines[3]  )

private

Helper function to solve()

Given 3D distances between three points and cosines of 3 angles at the apex, calculates the lentghs of the line segments connecting projection center (P) and the three 3D points (A, B, C).

Parameters

[out]	lengths	Internal lengths used for P3P
[in]	distances	Internal distances used for computation of lengths
[in]	cosines	Internal cosines used for computation of lengths

Returns

Returned distances are for |PA|, |PB|, |PC| respectively. Only the solution to the main branch. Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem" IEEE Trans. on PAMI, vol. 25, No. 8, August 2003

Parameters

lengths3D	Lengths of line segments up to four solutions.
dist3D	Distance between 3D points in pairs |BC|, |AC|, |AB|.
cosines	Cosine of the angles /_BPC, /_APC, /_APB.

Returns

Number of solutions. WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED

Definition at line 241 of file p3p.cpp.

References mrpt::obs::gnss::a2, mrpt::obs::gnss::b0, mrpt::obs::gnss::b1, mrpt::obs::gnss::b2, and solve_deg4().

Member Data Documentation

cx

double mrpt::vision::pnp::p3p::cx

private

Focal length y.

Definition at line 208 of file p3p.h.

Referenced by extract_points(), and init_inverse_parameters().

cx_fx

double mrpt::vision::pnp::p3p::cx_fx

private

Inverse of focal length y.

Definition at line 212 of file p3p.h.

Referenced by init_inverse_parameters().

cy

double mrpt::vision::pnp::p3p::cy

private

Image center x.

Definition at line 209 of file p3p.h.

Referenced by extract_points(), and init_inverse_parameters().

cy_fy

double mrpt::vision::pnp::p3p::cy_fy

private

Inverse of image center point x.

Definition at line 213 of file p3p.h.

Referenced by init_inverse_parameters().

fx

double mrpt::vision::pnp::p3p::fx

private

Definition at line 206 of file p3p.h.

Referenced by extract_points(), and init_inverse_parameters().

fy

double mrpt::vision::pnp::p3p::fy

private

Focal length x.

Definition at line 207 of file p3p.h.

Referenced by extract_points(), and init_inverse_parameters().

inv_fx

double mrpt::vision::pnp::p3p::inv_fx

private

Image center y.

Definition at line 210 of file p3p.h.

Referenced by init_inverse_parameters().

inv_fy

double mrpt::vision::pnp::p3p::inv_fy

private

Inverse of focal length x.

Definition at line 211 of file p3p.h.

Referenced by init_inverse_parameters().