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

Versions:
- First version: By Antonio J. Ortiz de Galistea.
- 2009-2010: Rewritten by various authors.

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

Definition at line 33 of file CCamModel.h.

#include <mrpt/vision/CCamModel.h>

Inheritance diagram for mrpt::vision::CCamModel:

Classes
struct	CameraTempVariables

Public Member Functions
	CCamModel ()
	Default Constructor. More...

void	loadFromConfigFile (const mrpt::utils::CConfigFileBase &source, const std::string &section) 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 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

◆ CCamModel() [1/2]

CCamModel::CCamModel ( )

Default Constructor.

Constructor.

Definition at line 24 of file CCamModel.cpp.

◆ CCamModel() [2/2]

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

Constructor from a ini file.

Member Function Documentation

◆ distort_a_point()

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

Return the pixel position distorted by the camera.

Definition at line 102 of file CCamModel.cpp.

References cam, mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::k1(), mrpt::math::square(), mrpt::utils::TPixelCoordf::x, and mrpt::utils::TPixelCoordf::y.

◆ dumpToConsole()

void CLoadableOptions::dumpToConsole ( ) const

inherited

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

Definition at line 47 of file CLoadableOptions.cpp.

References mrpt::utils::CLoadableOptions::dumpToTextStream(), and loadable_opts_my_cout.

Referenced by mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), mrpt::hmtslam::CHMTSLAM::loadOptions(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::printParams().

◆ dumpToTextStream()

void 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.

Definition at line 313 of file CCamModel.cpp.

References MRPT_UNUSED_PARAM.

◆ dumpVar_bool()

void CLoadableOptions::dumpVar_bool	(	CStream &	out,
		const char *	varName,
		bool	v
	)

staticprotectedinherited

Definition at line 67 of file CLoadableOptions.cpp.

References LOADABLEOPTS_COLUMN_WIDTH, and mrpt::utils::CStream::printf().

◆ dumpVar_double()

void CLoadableOptions::dumpVar_double	(	CStream &	out,
		const char *	varName,
		double	v
	)

staticprotectedinherited

Definition at line 62 of file CLoadableOptions.cpp.

References LOADABLEOPTS_COLUMN_WIDTH, and mrpt::utils::CStream::printf().

◆ dumpVar_float()

void CLoadableOptions::dumpVar_float	(	CStream &	out,
		const char *	varName,
		float	v
	)

staticprotectedinherited

Definition at line 57 of file CLoadableOptions.cpp.

References LOADABLEOPTS_COLUMN_WIDTH, and mrpt::utils::CStream::printf().

◆ dumpVar_int()

void CLoadableOptions::dumpVar_int	(	CStream &	out,
		const char *	varName,
		int	v
	)

staticprotectedinherited

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

Definition at line 52 of file CLoadableOptions.cpp.

References LOADABLEOPTS_COLUMN_WIDTH, and mrpt::utils::CStream::printf().

◆ dumpVar_string()

void CLoadableOptions::dumpVar_string	(	CStream &	out,
		const char *	varName,
		const std::string &	v
	)

staticprotectedinherited

Definition at line 72 of file CLoadableOptions.cpp.

References LOADABLEOPTS_COLUMN_WIDTH, and mrpt::utils::CStream::printf().

◆ firstInverseJacobian()

mrpt::math::CMatrixFixedNumeric<double,2,2> mrpt::vision::CCamModel::firstInverseJacobian ( ) const

inlineprivate

Definition at line 200 of file CCamModel.h.

◆ getFullInverseModelWithJacobian()

template<typename POINTIN , typename POINTOUT , typename MAT22 >

void mrpt::vision::CCamModel::getFullInverseModelWithJacobian	(	const POINTIN &	pIn,
		POINTOUT &	pOut,
		MAT22 &	jOut
	)		const

inline

Definition at line 227 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::mrpt::math::square(), and mrpt::math::square().

◆ getFullJacobian()

template<typename T , typename POINT , typename MATRIX >

void mrpt::vision::CCamModel::getFullJacobian	(	const POINT &	pIn,
		MATRIX &	mOut
	)		const

inline

Definition at line 155 of file CCamModel.h.

◆ getFullJacobianT()

template<typename T , typename POINT , typename MATRIX >

void mrpt::vision::CCamModel::getFullJacobianT	(	const POINT &	pIn,
		const CameraTempVariables< T > &	tmp,
		MATRIX &	mOut
	)		const

inline

Definition at line 161 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::math::square(), mrpt::vision::CCamModel::CameraTempVariables< T >::x_, and mrpt::vision::CCamModel::CameraTempVariables< T >::y_.

◆ getFullProjection()

template<typename T , typename POINT , typename PIXEL >

void mrpt::vision::CCamModel::getFullProjection	(	const POINT &	pIn,
		PIXEL &	pOut
	)		const

inline

Definition at line 144 of file CCamModel.h.

◆ getFullProjectionT()

template<typename T , typename PIXEL >

void mrpt::vision::CCamModel::getFullProjectionT	(	const CameraTempVariables< T > &	tmp,
		PIXEL &	pOut
	)		const

inline

Definition at line 150 of file CCamModel.h.

References mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::vision::CCamModel::CameraTempVariables< T >::x__, and mrpt::vision::CCamModel::CameraTempVariables< T >::y__.

◆ getTemporaryVariablesForTransform()

template<typename T , typename POINT >

void mrpt::vision::CCamModel::getTemporaryVariablesForTransform	(	const POINT &	p,
		CameraTempVariables< T > &	v
	)		const

inline

Definition at line 132 of file CCamModel.h.

References mrpt::utils::TCamera::k1(), mrpt::utils::TCamera::k2(), mrpt::utils::TCamera::k3(), mrpt::utils::TCamera::p1(), mrpt::utils::TCamera::p2(), and mrpt::math::square().

◆ jacob_undistor()

void CCamModel::jacob_undistor	(	const mrpt::utils::TPixelCoordf &	p,
		mrpt::math::CMatrixDouble &	J_undist
	)

Calculate the image coordinates undistorted.

Definition at line 53 of file CCamModel.cpp.

References cam, mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::k1(), and mrpt::math::square().

◆ jacob_undistor_fm()

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

Jacobian for undistortion the image coordinates.

Definition at line 29 of file CCamModel.cpp.

References cam, mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::utils::TCamera::k1(), and mrpt::utils::TCamera::k2().

◆ jacobian_project_with_distortion()

void 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

Definition at line 191 of file CCamModel.cpp.

References ASSERT_, cam, mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::utils::TCamera::k1(), mrpt::math::square(), mrpt::math::TPoint3D::x, mrpt::math::TPoint3D::y, and mrpt::math::TPoint3D::z.

◆ jacobian_unproject_with_distortion()

void 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

Definition at line 249 of file CCamModel.cpp.

References cam, mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::utils::TCamera::k1(), and mrpt::math::square().

◆ loadFromConfigFile()

void CCamModel::loadFromConfigFile	(	const mrpt::utils::CConfigFileBase &	source,
		const std::string &	section
	)

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:

[section]
resolution    = 0.10   // blah blah...
modeSelection = 1      // 0=blah, 1=blah,...

See also: loadFromConfigFileName, saveToConfigFile

Implements mrpt::utils::CLoadableOptions.

Definition at line 284 of file CCamModel.cpp.

References ASSERT_, cam, MRPT_END, MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT, MRPT_START, mrpt::utils::TCamera::setDistortionParamsVector(), and mrpt::utils::TCamera::setIntrinsicParamsFromValues().

◆ loadFromConfigFileName()

void 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

Definition at line 23 of file CLoadableOptions.cpp.

References mrpt::utils::CLoadableOptions::loadFromConfigFile().

Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::loadParams().

◆ project_3D_point()

void 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

Definition at line 148 of file CCamModel.cpp.

References ASSERT_, cam, mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::utils::TCamera::k1(), mrpt::math::square(), mrpt::utils::TPixelCoordf::x, mrpt::math::TPoint3D::x, mrpt::utils::TPixelCoordf::y, mrpt::math::TPoint3D::y, and mrpt::math::TPoint3D::z.

◆ saveToConfigFile()

void 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::CPTG_RobotShape_Circular, mrpt::nav::CPTG_RobotShape_Polygonal, mrpt::nav::CParameterizedTrajectoryGenerator, mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams, mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams, mrpt::nav::CPTG_DiffDrive_CollisionGridBased, mrpt::nav::CHolonomicND::TOptions, mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams, mrpt::nav::CHolonomicFullEval::TOptions, mrpt::nav::CReactiveNavigationSystem::TReactiveNavigatorParams, mrpt::nav::CHolonomicVFF::TOptions, mrpt::nav::CMultiObjectiveMotionOptimizerBase::TParamsBase, mrpt::maps::CPointCloudFilterByDistance::TOptions, mrpt::nav::CPTG_DiffDrive_C, mrpt::nav::CPTG_DiffDrive_alpha, mrpt::nav::CMultiObjMotionOpt_Scalarization::TParams, mrpt::nav::CPTG_DiffDrive_CC, mrpt::nav::CPTG_DiffDrive_CCS, mrpt::nav::CPTG_DiffDrive_CS, and mrpt::nav::CPTG_Holo_Blend.

Definition at line 31 of file CLoadableOptions.cpp.

References MRPT_UNUSED_PARAM.

Referenced by mrpt::utils::CLoadableOptions::dumpToTextStream(), and mrpt::utils::CLoadableOptions::saveToConfigFileName().

◆ saveToConfigFileName()

void 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

Definition at line 39 of file CLoadableOptions.cpp.

References mrpt::utils::CLoadableOptions::saveToConfigFile().

◆ secondInverseJacobian()

mrpt::math::CMatrixFixedNumeric<double,4,2> mrpt::vision::CCamModel::secondInverseJacobian ( ) const

inlineprivate

Definition at line 206 of file CCamModel.h.

◆ thirdInverseJacobian()

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

inlineprivate

Definition at line 214 of file CCamModel.h.

◆ undistort_point()

void 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)

Definition at line 121 of file CCamModel.cpp.

References ASSERT_, cam, mrpt::utils::TCamera::getDistortionParamsAsVector(), mrpt::utils::TCamera::intrinsicParams, and mrpt::vision::pinhole::undistort_points().

◆ unproject_3D_point()

void 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.

Definition at line 172 of file CCamModel.cpp.

References cam, mrpt::utils::TCamera::cx(), mrpt::utils::TCamera::cy(), mrpt::utils::TCamera::fx(), mrpt::utils::TCamera::fy(), mrpt::utils::TCamera::k1(), mrpt::math::square(), mrpt::utils::TPixelCoordf::x, mrpt::math::TPoint3D::x, mrpt::utils::TPixelCoordf::y, mrpt::math::TPoint3D::y, and mrpt::math::TPoint3D::z.

Member Data Documentation

◆ cam

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

The parameters of a camera.

Definition at line 36 of file CCamModel.h.

Referenced by distort_a_point(), jacob_undistor(), jacob_undistor_fm(), jacobian_project_with_distortion(), jacobian_unproject_with_distortion(), loadFromConfigFile(), project_3D_point(), undistort_point(), and unproject_3D_point().

Detailed Description

Classes

Public Member Functions

Public Attributes

Static Protected Member Functions

Private Member Functions

Constructor & Destructor Documentation

◆ CCamModel() [1/2]

◆ CCamModel() [2/2]

Member Function Documentation

◆ distort_a_point()

◆ dumpToConsole()

◆ dumpToTextStream()

◆ dumpVar_bool()

◆ dumpVar_double()

◆ dumpVar_float()

◆ dumpVar_int()

◆ dumpVar_string()

◆ firstInverseJacobian()

◆ getFullInverseModelWithJacobian()

◆ getFullJacobian()

◆ getFullJacobianT()

◆ getFullProjection()

◆ getFullProjectionT()

◆ getTemporaryVariablesForTransform()

◆ jacob_undistor()

◆ jacob_undistor_fm()

◆ jacobian_project_with_distortion()

◆ jacobian_unproject_with_distortion()

◆ loadFromConfigFile()

◆ loadFromConfigFileName()

◆ project_3D_point()

◆ saveToConfigFile()

◆ saveToConfigFileName()

◆ secondInverseJacobian()

◆ thirdInverseJacobian()

◆ undistort_point()

◆ unproject_3D_point()

Member Data Documentation

◆ cam