Detailed Description

Author: Chandra Mangipudi

Date: 11/08/16

Definition at line 36 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 @func 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 168 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	(	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() [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	(	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_for_lengths()

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

private

Helper function to @func 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.