Namespace for operations on Anchored Homogeneous Points. More...

Detailed Description

Namespace for operations on Anchored Homogeneous Points.

Functions
template<class AHP , class P0 , class M >
void	split (const AHP &ahp, P0 &p0, M &m, double &rho)
	Split AHP.
template<class P0 , class M >
vec7	compose (const P0 &p0, const M &m, const double rho)
	Compose AHP.
template<class VF , class Vlf >
vec	fromFrame (const VF &F, const Vlf &ahpf)
template<class VF , class Vahpf , class Vahp , class MAHP_f , class MAHP_ahpf >
void	fromFrame (const VF &F, const Vahpf &ahpf, Vahp &ahp, MAHP_f &AHP_f, MAHP_ahpf &AHP_ahpf)
template<class VF , class Vahp >
vec	toFrame (const VF &F, const Vahp &ahp)
template<class VF , class Vahp , class Vahpf , class MAHPF_f , class MAHPF_ahp >
void	toFrame (const VF &F, const Vahp &ahp, Vahpf &ahpf, MAHPF_f &AHPF_f, MAHPF_ahp &AHPF_ahp)
template<class VA >
vec3	ahp2euc (const VA &ahp)
	Reparametrize to Euclidean.
template<class VA , class VE , class ME_a >
void	ahp2euc (const VA &ahp, VE &euc, ME_a &EUC_ahp)
	Reparametrize to Euclidean, with Jacobians.
template<class VS , class VA >
vec3	toBearingOnlyFrame (const VS &s, const VA &ahp)
	Bring landmark to bearing-only sensor frame (without range information).
template<class VS , class VA , class VV >
void	toBearingOnlyFrame (const VS &s, const VA &ahp, VV &v, double &dist)
	Bring landmark to bearing-only sensor frame (give distance information).
template<class VS , class VA , class VV , class MV_s , class MV_a >
void	toBearingOnlyFrame (const VS &s, const VA &ahp, VV &v, double &dist, MV_s &V_s, MV_a &V_ahp)
	Bring landmark to bearing-only sensor frame (give distance information).
template<class VS , class VLS >
vec7	fromBearingOnlyFrame (const VS &s, const VLS &v, const double _rho)
	AHP landmark from bearing-only retro-projection.
template<class VS , class VLS , class VA , class MA_s , class MA_v , class MA_rho >
void	fromBearingOnlyFrame (const VS &s, const VLS &v, const double _rho, VA &ahp, MA_s &AHP_s, MA_v &AHP_v, MA_rho &AHP_rho)
	AHP landmark from bearing-only retro-projection, with Jacobians.
template<class VS , class VA , class MA_s >
double	linearityScore (const VS &senpose, const VA &ahp, const MA_s &AHP)

Function Documentation

template<class VA >

vec3 jafar::rtslam::lmkAHP::ahp2euc ( const VA & ahp )

Reparametrize to Euclidean.

Parameters:

ahp	the anchored homogeneous point to be reparametrized.

Returns:: the Euclidean point.

Definition at line 156 of file ahpTools.hpp.

Referenced by ahp2euc(), and jafar::rtslam::LandmarkAnchoredHomogeneousPoint::reparametrize_func().

template<class VA , class VE , class ME_a >

void jafar::rtslam::lmkAHP::ahp2euc	(	const VA &	ahp,
		VE &	euc,
		ME_a &	EUC_ahp
	)

Reparametrize to Euclidean, with Jacobians.

Parameters:

ahp	the anchored homogeneous point to be reparametrized.
euc	the returned Euclidean point.
EUC_ahp	the Jacobian of the conversion.

Definition at line 168 of file ahpTools.hpp.

References ahp2euc().

template<class P0 , class M >

vec7 jafar::rtslam::lmkAHP::compose	(	const P0 &	p0,
		const M &	m,
		const double	rho
	)

Compose AHP.

This is the opposite as split()

Parameters:

p0	the output anchor.
m	the output director vector
rho	the homogeneous parameter (inverse-distance)

Definition at line 54 of file ahpTools.hpp.

template<class VS , class VLS >

vec7 jafar::rtslam::lmkAHP::fromBearingOnlyFrame	(	const VS &	s,
		const VLS &	v,
		const double	_rho
	)

AHP landmark from bearing-only retro-projection.

This function is the inverse of toBearingOnlyFrame(). It builds the Anchored Homogeneous Point (AHP) landmark from a sensor frame s, a retro-projected director vector v, and a inverse-distance proportional prior rho.

It uses the formula (See Sola etal. PAMI 2010):

AHP = [ t ; R(q) * v ; rho * norm(v) ]

Parameters:

s	the sensor frame s = [t ; q]
v	the retro-projected director vector in sensor frame
rho	the prior, proportional to inverse-distance

Returns:: the AHP landmark.

Definition at line 305 of file ahpTools.hpp.

References jafar::jmath::ublasExtra::norm_2(), and jafar::rtslam::quaternion::rotate().

template<class VS , class VLS , class VA , class MA_s , class MA_v , class MA_rho >

void jafar::rtslam::lmkAHP::fromBearingOnlyFrame	(	const VS &	s,
		const VLS &	v,
		const double	_rho,
		VA &	ahp,
		MA_s &	AHP_s,
		MA_v &	AHP_v,
		MA_rho &	AHP_rho
	)

AHP landmark from bearing-only retro-projection, with Jacobians.

This function is the inverse of toBearingOnlyFrame(). It builds the Anchored Homogeneous Point (AHP) landmark from a sensor frame s, a retro-projected director vector v, and a inverse-distance proportional prior rho.

It uses the formula (See Sola etal. PAMI 2010):

AHP = [ t ; R(q) * v ; rho * norm(v) ]

Parameters:

s	the sensor frame
v	the retro-projected director vector in sensor frame
rho	the prior, proportional to inverse-distance
ahp	the AHP landmark.
AHP_s	the Jacobian wrt s
AHP_v	the Jacobian wrt v
AHP_rho	the Jacobian wrt rho

Definition at line 334 of file ahpTools.hpp.

References jafar::jmath::ublasExtra::norm_2(), and jafar::rtslam::quaternion::rotate().

template<class AHP , class P0 , class M >

void jafar::rtslam::lmkAHP::split	(	const AHP &	ahp,
		P0 &	p0,
		M &	m,
		double &	rho
	)

Split AHP.

Parameters:

p0	the output anchor.
m	the output director vector
rho	the homogeneous parameter (inverse-distance)

Definition at line 38 of file ahpTools.hpp.

Referenced by toBearingOnlyFrame().

template<class VS , class VA >

vec3 jafar::rtslam::lmkAHP::toBearingOnlyFrame	(	const VS &	s,
		const VA &	ahp
	)

Bring landmark to bearing-only sensor frame (without range information).

For a landmark and sensor frame

ahp = [p0 m rho]
s = [t q],

this function computes the chain (See Sola etal. PAMI 2010):

R'(q) * ( m - (t - p0) * rho )

which is a vector in sensor frame in the direction of the landmark. The range information is lost.

Parameters:

s	the sensor frame
ahp	the AHP landmark

Returns:: the bearing-only landmark in sensor frame

Definition at line 197 of file ahpTools.hpp.

References jafar::rtslam::quaternion::rotateInv(), and split().

template<class VS , class VA , class VV >

void jafar::rtslam::lmkAHP::toBearingOnlyFrame	(	const VS &	s,
		const VA &	ahp,
		VV &	v,
		double &	dist
	)

Bring landmark to bearing-only sensor frame (give distance information).

For a landmark and sensor frame

ahp = [p0 m rho]
s = [t q],

this function computes the chain (See Sola etal. PAMI 2010):

v = R'(q) * ( m - (t - p0) * rho )

which is a vector in sensor frame in the direction of the landmark.

The range information is recuperated in dist as the distance from sensor to landmark.

Parameters:

s	the sensor frame
ahp	the AHP landmark
v	the bearing-only landmark in sensor frame
dist	the non-observable distance

Definition at line 228 of file ahpTools.hpp.

References jafar::jmath::ublasExtra::norm_2(), jafar::rtslam::quaternion::rotateInv(), and split().

template<class VS , class VA , class VV , class MV_s , class MV_a >

void jafar::rtslam::lmkAHP::toBearingOnlyFrame	(	const VS &	s,
		const VA &	ahp,
		VV &	v,
		double &	dist,
		MV_s &	V_s,
		MV_a &	V_ahp
	)

Bring landmark to bearing-only sensor frame (give distance information).

For a landmark and sensor frame

ahp = [p0 m rho]
s = [t q],

this function computes the chain (See Sola etal. PAMI 2010):

R'(q) * ( m - (t - p0) * rho )

which is a vector in sensor frame in the direction of the landmark.

The range information is recuperated in dist as the distance from sensor to landmark.

and returns the Jacobians wrt s and ahp.

Parameters:

s	the sensor frame
ahp	the AHP landmark
v	the bearing-only landmark in sensor frame
V_s	the Jacobian of v wrt s
V_ahp	the Jacobian of v wrt ahp

Definition at line 262 of file ahpTools.hpp.

References jafar::jmath::ublasExtra::norm_2(), jafar::rtslam::quaternion::rotateInv(), and split().

Detailed Description

Functions

Function Documentation