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
Definition at line 32 of file CCamModel.h.
#include <mrpt/vision/CCamModel.h>
Classes | |
struct | CameraTempVariables |
Public Member Functions | |
CCamModel () | |
Default Constructor. More... | |
void | loadFromConfigFile (const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE |
This method load the options from a ".ini"-like file or memory-stored string list. More... | |
void | dumpToTextStream (mrpt::utils::CStream &out) const MRPT_OVERRIDE |
This method should clearly display 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 ![]() | |
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 ![]() | |
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 §ion) |
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 §ion) 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 §ion) 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 |
mrpt::vision::CCamModel::CCamModel | ( | ) |
Default Constructor.
mrpt::vision::CCamModel::CCamModel | ( | const mrpt::utils::CConfigFileBase & | cfgIni | ) |
Constructor from a ini file.
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.
|
inherited |
Just like dumpToTextStream() but sending the text to the console (std::cout)
|
virtual |
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
The default implementation in this base class relies on saveToConfigFile() to generate a plain text representation of all the parameters.
Reimplemented from mrpt::utils::CLoadableOptions.
|
staticprotectedinherited |
|
staticprotectedinherited |
|
staticprotectedinherited |
|
staticprotectedinherited |
Used to print variable info from dumpToTextStream with the macro LOADABLEOPTS_DUMP_VAR.
|
staticprotectedinherited |
|
inlineprivate |
Definition at line 199 of file CCamModel.h.
|
inline |
Definition at line 226 of file CCamModel.h.
References mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::utils::TCamera::k1(), mrpt::utils::TCamera::k2(), mrpt::utils::TCamera::k3(), mrpt::utils::TCamera::p1(), mrpt::utils::TCamera::p2(), mrpt::utils::square(), and mrpt::mrpt::utils::square().
|
inline |
Definition at line 154 of file CCamModel.h.
|
inline |
Definition at line 160 of file CCamModel.h.
References mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::vision::CCamModel::CameraTempVariables< T >::K, mrpt::utils::TCamera::k1(), mrpt::utils::TCamera::k2(), mrpt::utils::TCamera::k3(), mrpt::utils::TCamera::p1(), mrpt::utils::TCamera::p2(), mrpt::vision::CCamModel::CameraTempVariables< T >::R, mrpt::utils::square(), mrpt::vision::CCamModel::CameraTempVariables< T >::x_, and mrpt::vision::CCamModel::CameraTempVariables< T >::y_.
|
inline |
Definition at line 143 of file CCamModel.h.
|
inline |
|
inline |
Definition at line 131 of file CCamModel.h.
References mrpt::vision::CCamModel::CameraTempVariables< T >::K, mrpt::utils::TCamera::k1(), mrpt::utils::TCamera::k2(), mrpt::utils::TCamera::k3(), mrpt::utils::TCamera::p1(), mrpt::utils::TCamera::p2(), mrpt::vision::CCamModel::CameraTempVariables< T >::R, mrpt::utils::square(), mrpt::vision::CCamModel::CameraTempVariables< T >::x_, mrpt::vision::CCamModel::CameraTempVariables< T >::x_2, mrpt::vision::CCamModel::CameraTempVariables< T >::x__, mrpt::vision::CCamModel::CameraTempVariables< T >::y_, mrpt::vision::CCamModel::CameraTempVariables< T >::y_2, and mrpt::vision::CCamModel::CameraTempVariables< T >::y__.
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 , evaluated at the point p3D (read below the full explanation)
We define as the projected point in pixels (origin at the top-left corner), and
as the 3D point in space, in coordinates relative to the camera (+Z pointing forwards).
Then this method computes the 2x3 Jacobian:
With:
where is the focal length in units of pixel sizes in x and y, respectively. And, if we define:
then:
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 , evaluated at the pixel p.
|
virtual |
This method load the options from a ".ini"-like file or memory-stored string list.
Only those parameters found in the given "section" and having the same name that the variable are loaded. Those not found in the file will stay with their previous values (usually the default values loaded at initialization). An example of an ".ini" file:
Implements mrpt::utils::CLoadableOptions.
|
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.
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)
|
virtualinherited |
This method saves the options to a ".ini"-like file or memory-stored string list.
Reimplemented in mrpt::vision::TMultiResDescOptions, mrpt::vision::TMultiResDescMatchOptions, mrpt::nav::CHolonomicND::TOptions, and mrpt::nav::CHolonomicVFF::TOptions.
|
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.
|
inlineprivate |
Definition at line 205 of file CCamModel.h.
|
inlineprivate |
Definition at line 213 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.
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.4.0 SVN:Unversioned directory at Sun Jul 10 11:38:36 UTC 2016 |