Detailed Description

A collection of functions to compute jacobians of diverse transformations, etc (some functions are redirections to existing methods elsewhere, so this namespace is actually used with grouping purposes).

Since most functions in this namespace are inline, their use implies no execution time overload and the code may be more clear to read, so it's recommended to use them where needed.

Functions
void	jacob_quat_from_yawpitchroll (CMatrixFixedNumeric< double, 4, 3 > &out_dq_dr, const double yaw, const double pitch, const double roll)
	Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of: $\mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right)$ With : $\phi = roll$ , $\theta = pitch$ and $\psi = yaw$ .

void	jacob_quat_from_yawpitchroll (CMatrixFixedNumeric< double, 4, 3 > &out_dq_dr, const CPose3D &in_pose)
	Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of: $\mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right)$ With : $\phi = roll$ , $\theta = pitch$ and $\psi = yaw$ .

void	jacob_yawpitchroll_from_quat (CMatrixFixedNumeric< double, 3, 4 > &out_dr_dq)
	Computes the 3x4 Jacobian of the transformation from a quaternion (qr qx qy qz) to 3D pose angles (yaw pitch roll).

template<class QUATERNION , class MATRIXLIKE >
void	jacob_quat_rotation (const QUATERNION &quaternion, MATRIXLIKE &out_mat4x4)
	Compute the Jacobian of the rotation composition operation $p = f(\cdot) = q_{this} \times r$ , that is the 4x4 matrix $\frac{\partial f}{\partial q_{this} }$ .

void	jacobs_6D_pose_comp (const CPose3D &x, const CPose3D &u, CMatrixDouble66 &out_df_dx, CMatrixDouble66 &out_df_du)
	Given the 3D(6D) pose composition $f(x,u) = x \oplus u$ , compute the two 6x6 Jacobians $\frac{\partial f}{\partial x}$ and $\frac{\partial f}{\partial u}$ .

void	jacobs_6D_pose_comp (const CPose3DQuat &x, const CPose3DQuat &u, CMatrixDouble77 &out_df_dx, CMatrixDouble77 &out_df_du)
	Given the 3D(6D) pose composition $f(x,u) = x \oplus u$ , compute the two 6x6 Jacobians $\frac{\partial f}{\partial x}$ and $\frac{\partial f}{\partial u}$ .

void	jacobs_2D_pose_comp (const CPosePDFGaussian &x, const CPosePDFGaussian &u, CMatrixDouble33 &out_df_dx, CMatrixDouble33 &out_df_du)
	Given the 2D pose composition $f(x,u) = x \oplus u$ , compute the two 3x3 Jacobians $\frac{\partial f}{\partial x}$ and $\frac{\partial f}{\partial u}$ .

template<class VECTORLIKE , class VECTORLIKE2 , class VECTORLIKE3 , class MATRIXLIKE , class USERPARAM >
void	jacob_numeric_estimate (const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
	Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::math::estimateJacobian, see that function for documentation.

Function Documentation

template<class VECTORLIKE , class VECTORLIKE2 , class VECTORLIKE3 , class MATRIXLIKE , class USERPARAM >

void mrpt::math::jacobians::jacob_numeric_estimate	(	const VECTORLIKE &	x,
		void(*)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)	functor,
		const VECTORLIKE2 &	increments,
		const USERPARAM &	userParam,
		MATRIXLIKE &	out_Jacobian
	)

inline

Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::math::estimateJacobian, see that function for documentation.

Definition at line 141 of file jacobians.h.

References mrpt::math::estimateJacobian().

Referenced by mrpt::math::transform_gaussian_linear().

void mrpt::math::jacobians::jacob_quat_from_yawpitchroll	(	CMatrixFixedNumeric< double, 4, 3 > &	out_dq_dr,
		const double	yaw,
		const double	pitch,
		const double	roll
	)

inline

Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:

$\mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right)$

With : $\phi = roll$ , $\theta = pitch$ and $\psi = yaw$ .

See Also: jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion

Definition at line 57 of file jacobians.h.

References mrpt::poses::CPose3D::getAsQuaternion(), and mrpt::math::UNINITIALIZED_QUATERNION.

void mrpt::math::jacobians::jacob_quat_from_yawpitchroll	(	CMatrixFixedNumeric< double, 4, 3 > &	out_dq_dr,
		const CPose3D &	in_pose
	)

inline

Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:

$\mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right)$

With : $\phi = roll$ , $\theta = pitch$ and $\psi = yaw$ .

See Also: jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion

Definition at line 74 of file jacobians.h.

References mrpt::poses::CPose3D::getAsQuaternion(), and mrpt::math::UNINITIALIZED_QUATERNION.

template<class QUATERNION , class MATRIXLIKE >

void mrpt::math::jacobians::jacob_quat_rotation	(	const QUATERNION &	quaternion,
		MATRIXLIKE &	out_mat4x4
	)

inline

Compute the Jacobian of the rotation composition operation $p = f(\cdot) = q_{this} \times r$ , that is the 4x4 matrix $\frac{\partial f}{\partial q_{this} }$ .

The output matrix can be a dynamic or fixed size (4x4) matrix. The input quaternion can be mrpt::math::CQuaternionFloat or mrpt::math::CQuaternionDouble.

Definition at line 98 of file jacobians.h.

void mrpt::math::jacobians::jacob_yawpitchroll_from_quat ( CMatrixFixedNumeric< double, 3, 4 > & out_dr_dq )

inline

Computes the 3x4 Jacobian of the transformation from a quaternion (qr qx qy qz) to 3D pose angles (yaw pitch roll).

See Also: jacob_quat_from_yawpitchroll

Definition at line 87 of file jacobians.h.

References THROW_EXCEPTION.

void mrpt::math::jacobians::jacobs_2D_pose_comp	(	const CPosePDFGaussian &	x,
		const CPosePDFGaussian &	u,
		CMatrixDouble33 &	out_df_dx,
		CMatrixDouble33 &	out_df_du
	)

inline

Given the 2D pose composition $f(x,u) = x \oplus u$ , compute the two 3x3 Jacobians $\frac{\partial f}{\partial x}$ and $\frac{\partial f}{\partial u}$ .

For the equations, see CPosePDF::jacobiansPoseComposition

Definition at line 130 of file jacobians.h.

void mrpt::math::jacobians::jacobs_6D_pose_comp	(	const CPose3D &	x,
		const CPose3D &	u,
		CMatrixDouble66 &	out_df_dx,
		CMatrixDouble66 &	out_df_du
	)

inline

Given the 3D(6D) pose composition $f(x,u) = x \oplus u$ , compute the two 6x6 Jacobians $\frac{\partial f}{\partial x}$ and $\frac{\partial f}{\partial u}$ .

For the equations, see CPose3DPDF::jacobiansPoseComposition

Definition at line 106 of file jacobians.h.

void mrpt::math::jacobians::jacobs_6D_pose_comp	(	const CPose3DQuat &	x,
		const CPose3DQuat &	u,
		CMatrixDouble77 &	out_df_dx,
		CMatrixDouble77 &	out_df_du
	)

inline

Given the 3D(6D) pose composition $f(x,u) = x \oplus u$ , compute the two 6x6 Jacobians $\frac{\partial f}{\partial x}$ and $\frac{\partial f}{\partial u}$ .

For the equations, see CPose3DQuatPDF::jacobiansPoseComposition

Definition at line 118 of file jacobians.h.

Detailed Description

Functions

Function Documentation