Main MRPT website > C++ reference for MRPT 1.3.2
List of all members | Classes | Public Member Functions | Public Attributes | Static Protected Member Functions | Private Member Functions
mrpt::vision::CCamModel Class Reference

Detailed Description

This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobians.

The camera parameters are accessible in the public member CCamModel::cam

See also
mrpt::utils::TCamera, CMonoSlam, the application camera-calib-gui for calibrating a camera

Definition at line 32 of file CCamModel.h.

#include <mrpt/vision/CCamModel.h>

Inheritance diagram for mrpt::vision::CCamModel:
Inheritance graph

Classes

struct  CameraTempVariables
 

Public Member Functions

 CCamModel ()
 Default Constructor. More...
 
void loadFromConfigFile (const mrpt::utils::CConfigFileBase &source, const std::string &section)
 This method loads the options from a ".ini"-like file or memory-stored string list. More...
 
void dumpToTextStream (mrpt::utils::CStream &out) const
 This method displays clearly all the contents of the structure in textual form, sending it to a CStream. More...
 
 CCamModel (const mrpt::utils::CConfigFileBase &cfgIni)
 Constructor from a ini file. More...
 
void jacob_undistor_fm (const mrpt::utils::TPixelCoordf &uvd, math::CMatrixDouble &J_undist)
 Jacobian for undistortion the image coordinates. More...
 
void jacob_undistor (const mrpt::utils::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist)
 Calculate the image coordinates undistorted. More...
 
void distort_a_point (const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &distorted_p)
 Return the pixel position distorted by the camera. More...
 
void undistort_point (const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &undistorted_p)
 Return the pixel position undistorted by the camera The input values 'col' and 'row' will be replace for the new values (undistorted) More...
 
void project_3D_point (const mrpt::math::TPoint3D &p3D, mrpt::utils::TPixelCoordf &distorted_p) const
 Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right) More...
 
void unproject_3D_point (const mrpt::utils::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const
 Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position. More...
 
void jacobian_project_with_distortion (const mrpt::math::TPoint3D &p3D, math::CMatrixDouble &dh_dy) const
 Jacobian of the projection of 3D points (with distortion), as done in project_3D_point $ \frac{\partial h}{\partial y} $, evaluated at the point p3D (read below the full explanation) More...
 
void jacobian_unproject_with_distortion (const mrpt::utils::TPixelCoordf &p, math::CMatrixDouble &dy_dh) const
 Jacobian of the unprojection of a pixel (with distortion) back into a 3D point, as done in unproject_3D_point $ \frac{\partial y}{\partial h} $, evaluated at the pixel p. More...
 
template<typename T , typename POINT >
void getTemporaryVariablesForTransform (const POINT &p, CameraTempVariables< T > &v) const
 
template<typename T , typename POINT , typename PIXEL >
void getFullProjection (const POINT &pIn, PIXEL &pOut) const
 
template<typename T , typename PIXEL >
void getFullProjectionT (const CameraTempVariables< T > &tmp, PIXEL &pOut) const
 
template<typename T , typename POINT , typename MATRIX >
void getFullJacobian (const POINT &pIn, MATRIX &mOut) const
 
template<typename T , typename POINT , typename MATRIX >
void getFullJacobianT (const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
 
template<typename POINTIN , typename POINTOUT , typename MAT22 >
void getFullInverseModelWithJacobian (const POINTIN &pIn, POINTOUT &pOut, MAT22 &jOut) const
 
void loadFromConfigFileName (const std::string &config_file, const std::string &section)
 Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to load the file. More...
 
virtual void saveToConfigFile (mrpt::utils::CConfigFileBase &target, const std::string &section) const
 This method saves the options to a ".ini"-like file or memory-stored string list. More...
 
void saveToConfigFileName (const std::string &config_file, const std::string &section) const
 Behaves like saveToConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to save the file. More...
 
void dumpToConsole () const
 Just like dumpToTextStream() but sending the text to the console (std::cout) More...
 

Public Attributes

mrpt::utils::TCamera cam
 The parameters of a camera. More...
 

Static Protected Member Functions

static void dumpVar_int (CStream &out, const char *varName, int v)
 Used to print variable info from dumpToTextStream with the macro LOADABLEOPTS_DUMP_VAR. More...
 
static void dumpVar_float (CStream &out, const char *varName, float v)
 
static void dumpVar_double (CStream &out, const char *varName, double v)
 
static void dumpVar_bool (CStream &out, const char *varName, bool v)
 
static void dumpVar_string (CStream &out, const char *varName, const std::string &v)
 

Private Member Functions

mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian () const
 
mrpt::math::CMatrixFixedNumeric< double, 4, 2 > secondInverseJacobian () const
 
mrpt::math::CMatrixFixedNumeric< double, 3, 4 > thirdInverseJacobian () const
 

Constructor & Destructor Documentation

mrpt::vision::CCamModel::CCamModel ( )

Default Constructor.

mrpt::vision::CCamModel::CCamModel ( const mrpt::utils::CConfigFileBase cfgIni)

Constructor from a ini file.

Member Function Documentation

void mrpt::vision::CCamModel::distort_a_point ( const mrpt::utils::TPixelCoordf p,
mrpt::utils::TPixelCoordf distorted_p 
)

Return the pixel position distorted by the camera.

void mrpt::utils::CLoadableOptions::dumpToConsole ( ) const
inherited

Just like dumpToTextStream() but sending the text to the console (std::cout)

void mrpt::vision::CCamModel::dumpToTextStream ( mrpt::utils::CStream out) const
virtual

This method displays clearly all the contents of the structure in textual form, sending it to a CStream.

Reimplemented from mrpt::utils::CLoadableOptions.

static void mrpt::utils::CLoadableOptions::dumpVar_bool ( CStream out,
const char *  varName,
bool  v 
)
staticprotectedinherited
static void mrpt::utils::CLoadableOptions::dumpVar_double ( CStream out,
const char *  varName,
double  v 
)
staticprotectedinherited
static void mrpt::utils::CLoadableOptions::dumpVar_float ( CStream out,
const char *  varName,
float  v 
)
staticprotectedinherited
static void mrpt::utils::CLoadableOptions::dumpVar_int ( CStream out,
const char *  varName,
int  v 
)
staticprotectedinherited

Used to print variable info from dumpToTextStream with the macro LOADABLEOPTS_DUMP_VAR.

static void mrpt::utils::CLoadableOptions::dumpVar_string ( CStream out,
const char *  varName,
const std::string &  v 
)
staticprotectedinherited
mrpt::math::CMatrixFixedNumeric<double,2,2> mrpt::vision::CCamModel::firstInverseJacobian ( ) const
inlineprivate

Definition at line 205 of file CCamModel.h.

template<typename POINTIN , typename POINTOUT , typename MAT22 >
void mrpt::vision::CCamModel::getFullInverseModelWithJacobian ( const POINTIN &  pIn,
POINTOUT &  pOut,
MAT22 &  jOut 
) const
inline
template<typename T , typename POINT , typename MATRIX >
void mrpt::vision::CCamModel::getFullJacobian ( const POINT &  pIn,
MATRIX &  mOut 
) const
inline

Definition at line 160 of file CCamModel.h.

template<typename T , typename POINT , typename MATRIX >
void mrpt::vision::CCamModel::getFullJacobianT ( const POINT &  pIn,
const CameraTempVariables< T > &  tmp,
MATRIX &  mOut 
) const
inline
template<typename T , typename POINT , typename PIXEL >
void mrpt::vision::CCamModel::getFullProjection ( const POINT &  pIn,
PIXEL &  pOut 
) const
inline

Definition at line 149 of file CCamModel.h.

template<typename T , typename PIXEL >
void mrpt::vision::CCamModel::getFullProjectionT ( const CameraTempVariables< T > &  tmp,
PIXEL &  pOut 
) const
inline
template<typename T , typename POINT >
void mrpt::vision::CCamModel::getTemporaryVariablesForTransform ( const POINT &  p,
CameraTempVariables< T > &  v 
) const
inline
void mrpt::vision::CCamModel::jacob_undistor ( const mrpt::utils::TPixelCoordf p,
mrpt::math::CMatrixDouble J_undist 
)

Calculate the image coordinates undistorted.

void mrpt::vision::CCamModel::jacob_undistor_fm ( const mrpt::utils::TPixelCoordf uvd,
math::CMatrixDouble J_undist 
)

Jacobian for undistortion the image coordinates.

void mrpt::vision::CCamModel::jacobian_project_with_distortion ( const mrpt::math::TPoint3D p3D,
math::CMatrixDouble dh_dy 
) const

Jacobian of the projection of 3D points (with distortion), as done in project_3D_point $ \frac{\partial h}{\partial y} $, evaluated at the point p3D (read below the full explanation)

We define $ h = (h_x ~ h_y) $ as the projected point in pixels (origin at the top-left corner), and $ y=( y_x ~ y_y ~ y_z ) $ as the 3D point in space, in coordinates relative to the camera (+Z pointing forwards).

Then this method computes the 2x3 Jacobian:

\[ \frac{\partial h}{\partial y} = \frac{\partial h}{\partial u} \frac{\partial u}{\partial y} \]

With:

\[ \frac{\partial u}{\partial y} = \left( \begin{array}{ccc} \frac{f_x}{y_z} & 0 & - y \frac{f_x}{y_z^2} \\ 0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\ \end{array} \right) \]

where $ f_x, f_y $ is the focal length in units of pixel sizes in x and y, respectively. And, if we define:

\[ f = 1+ 2 k_1 (u_x^2+u_y^2) \]

then:

\[ \frac{\partial h}{\partial u} = \left( \begin{array}{cc} \frac{ 1+2 k_1 u_y^2 }{f^{3/2}} & -\frac{2 u_x u_y k_1 }{f^{3/2}} \\ -\frac{2 u_x u_y k_1 }{f^{3/2}} & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}} \end{array} \right) \]

Note
JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian
See also
project_3D_point
void mrpt::vision::CCamModel::jacobian_unproject_with_distortion ( const mrpt::utils::TPixelCoordf p,
math::CMatrixDouble dy_dh 
) const

Jacobian of the unprojection of a pixel (with distortion) back into a 3D point, as done in unproject_3D_point $ \frac{\partial y}{\partial h} $, evaluated at the pixel p.

Note
JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::UnprojectionJacobian
See also
unproject_3D_point
void mrpt::vision::CCamModel::loadFromConfigFile ( const mrpt::utils::CConfigFileBase source,
const std::string &  section 
)
virtual

This method loads the options from a ".ini"-like file or memory-stored string list.

Implements mrpt::utils::CLoadableOptions.

void mrpt::utils::CLoadableOptions::loadFromConfigFileName ( const std::string &  config_file,
const std::string &  section 
)
inherited

Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to load the file.

See also
loadFromConfigFile
void mrpt::vision::CCamModel::project_3D_point ( const mrpt::math::TPoint3D p3D,
mrpt::utils::TPixelCoordf distorted_p 
) const

Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right)

See also
unproject_3D_point
virtual void mrpt::utils::CLoadableOptions::saveToConfigFile ( mrpt::utils::CConfigFileBase target,
const std::string &  section 
) const
virtualinherited

This method saves the options to a ".ini"-like file or memory-stored string list.

See also
loadFromConfigFile, saveToConfigFileName

Reimplemented in mrpt::vision::TMultiResDescOptions, mrpt::vision::TMultiResDescMatchOptions, mrpt::nav::CHolonomicND::TOptions, and mrpt::nav::CHolonomicVFF::TOptions.

void mrpt::utils::CLoadableOptions::saveToConfigFileName ( const std::string &  config_file,
const std::string &  section 
) const
inherited

Behaves like saveToConfigFile, but you can pass directly a file name and a temporary CConfigFile object will be created automatically to save the file.

See also
saveToConfigFile, loadFromConfigFileName
mrpt::math::CMatrixFixedNumeric<double,4,2> mrpt::vision::CCamModel::secondInverseJacobian ( ) const
inlineprivate

Definition at line 211 of file CCamModel.h.

mrpt::math::CMatrixFixedNumeric<double,3,4> mrpt::vision::CCamModel::thirdInverseJacobian ( ) const
inlineprivate

Definition at line 219 of file CCamModel.h.

void mrpt::vision::CCamModel::undistort_point ( const mrpt::utils::TPixelCoordf p,
mrpt::utils::TPixelCoordf undistorted_p 
)

Return the pixel position undistorted by the camera The input values 'col' and 'row' will be replace for the new values (undistorted)

void mrpt::vision::CCamModel::unproject_3D_point ( const mrpt::utils::TPixelCoordf distorted_p,
mrpt::math::TPoint3D p3D 
) const

Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position.

See also
project_3D_point
Note
Of course, there is a depth ambiguity, so the returned 3D point must be considered a direction from the camera focus, or a vector, rather than a meaninful physical point.

Member Data Documentation

mrpt::utils::TCamera mrpt::vision::CCamModel::cam

The parameters of a camera.

Definition at line 35 of file CCamModel.h.




Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN: at Mon May 9 06:50:38 UTC 2016