MRPT  2.0.0
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 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...
 
 p3p (cv::Mat cameraMatrix)
 Constructor using OpenCV matrix. More...
 
bool solve (cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints)
 Function to compute pose by P3P using OpenCV. 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

template<typename T >
void init_camera_parameters (const cv::Mat &cameraMatrix)
 OpenCV Initialization function to access camera intrinsic matrix. More...
 
template<typename OpointType , typename IpointType >
void extract_points (const cv::Mat &opoints, const cv::Mat &ipoints, std::vector< double > &points)
 OpoenCV wrapper for extracting object and image points. More...
 
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/3]

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

Constructor for p3p class using C.

Definition at line 35 of file p3p.cpp.

◆ p3p() [2/3]

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

Constructor using Eigen matrix.

Definition at line 26 of file p3p.cpp.

◆ p3p() [3/3]

mrpt::vision::pnp::p3p::p3p ( cv::Mat  cameraMatrix)

Constructor using OpenCV matrix.

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

References R.

◆ extract_points() [1/2]

template<typename OpointType , typename IpointType >
void mrpt::vision::pnp::p3p::extract_points ( const cv::Mat &  opoints,
const cv::Mat &  ipoints,
std::vector< double > &  points 
)
inlineprivate

OpoenCV wrapper for extracting object and image points.

Parameters
[in]opointsObject points (OpenCV Mat)
[in]ipointsImage points (OpenCV Mat)
[out]pointsCombination of object and image points (C structure)

Definition at line 141 of file p3p.h.

References cx, cy, fx, and fy.

◆ extract_points() [2/2]

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 164 of file p3p.h.

References cx, cy, fx, and fy.

◆ init_camera_parameters()

template<typename T >
void mrpt::vision::pnp::p3p::init_camera_parameters ( const cv::Mat &  cameraMatrix)
inlineprivate

OpenCV Initialization function to access camera intrinsic matrix.

Parameters
[in]cameraMatrixCamera Intrinsic Matrix in OpenCV Mat format

Definition at line 126 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 18 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 412 of file p3p.cpp.

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

Here is the call graph for this function:

◆ solve() [1/4]

bool mrpt::vision::pnp::p3p::solve ( cv::Mat &  R,
cv::Mat &  tvec,
const cv::Mat &  opoints,
const cv::Mat &  ipoints 
)

Function to compute pose by P3P using OpenCV.

Parameters
[out]RRotation Matrix
[out]tvecTranslation Vector
[in]opointsObject points
[in]ipointsImage points
Returns
Success flag

Definition at line 54 of file p3p.cpp.

References R.

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

Here is the caller graph for this function:

◆ solve() [2/4]

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 83 of file p3p.cpp.

References R.

◆ solve() [3/4]

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 144 of file p3p.cpp.

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

Here is the call graph for this function:

◆ solve() [4/4]

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 233 of file p3p.cpp.

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

Here is the call graph for this function:

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(), init_camera_parameters(), 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(), init_camera_parameters(), 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(), init_camera_parameters(), 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(), init_camera_parameters(), 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().




Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at miƩ abr 1 00:50:30 CEST 2020