Namespace for quaternion algebra functions. More...

Detailed Description

Namespace for quaternion algebra functions.

Functions
vec4	identQ ()
	Null rotation quaternion.
template<class VecQ >
vec4	q2qc (const VecQ &q)
	Conjugate of quaternion.
template<class VecQ , class MatQC_q >
void	q2qc_by_dq (const VecQ &q, MatQC_q &QC_q)
	Jacobian of conjugated quaternion wrt input quaternion.
template<class VecQ , class VecQc , class MatQC_q >
void	q2qc (const VecQ &q, VecQc &qc, MatQC_q &QC_q)
	Conjugate of a quaternion, with Jacobians.
template<class VecQ >
mat33	q2R (const VecQ &q)
	Rotation matrix from quaternion.
template<class MatR >
vec4	R2q (const MatR &R)
	Quaternion from rotation matrix.
template<class VecQ >
mat33	q2Rt (const VecQ &q)
	The transposed rotation matrix from a quaternion.
template<class VecQ , class Vec >
vec3	rotate (const VecQ &q, const Vec &v)
	Rotate a vector from given quaternion.
template<class VecQ , class Vec , class MatVO_q >
void	rotate_by_dq (const VecQ &q, const Vec &v, MatVO_q &VO_q)
	Jacobian of vector rotation from quaternion, wrt quaternion.
template<class VecQ , class MatVO_v >
void	rotate_by_dv (const VecQ &q, MatVO_v &VO_v)
	Jacobian of vector rotation from quaternion, wrt vector.
template<class VecQ , class Vec , class VecO , class MatVO_q , class MatVO_v >
void	rotate (const VecQ &q, const Vec &v, VecO &vo, MatVO_q &VO_q, MatVO_v &VO_v)
	Rotate a vector from given quaternion, with all Jacobians.
template<class VecQ , class Vec >
vec3	rotateInv (const VecQ &q, const Vec &v)
	Rotate inversely a vector from given quaternion.
template<class VecQ , class Vec , class MatVO_q >
void	rotateInv_by_dq (const VecQ &q, const Vec &v, MatVO_q &VO_q)
	Jacobian of inverse vector rotation from quaternion, wrt quaternion.
template<class VecQ , class MatVO_v >
void	rotateInv_by_dv (const VecQ &q, MatVO_v &VO_v)
	Jacobian of vector inverse rotation from quaternion, wrt vector.
template<class VecQ , class Vec , class VecO , class MatVO_q , class MatVO_v >
void	rotateInv (const VecQ &q, const Vec &v, VecO &vo, MatVO_q &VO_q, MatVO_v &VO_v)
	Rotate inversely a vector from given quaternion, with all Jacobians.
template<class VecQ1 , class VecQ2 >
vec4	qProd (const VecQ1 &q1, const VecQ2 &q2)
	Quaternion product.
template<class VecQ2 , class MatQ_q1 >
void	qProd_by_dq1 (const VecQ2 &q2, MatQ_q1 &Q_q1)
	Jacobian of quaternion product wrt first quaternion, d(q1**q2)/dq1.
template<class VecQ1 , class MatQ_q2 >
void	qProd_by_dq2 (const VecQ1 &q1, MatQ_q2 &Q_q2)
	Jacobian of quaternion product wrt second quaternion, d(q1**q2)/dq2.
template<class VecQ1 , class VecQ2 , class VecQ , class MatQ_q1 , class MatQ_q2 >
void	qProd (const VecQ1 &q1, const VecQ2 &q2, VecQ &q, MatQ_q1 &Q_q1, MatQ_q2 &Q_q2)
	Quaternion product, with Jacobians.
template<class Vec >
vec4	v2q (const Vec &v)
	Quaternion from rotation vector.
template<class Vec , class MatQ_v >
void	v2q_by_dv (const Vec &v, MatQ_v &Q_v)
	Jacobian of quaternion wrt rotation vector.
template<class Vec , class VecQ , class MatQ_v >
void	v2q (const Vec &v, VecQ &q, MatQ_v &Q_v)
	Rotation vector to quaternion conversion, with Jacobian.
template<class Vec >
vec4	e2q (const Vec &e)
	Quaternion from Euler angles.
template<class Vec , class MatQ_e >
void	e2q_by_de (const Vec &e, MatQ_e &Q_e)
	Jacobian of quaternion wrt Euler angles.
template<class Vec , class VecQ , class MatQ_e >
void	e2q (const Vec &e, VecQ &q, MatQ_e &Q_e)
	Euler angles to quaternion conversion, with Jacobian.
template<class VecXE , class MatPE , class VecXQ , class MatPQ >
void	e2q (const VecXE &e, const MatPE &E, VecXQ &q, MatPQ &Q)
	Converts euler angles with its uncertainty to quaternion.
template<class VecE >
vec7	e2q_frame (const VecE &e)
template<class VecXE , class MatPE , class VecXQ , class MatPQ >
void	e2q_frame (const VecXE &e, const MatPE &E, VecXQ &q, MatPQ &Q)
template<class VecQ >
vec3	q2e (const VecQ &q)
	Quaternion to Euler angles conversion.
template<class VecQ , class VecE , class MatE_q >
void	q2e (const VecQ &q, VecE &e, MatE_q &E_q)
	Quaternion-to-Euler conversion, with Jacobian.
template<class VecXQ , class MatPQ , class VecXE , class MatPE >
void	q2e (const VecXQ &q, const MatPQ &Q, VecXE &e, MatPE &E)
	Converts a quaternion with its uncertainty to euler angles.
template<class VecQ >
vec6	q2e_frame (const VecQ &q)
template<class VecXQ , class MatPQ , class VecXE , class MatPE >
void	q2e_frame (const VecXQ &q, const MatPQ &Q, VecXE &e, MatPE &E)
vec7	originFrame ()
	Origin frame.
vec4	flu2rdfQuat ()
	Quaternion defining the RDF frame from a FLU frame.
vec7	flu2rdfFrame ()
	A frame at the origin position and the orientation at [-90, 0, -90] deg.
template<class VecF , class Pnt >
vec3	eucToFrame (const VecF &F, const Pnt &p)
	To-frame transformation for Euclidean points.
template<class VecF , class Pnt , class PntF , class MatPF_f , class MatPF_p >
void	eucToFrame (const VecF &F, const Pnt &p, PntF &pf, MatPF_f &PF_f, MatPF_p &PF_p, jblas::vec p_fej=jblas::vec(0))
	To-frame transformation for Euclidean points, with Jacobians.
template<class VecF , class Vec >
vec3	vecToFrame (const VecF &F, const Vec &v)
	To-frame transformation for vectors.
template<class VecF , class Vec , class Vecf , class MatVF_f , class MatVF_v >
void	vecToFrame (const VecF &F, const Vec &v, Vecf &vf, MatVF_f &VF_f, MatVF_v &VF_v)
	To-frame transformation for vectors, with Jacobians.
template<class VecF , class Pnt >
vec3	eucFromFrame (const VecF &F, const Pnt &pf)
	From-frame transformation for Euclidean points.
template<class VecF , class PntF , class Pnt , class MatP_f , class MatP_pf >
void	eucFromFrame (const VecF &F, const PntF &pf, Pnt &p, MatP_f &P_f, MatP_pf &P_pf)
	From-frame transformation for Euclidean points, with Jacobians.
template<class VecF , class Vec >
vec3	vecFromFrame (const VecF &F, const Vec &vf)
	From-frame transformation for vectors.
template<class VecF , class Vecf , class Vec , class MatV_f , class MatV_vf >
void	vecFromFrame (const VecF &F, const Vecf &vf, Vec &v, MatV_f &V_f, MatV_vf &V_vf)
	From-frame transformation for vectors, with Jacobians.
template<class VecG , class VecF >
jblas::vec7	composeFrames (const VecG &G, const VecF &F)
	Compose frames.
template<class VecG , class VecL , class MatC_g >
void	composeFrames_by_dglobal (const VecG &G, const VecL &L, MatC_g &C_g)
	Jacobian of frame composition wrt global frame.
template<class VecG , class VecL , class MatC_l >
void	composeFrames_by_dlocal (const VecG &G, const VecL &L, MatC_l &C_l)
	Jacobian of frame composition wrt global frame.
template<class VecG , class VecL , class VecC , class MatC_g , class MatC_l >
void	composeFrames (const VecG &G, const VecL &L, VecC &C, MatC_g &C_g, MatC_l &C_l)
	Compose frames, give Jacobians.
template<class VecF >
jblas::vec7	invertFrame (const VecF &F)
	Invert frame.
template<class VecF , class VecI , class MatI_f >
void	invertFrame (const VecF &F, VecI &I, MatI_f &I_f)
	Invert frame, with Jacobian.
template<class VecF1 , class VecF2 >
jblas::vec7	frameIncrement (const VecF1 &F1, const VecF2 &F2)
	Frame increment.
template<class VecF1 , class VecF2 , class MatF_f1 >
void	frameIncrement_by_f1 (const VecF1 &F1, const VecF2 &F2, MatF_f1 &F_f1)
	Frame increment, first Jacobian.
template<class VecF1 , class VecF2 , class MatF_f2 >
void	frameIncrement_by_f2 (const VecF1 &F1, const VecF2 &F2, MatF_f2 &F_f2)
	Frame increment, second Jacobian.
template<class VecF1 , class VecF2 , class VecF , class MatF_f1 , class MatF_f2 >
void	frameIncrement (const VecF1 &F1, const VecF2 &F2, VecF &F, MatF_f1 &F_f1, MatF_f2 &F_f2)
	Frame increment, first Jacobian.
template<class Vec1 , class Vec2 , class VecL >
void	getZoomRotation (const Vec1 &sensorPose1, const Vec2 &sensorPose2, const VecL &landmarkPosition, double &zoom, double &rotation)
	get zoom and rotation of appearance between two points of view

Function Documentation

template<class VecG , class VecF >

jblas::vec7 jafar::rtslam::quaternion::composeFrames	(	const VecG &	G,
		const VecF &	F
	)

Compose frames.

Parameters:

G	the global frame
F	the local frame (expressed in G)

Returns:: the composed frame GoF

Definition at line 919 of file quatTools.hpp.

References eucFromFrame(), and qProd().

Referenced by frameIncrement(), and jafar::rtslam::RobotAbstract::slamPoseToRobotPose().

template<class VecG , class VecL , class VecC , class MatC_g , class MatC_l >

void jafar::rtslam::quaternion::composeFrames	(	const VecG &	G,
		const VecL &	L,
		VecC &	C,
		MatC_g &	C_g,
		MatC_l &	C_l
	)

Compose frames, give Jacobians.

Parameters:

G	the global frame
L	the local frame
C	the composed frame GoL
C_g	the Jacobian of C wrt G
C_l	the Jacobian wrt L

Definition at line 987 of file quatTools.hpp.

References eucFromFrame(), and qProd().

template<class VecG , class VecL , class MatC_g >

void jafar::rtslam::quaternion::composeFrames_by_dglobal	(	const VecG &	G,
		const VecL &	L,
		MatC_g &	C_g
	)

Jacobian of frame composition wrt global frame.

Parameters:

G	the global frame
L	the local frame
C_g	the Jacobian of C wrt G

Definition at line 938 of file quatTools.hpp.

References qProd_by_dq1(), and rotate_by_dq().

Referenced by frameIncrement_by_f1(), and jafar::rtslam::RobotAbstract::slamPoseToRobotPose().

template<class VecG , class VecL , class MatC_l >

void jafar::rtslam::quaternion::composeFrames_by_dlocal	(	const VecG &	G,
		const VecL &	L,
		MatC_l &	C_l
	)

Jacobian of frame composition wrt global frame.

Parameters:

G	the global frame
L	the local frame
C_g	the Jacobian of C wrt G

Definition at line 964 of file quatTools.hpp.

References qProd_by_dq2(), and rotate_by_dv().

Referenced by frameIncrement_by_f2().

template<class Vec >

vec4 jafar::rtslam::quaternion::e2q ( const Vec & e )

Quaternion from Euler angles.

Parameters:

e	the Euler angles [roll, pitch, yaw]

Returns:: the quaternion [qw, qx, qy, qz]

Definition at line 520 of file quatTools.hpp.

References qProd(), and v2q().

Referenced by e2q(), and jafar::rtslam::SensorAbsloc::process().

template<class Vec , class VecQ , class MatQ_e >

void jafar::rtslam::quaternion::e2q	(	const Vec &	e,
		VecQ &	q,
		MatQ_e &	Q_e
	)

Euler angles to quaternion conversion, with Jacobian.

Parameters:

e	the Euler angles [roll, pitch, yaw]
q	the output quaternion [qw, qx, qy, qz]
Q_e	the Jacobian

Definition at line 575 of file quatTools.hpp.

References e2q(), and e2q_by_de().

template<class VecXE , class MatPE , class VecXQ , class MatPQ >

void jafar::rtslam::quaternion::e2q	(	const VecXE &	e,
		const MatPE &	E,
		VecXQ &	q,
		MatPQ &	Q
	)

Converts euler angles with its uncertainty to quaternion.

Parameters:

e	the euler angles
E	the covariance of the euler angles
q	the quaternion
Q	the covariance of the quaternion

Definition at line 588 of file quatTools.hpp.

References e2q(), and jafar::jmath::ublasExtra::prod_JPJt().

template<class Vec , class MatQ_e >

void jafar::rtslam::quaternion::e2q_by_de	(	const Vec &	e,
		MatQ_e &	Q_e
	)

Jacobian of quaternion wrt Euler angles.

Parameters:

e	the Euler angles [roll, pitch, yaw]
Q_e	the output Jacobian

Definition at line 545 of file quatTools.hpp.

Referenced by e2q().

template<class VecF , class Pnt >

vec3 jafar::rtslam::quaternion::eucFromFrame	(	const VecF &	F,
		const Pnt &	pf
	)

From-frame transformation for Euclidean points.

Parameters:

F	frame
pf	point in frame F

Returns:: the point in global frame

Definition at line 849 of file quatTools.hpp.

References rotate().

Referenced by composeFrames().

template<class VecF , class PntF , class Pnt , class MatP_f , class MatP_pf >

void jafar::rtslam::quaternion::eucFromFrame	(	const VecF &	F,
		const PntF &	pf,
		Pnt &	p,
		MatP_f &	P_f,
		MatP_pf &	P_pf
	)

From-frame transformation for Euclidean points, with Jacobians.

Parameters:

F	frame
pf	point in frame F
p	the point in global frame
P_f	the Jacobian wrt F
P_pf	the Jacobian wrt pf

Definition at line 866 of file quatTools.hpp.

References rotate().

template<class VecF , class Pnt >

vec3 jafar::rtslam::quaternion::eucToFrame	(	const VecF &	F,
		const Pnt &	p
	)

To-frame transformation for Euclidean points.

Parameters:

F	frame
p	point in global frame

Returns:: the point in frame F

Definition at line 774 of file quatTools.hpp.

References rotateInv().

template<class VecF , class Pnt , class PntF , class MatPF_f , class MatPF_p >

void jafar::rtslam::quaternion::eucToFrame	(	const VecF &	F,
		const Pnt &	p,
		PntF &	pf,
		MatPF_f &	PF_f,
		MatPF_p &	PF_p,
		jblas::vec	p_fej = `jblas::vec(0)`
	)

To-frame transformation for Euclidean points, with Jacobians.

Parameters:

F	frame
p	point in global frame
pf	the point in frame F
PF_f	the Jacobian wrt F
PF_p	the Jacobian wrt p

Definition at line 796 of file quatTools.hpp.

References rotateInv(), and rotateInv_by_dq().

vec7 jafar::rtslam::quaternion::flu2rdfFrame ( )

A frame at the origin position and the orientation at [-90, 0, -90] deg.

See also:: flu2rdfQuat()

vec4 jafar::rtslam::quaternion::flu2rdfQuat ( )

Quaternion defining the RDF frame from a FLU frame.

RDF: x-Right, y-Down, z-Front frame used for pin-hole cameras. FLU: x-Front, y-Left, z-Up frame used for mobile objects. This transformation is the typical expressed by Euler angles [-90 0 -90] deg.

Returns:: a quaternion q = [0.5 -0.5 0.5 -0.5]

template<class VecF1 , class VecF2 >

jblas::vec7 jafar::rtslam::quaternion::frameIncrement	(	const VecF1 &	F1,
		const VecF2 &	F2
	)

Frame increment.

Returns F = composeFrames(invertFrame(F1) , F2)

Parameters:

F1	the first frame
F2	the second frame

Returns:: the frame F2 expressed in F1

Definition at line 1060 of file quatTools.hpp.

References composeFrames(), and invertFrame().

template<class VecF1 , class VecF2 , class VecF , class MatF_f1 , class MatF_f2 >

void jafar::rtslam::quaternion::frameIncrement	(	const VecF1 &	F1,
		const VecF2 &	F2,
		VecF &	F,
		MatF_f1 &	F_f1,
		MatF_f2 &	F_f2
	)

Frame increment, first Jacobian.

Parameters:

F1	the first frame
F2	the second frame
F	the incremental frame from F1 to F2
F_f1	the Jacobian of frameIncrement() wrt F1
F_f2	the Jacobian of frameIncrement() wrt F2

Definition at line 1099 of file quatTools.hpp.

References composeFrames(), and invertFrame().

template<class VecF1 , class VecF2 , class MatF_f1 >

void jafar::rtslam::quaternion::frameIncrement_by_f1	(	const VecF1 &	F1,
		const VecF2 &	F2,
		MatF_f1 &	F_f1
	)

Frame increment, first Jacobian.

Parameters:

F1	the first frame
F2	the second frame
F_f1	the Jacobian of frameIncrement() wrt F1

Definition at line 1071 of file quatTools.hpp.

References composeFrames_by_dglobal(), and invertFrame().

template<class VecF1 , class VecF2 , class MatF_f2 >

void jafar::rtslam::quaternion::frameIncrement_by_f2	(	const VecF1 &	F1,
		const VecF2 &	F2,
		MatF_f2 &	F_f2
	)

Frame increment, second Jacobian.

Parameters:

F1	the first frame
F2	the second frame
F_f2	the Jacobian of frameIncrement() wrt F2

Definition at line 1086 of file quatTools.hpp.

References composeFrames_by_dlocal(), and invertFrame().

vec4 jafar::rtslam::quaternion::identQ ( )

Null rotation quaternion.

Returns:: the identity quaternion q = [1, 0, 0, 0]

Referenced by v2q().

template<class VecF >

jblas::vec7 jafar::rtslam::quaternion::invertFrame ( const VecF & F )

Invert frame.

Parameters:

F	the frame to invert.

Returns:: the inverse of F.

Definition at line 1014 of file quatTools.hpp.

References q2qc(), and rotateInv().

Referenced by frameIncrement(), frameIncrement_by_f1(), and frameIncrement_by_f2().

template<class VecF , class VecI , class MatI_f >

void jafar::rtslam::quaternion::invertFrame	(	const VecF &	F,
		VecI &	I,
		MatI_f &	I_f
	)

Invert frame, with Jacobian.

Parameters:

F	the frame to invert.
I	the inverse of F.
I_f	the Jacobian

Definition at line 1031 of file quatTools.hpp.

References q2qc(), and rotateInv().

vec7 jafar::rtslam::quaternion::originFrame ( )

Origin frame.

Returns:: a pose at the origin, F = [0 0 0 1 0 0 0]

template<class VecQ >

vec3 jafar::rtslam::quaternion::q2e ( const VecQ & q )

Quaternion to Euler angles conversion.

Parameters:

q	the quaternion [qw, qx, qy, qz]

Returns:: the Euler angles [roll, pitch, yaw]

Definition at line 624 of file quatTools.hpp.

Referenced by getZoomRotation(), jafar::rtslam::SensorAbsloc::process(), q2e(), and jafar::rtslam::RobotAbstract::slamPoseToRobotPose().

template<class VecQ , class VecE , class MatE_q >

void jafar::rtslam::quaternion::q2e	(	const VecQ &	q,
		VecE &	e,
		MatE_q &	E_q
	)

Quaternion-to-Euler conversion, with Jacobian.

Parameters:

q	the quaternion [qw, qx, qy, qz]
e	the Euler angles [roll, pitch, yaw]
E_q	the Jacobian

Definition at line 645 of file quatTools.hpp.

template<class VecXQ , class MatPQ , class VecXE , class MatPE >

void jafar::rtslam::quaternion::q2e	(	const VecXQ &	q,
		const MatPQ &	Q,
		VecXE &	e,
		MatPE &	E
	)

Converts a quaternion with its uncertainty to euler angles.

Parameters:

q	the quaternion
Q	the covariance of the quaternion
e	the euler angles
E	the covariance of the euler angles

Definition at line 716 of file quatTools.hpp.

References jafar::jmath::ublasExtra::prod_JPJt(), and q2e().

template<class VecQ >

vec4 jafar::rtslam::quaternion::q2qc ( const VecQ & q )

Conjugate of quaternion.

Parameters:

q	The input quaternion [qw, qx, qy, qz].

Returns:: the conjugated quaternion.

Definition at line 45 of file quatTools.hpp.

Referenced by getZoomRotation(), invertFrame(), jafar::rtslam::SensorAbsloc::process(), q2qc(), and q2Rt().

template<class VecQ , class MatQC_q >

void jafar::rtslam::quaternion::q2qc_by_dq	(	const VecQ &	q,
		MatQC_q &	QC_q
	)

Jacobian of conjugated quaternion wrt input quaternion.

Jac = QC_q = dqc/dq.

Parameters:

q	the input quaternion [qw, qx, qy, qz]
QC_q	the Jacobian.

Definition at line 63 of file quatTools.hpp.

Referenced by q2qc().

template<class VecQ >

mat33 jafar::rtslam::quaternion::q2Rt ( const VecQ & q )

The transposed rotation matrix from a quaternion.

Parameters:

q	the quaternion [qw, qx, qy, qz].

Returns:: the transposed rotation matrix.

Definition at line 136 of file quatTools.hpp.

References q2qc(), and q2R().

Referenced by rotateInv(), and rotateInv_by_dv().

template<class VecQ1 , class VecQ2 >

vec4 jafar::rtslam::quaternion::qProd	(	const VecQ1 &	q1,
		const VecQ2 &	q2
	)

Quaternion product.

Parameters:

q1	the first quaternion [qw, qx, qy, qz]
q2	the second quaternion

Returns:: the product q1**q2, where ** means quaternion product

Definition at line 310 of file quatTools.hpp.

Referenced by composeFrames(), e2q(), getZoomRotation(), jafar::rtslam::SensorAbsloc::process(), and qProd().

template<class VecQ1 , class VecQ2 , class VecQ , class MatQ_q1 , class MatQ_q2 >

void jafar::rtslam::quaternion::qProd	(	const VecQ1 &	q1,
		const VecQ2 &	q2,
		VecQ &	q,
		MatQ_q1 &	Q_q1,
		MatQ_q2 &	Q_q2
	)

Quaternion product, with Jacobians.

Parameters:

q1	the first quaternion [qw, qx, qy, qz]
q2	the second quaternion
q	the output q = q1**q2
Q_q1	the Jacobian of q wrt q1
Q_q2	the Jacobian of q wrt q2

Definition at line 412 of file quatTools.hpp.

References qProd(), qProd_by_dq1(), and qProd_by_dq2().

template<class VecQ2 , class MatQ_q1 >

void jafar::rtslam::quaternion::qProd_by_dq1	(	const VecQ2 &	q2,
		MatQ_q1 &	Q_q1
	)

Jacobian of quaternion product wrt first quaternion, d(q1**q2)/dq1.

This Jacobian only depends on q2.

Parameters:

q2	the second quaternion [qw, qx, qy, qz] of the product
Q_q1	the output Jacobian

Definition at line 338 of file quatTools.hpp.

Referenced by composeFrames_by_dglobal(), jafar::rtslam::SensorAbsloc::process(), and qProd().

template<class VecQ1 , class MatQ_q2 >

void jafar::rtslam::quaternion::qProd_by_dq2	(	const VecQ1 &	q1,
		MatQ_q2 &	Q_q2
	)

Jacobian of quaternion product wrt second quaternion, d(q1**q2)/dq2.

This Jacobian only depends on q1.

Parameters:

q1	the first quaternion [qw, qx, qy, qz] of the product
Q_q2	the output Jacobian

Definition at line 375 of file quatTools.hpp.

Referenced by composeFrames_by_dlocal(), and qProd().

template<class VecQ , class Vec >

vec3 jafar::rtslam::quaternion::rotate	(	const VecQ &	q,
		const Vec &	v
	)

Rotate a vector from given quaternion.

Parameters:

q	the quaternion [qw, qx, qy, qz].
v	the vector.

Returns:: the rotated vector.

Definition at line 148 of file quatTools.hpp.

References q2R().

Referenced by eucFromFrame(), jafar::rtslam::lmkAHP::fromBearingOnlyFrame(), jafar::rtslam::lmkAHPL::fromBearingOnlyFrame(), jafar::rtslam::SensorAbsloc::process(), rotate(), and vecFromFrame().

template<class VecQ , class Vec , class VecO , class MatVO_q , class MatVO_v >

void jafar::rtslam::quaternion::rotate	(	const VecQ &	q,
		const Vec &	v,
		VecO &	vo,
		MatVO_q &	VO_q,
		MatVO_v &	VO_v
	)

Rotate a vector from given quaternion, with all Jacobians.

Parameters:

q	the quaternion [qw, qx, qy, qz];
v	the vector
vo	the rotated vector
VO_q	the Jacobian wrt q
VO_v	the Jacobian wrt v

Definition at line 215 of file quatTools.hpp.

References rotate(), rotate_by_dq(), and rotate_by_dv().

template<class VecQ , class Vec , class MatVO_q >

void jafar::rtslam::quaternion::rotate_by_dq	(	const VecQ &	q,
		const Vec &	v,
		MatVO_q &	VO_q
	)

Jacobian of vector rotation from quaternion, wrt quaternion.

Parameters:

q	the quaternion [qw, qx, qy, qz]
v	the vector
VO_q	the Jacobian of R(q)*v wrt q

Definition at line 161 of file quatTools.hpp.

Referenced by composeFrames_by_dglobal(), jafar::rtslam::SensorAbsloc::process(), and rotate().

template<class VecQ , class MatVO_v >

void jafar::rtslam::quaternion::rotate_by_dv	(	const VecQ &	q,
		MatVO_v &	VO_v
	)

Jacobian of vector rotation from quaternion, wrt vector.

Parameters:

q	the quaternion [qw, qx, qy, qz]
VO_v	= R(q) the Jacobian of R(q)*v wrt v

Definition at line 201 of file quatTools.hpp.

References q2R().

Referenced by composeFrames_by_dlocal(), and rotate().

template<class VecQ , class Vec >

vec3 jafar::rtslam::quaternion::rotateInv	(	const VecQ &	q,
		const Vec &	v
	)

Rotate inversely a vector from given quaternion.

Parameters:

q	the quaternion [qw, qx, qy, qz];
v	the vector

Returns:: the rotated vector

Definition at line 229 of file quatTools.hpp.

References q2Rt().

Referenced by eucToFrame(), invertFrame(), jafar::rtslam::lmkAHP::toBearingOnlyFrame(), jafar::rtslam::lmkAHPL::toBearingOnlyFrame(), and vecToFrame().

template<class VecQ , class Vec , class VecO , class MatVO_q , class MatVO_v >

void jafar::rtslam::quaternion::rotateInv	(	const VecQ &	q,
		const Vec &	v,
		VecO &	vo,
		MatVO_q &	VO_q,
		MatVO_v &	VO_v
	)

Rotate inversely a vector from given quaternion, with all Jacobians.

Parameters:

q	the quaternion [qw, qx, qy, qz];
v	the vector
vo	the rotated vector
VO_q	the Jacobian wrt q
VO_v	the Jacobian wrt v

Definition at line 296 of file quatTools.hpp.

References q2Rt(), and rotateInv_by_dq().

template<class VecQ , class Vec , class MatVO_q >

void jafar::rtslam::quaternion::rotateInv_by_dq	(	const VecQ &	q,
		const Vec &	v,
		MatVO_q &	VO_q
	)

Jacobian of inverse vector rotation from quaternion, wrt quaternion.

Parameters:

q	the quaternion [qw, qx, qy, qz]
v	the vector
VO_q	the Jacobian of R(q)'*v wrt q

Definition at line 242 of file quatTools.hpp.

Referenced by eucToFrame(), and rotateInv().

template<class VecQ , class MatVO_v >

void jafar::rtslam::quaternion::rotateInv_by_dv	(	const VecQ &	q,
		MatVO_v &	VO_v
	)

Jacobian of vector inverse rotation from quaternion, wrt vector.

Parameters:

q	the quaternion [qw, qx, qy, qz]
VO_v	= R(q) the Jacobian of R(q)'*v wrt v

Definition at line 282 of file quatTools.hpp.

References q2Rt().

template<class Vec >

vec4 jafar::rtslam::quaternion::v2q ( const Vec & v )

Quaternion from rotation vector.

Parameters:

v	the rotation vector

Returns:: the quaternion [qw, qx, qy, qz]

Definition at line 425 of file quatTools.hpp.

References identQ().

Referenced by e2q(), and v2q().

template<class Vec , class VecQ , class MatQ_v >

void jafar::rtslam::quaternion::v2q	(	const Vec &	v,
		VecQ &	q,
		MatQ_v &	Q_v
	)

Rotation vector to quaternion conversion, with Jacobian.

Parameters:

v	the rotation vector
q	the output quaternion [qw, qx, qy, qz]
Q_v	the Jacobian

Definition at line 508 of file quatTools.hpp.

References v2q(), and v2q_by_dv().

template<class Vec , class MatQ_v >

void jafar::rtslam::quaternion::v2q_by_dv	(	const Vec &	v,
		MatQ_v &	Q_v
	)

Jacobian of quaternion wrt rotation vector.

Parameters:

v	the rotation vector
Q_v	the output Jacobian

Definition at line 446 of file quatTools.hpp.

References jafar::jmath::ublasExtra::norm_2(), and jafar::jmath::ublasExtra::normalizeJac().

Referenced by v2q().

template<class VecF , class Vec >

vec3 jafar::rtslam::quaternion::vecFromFrame	(	const VecF &	F,
		const Vec &	vf
	)

From-frame transformation for vectors.

Parameters:

F	frame
vf	vector in frame F

Returns:: the vector in global frame

Definition at line 886 of file quatTools.hpp.

References rotate().

template<class VecF , class Vecf , class Vec , class MatV_f , class MatV_vf >

void jafar::rtslam::quaternion::vecFromFrame	(	const VecF &	F,
		const Vecf &	vf,
		Vec &	v,
		MatV_f &	V_f,
		MatV_vf &	V_vf
	)

From-frame transformation for vectors, with Jacobians.

Parameters:

F	frame
vf	vector in frame F
v	the point in global frame
V_f	the Jacobian wrt F
V_vf	the Jacobian wrt vf

Definition at line 903 of file quatTools.hpp.

References rotate().

template<class VecF , class Vec >

vec3 jafar::rtslam::quaternion::vecToFrame	(	const VecF &	F,
		const Vec &	v
	)

To-frame transformation for vectors.

Parameters:

F	frame
v	vector in global frame

Returns:: the vector in frame F

Definition at line 817 of file quatTools.hpp.

References rotateInv().

template<class VecF , class Vec , class Vecf , class MatVF_f , class MatVF_v >

void jafar::rtslam::quaternion::vecToFrame	(	const VecF &	F,
		const Vec &	v,
		Vecf &	vf,
		MatVF_f &	VF_f,
		MatVF_v &	VF_v
	)

To-frame transformation for vectors, with Jacobians.

Parameters:

F	frame
v	vector in global frame
vf	the point in frame F
VF_f	the Jacobian wrt F
VF_v	the Jacobian wrt v

Definition at line 831 of file quatTools.hpp.

References rotateInv().

Detailed Description

Functions

Function Documentation