Main MRPT website > C++ reference for MRPT 1.5.7
List of all members | Public Member Functions | Private Member Functions | Private Attributes
mrpt::vision::pnp::p3p Class Reference

Detailed Description

Author
Chandra Mangipudi
Date
11/08/16

Definition at line 28 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 42 of file p3p.cpp.

◆ p3p() [2/2]

mrpt::vision::pnp::p3p::p3p ( Eigen::MatrixXd  cam_intrinsic)

Constructor using Eigen matrix.

Definition at line 33 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 303 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]opointsObject points (Eigen Mat)
[in]ipointsImage points (Eigen Mat)
[out]pointsCombination of object and image points (C structure)

Definition at line 154 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 25 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]AInput Matrix for which SVD is to be computed
[out]DDiagonal Matrix
[out]UMatrix of left eigen vectors
Returns

Definition at line 375 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 87 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]RRotation Matrix
[out]tTranslation Vector
[in]mu0x- coordinate of image point 1
[in]mv0y- coordinate of image point 1
[in]X0X- coordinate of object point 1
[in]Y0Y- coordinate of object point 1
[in]Z0Z- coordinate of object point 1
[in]mu1x- coordinate of image point 2
[in]mv1y- coordinate of image point 2
[in]X1X- coordinate of object point 2
[in]Y1Y- coordinate of object point 2
[in]Z1Z- coordinate of object point 2
[in]mu2x- coordinate of image point 3
[in]mv2y- coordinate of image point 3
[in]X2X- coordinate of object point 3
[in]Y2Y- coordinate of object point 3
[in]Z2Z- coordinate of object point 3
Returns
Success flag

Definition at line 137 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]RRotation Matrix
[out]tTranslation Vector
[in]mu0x- coordinate of image point 1
[in]mv0y- coordinate of image point 1
[in]X0X- coordinate of object point 1
[in]Y0Y- coordinate of object point 1
[in]Z0Z- coordinate of object point 1
[in]mu1x- coordinate of image point 2
[in]mv1y- coordinate of image point 2
[in]X1X- coordinate of object point 2
[in]Y1Y- coordinate of object point 2
[in]Z1Z- coordinate of object point 2
[in]mu2x- coordinate of image point 3
[in]mv2y- coordinate of image point 3
[in]X2X- coordinate of object point 3
[in]Y2Y- coordinate of object point 3
[in]Z2Z- coordinate of object point 3
[in]mu3x- coordinate of image point 4
[in]mv3y- coordinate of image point 4
[in]X3X- coordinate of object point 4
[in]Y3Y- coordinate of object point 4
[in]Z3Z- coordinate of object point 4
Returns

Definition at line 100 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]lengthsInternal lengths used for P3P
[in]distancesInternal distances used for computation of lengths
[in]cosinesInternal 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
lengths3DLengths of line segments up to four solutions.
dist3DDistance between 3D points in pairs |BC|, |AC|, |AB|.
cosinesCosine of the angles /_BPC, /_APC, /_APB.
Returns
Number of solutions. WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED

Definition at line 211 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 196 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 200 of file p3p.h.

Referenced by init_inverse_parameters().

◆ cy

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

Image center x.

Definition at line 197 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 201 of file p3p.h.

Referenced by init_inverse_parameters().

◆ fx

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

Definition at line 194 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 195 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 198 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 199 of file p3p.h.

Referenced by init_inverse_parameters().




Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019