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

Detailed Description

Namespace for operations on Anchored Homogeneous Points Lines.

Functions
template<class AHPL , class P0 , class M >
void	split (const AHPL &ahpl, P0 &p0, M &m1, M &m2, double &rho1, double &rho2)
	Split AHPL.
template<class P0 , class M >
vec11	compose (const P0 &p0, const M &m1, const M &m2, const double rho1, const double rho2)
	Compose AHP.
template<class VF , class Vlf >
vec	fromFrame (const VF &F, const Vlf &ahplf)
template<class VF , class Vahplf , class Vahpl , class MAHPL_f , class MAHPL_ahplf >
void	fromFrame (const VF &F, const Vahplf &ahplf, Vahpl &ahpl, MAHPL_f &AHPL_f, MAHPL_ahplf &AHPL_ahplf)
template<class VF , class Vahpl >
vec	toFrame (const VF &F, const Vahpl &ahpl)
template<class VF , class Vahpl , class Vahplf , class MAHPLF_f , class MAHPLF_ahp >
void	toFrame (const VF &F, const Vahpl &ahpl, Vahplf &ahplf, MAHPLF_f &AHPLF_f, MAHPLF_ahp &AHPLF_ahpl)
template<class VA >
vec6	ahpl2euc (const VA &ahpl)
	Reparametrize to Euclidean.
template<class VS , class VA , class VV >
void	toBearingOnlyFrame (const VS &s, const VA &ahpl, VV &v1, VV &v2)
	Reparametrize to Euclidean, with Jacobians.
template<class VS , class VA , class VV >
void	toBearingOnlyFrame (const VS &s, const VA &ahpl, VV &v1, VV &v2, double &dist1, double &dist2)
	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 &ahpl, VV &v1, VV &v2, double &dist1, double &dist2, MV_s &V1_s, MV_a &V1_ahpl, MV_s &V2_s, MV_a &V2_ahpl)
	Bring landmark to bearing-only sensor frame (give distance information).
template<class VS , class VLS >
vec11	fromBearingOnlyFrame (const VS &s, const VLS &v1, const VLS &v2, const double _rho1, const double _rho2)
	AHP landmark from bearing-only retro-projection.
void	fromBearingOnlyFrame (const vec7 &s, const vec3 &v1, const vec3 &v2, const double _rho1, const double _rho2, vec &ahpl, mat &AHPL_s, mat &AHPL_v1, mat &AHPL_v2, mat &AHPL_rho1, mat &AHPL_rho2)
	AHP landmark from bearing-only retro-projection, with Jacobians.
template<class VS , class VA , class MA_s >
double	linearityScore (const VS &senpose, const VA &ahpl, const MA_s &AHPL)

Function Documentation

template<class VA >

vec6 jafar::rtslam::lmkAHPL::ahpl2euc ( const VA & ahpl )

Reparametrize to Euclidean.

Parameters:

ahp	the anchored homogeneous point to be reparametrized.

Returns:: the Euclidean points.

Definition at line 189 of file ahplTools.hpp.

template<class P0 , class M >

vec11 jafar::rtslam::lmkAHPL::compose	(	const P0 &	p0,
		const M &	m1,
		const M &	m2,
		const double	rho1,
		const double	rho2
	)

Compose AHP.

This is the opposite as split()

Parameters:

m1	the first output director vector
m2	the second output director vector
rho1	the first homogeneous parameter (inverse-distance)
rho2	the second homogeneous parameter (inverse-distance)

Definition at line 59 of file ahplTools.hpp.

template<class VS , class VLS >

vec11 jafar::rtslam::lmkAHPL::fromBearingOnlyFrame	(	const VS &	s,
		const VLS &	v1,
		const VLS &	v2,
		const double	_rho1,
		const double	_rho2
	)

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 360 of file ahplTools.hpp.

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

void jafar::rtslam::lmkAHPL::fromBearingOnlyFrame	(	const vec7 &	s,
		const vec3 &	v1,
		const vec3 &	v2,
		const double	_rho1,
		const double	_rho2,
		vec &	ahpl,
		mat &	AHPL_s,
		mat &	AHPL_v1,
		mat &	AHPL_v2,
		mat &	AHPL_rho1,
		mat &	AHPL_rho2
	)

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

template<class AHPL , class P0 , class M >

void jafar::rtslam::lmkAHPL::split	(	const AHPL &	ahpl,
		P0 &	p0,
		M &	m1,
		M &	m2,
		double &	rho1,
		double &	rho2
	)

Split AHPL.

Parameters:

p0	the output anchor.
m1	the first output director vector
m2	the second output director vector
rho1	the first homogeneous parameter (inverse-distance)
rho2	the second homogeneous parameter (inverse-distance)

Definition at line 40 of file ahplTools.hpp.

Referenced by toBearingOnlyFrame().

template<class VS , class VA , class VV >

void jafar::rtslam::lmkAHPL::toBearingOnlyFrame	(	const VS &	s,
		const VA &	ahpl,
		VV &	v1,
		VV &	v2
	)

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. Bring landmark to bearing-only sensor frame (without range information).

For a landmark and sensor frame

ahpl = [p0 m1 rho1 m2 rho2]
s = [t q1 q2],

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
ahpl	the AHPL landmark

Returns:: the bearing-only landmark in sensor frame

Definition at line 233 of file ahplTools.hpp.

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

template<class VS , class VA , class VV >

void jafar::rtslam::lmkAHPL::toBearingOnlyFrame	(	const VS &	s,
		const VA &	ahpl,
		VV &	v1,
		VV &	v2,
		double &	dist1,
		double &	dist2
	)

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 266 of file ahplTools.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::lmkAHPL::toBearingOnlyFrame	(	const VS &	s,
		const VA &	ahpl,
		VV &	v1,
		VV &	v2,
		double &	dist1,
		double &	dist2,
		MV_s &	V1_s,
		MV_a &	V1_ahpl,
		MV_s &	V2_s,
		MV_a &	V2_ahpl
	)

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 303 of file ahplTools.hpp.

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

Detailed Description

Functions

Function Documentation