Functions for operations related to lines in SLAM (Plucker, homogeneous, rho-theta, and others) More...

Detailed Description

Functions for operations related to lines in SLAM (Plucker, homogeneous, rho-theta, and others)

Functions
template<class Mat >
void	pluckerIntrinsicMatrix (double u0, double v0, double au, double av, Mat &K)
	Build Plucker matrix from calibration parameters.
template<class Vec , class Mat >
void	pluckerIntrinsicMatrix (Vec const &k, Mat &K)
	Build Plucker matrix from calibration vector.
void	pluckerInverseIntrinsicMatrix (double u0, double v0, double au, double av, jblas::mat &iK)
	Build inverse Plucker camera matrix from calibration parameters.
void	pluckerInverseIntrinsicMatrix (jblas::vec4 const &k, jblas::mat &iK)
	Build inverse Plucker camera matrix from calibration vector.
jblas::vec2	homogeneousToRhoTheta (jblas::vec3 const &l)
	Homogeneous 2D line to rho-theta representation.
void	homogeneousToRhoTheta (jblas::vec3 const &l, jblas::vec2 &rt, jblas::mat &RT_l)
	homogeneous 2D line to rho-theta, with Jacobian
jblas::vec3	rhoThetaToHomogeneous (jblas::vec2 const &rt)
	Rho-theta to homogeneous 2D line transform.
void	rhoThetaToHomogeneous (jblas::vec2 const &rt, jblas::vec3 &l, jblas::mat &L_rt)
	Rho-theta to homogeneous 2D line transform -- Jacobian.
jblas::vec3	projectPlucker (jblas::mat33 const &K, jblas::vec6 const &L)
	Project Plucker 3D line into homogeneous 2D line.
void	projectPlucker (jblas::mat33 const &K, jblas::vec6 const &L, jblas::vec3 &l, jblas::mat &L_l)
	Project Plucker 3D line into homogeneous 2D line, with Jacobians.
jblas::vec3	retroProjectPluckerPlaneNormal (jblas::mat33 const &iK, jblas::vec3 const &l)
	Retro project homogeneous 2D line to representation plane normal vector.
void	retroProjectPluckerPlaneNormal (jblas::mat33 const &iK, jblas::vec3 const &l, jblas::vec3 &n, jblas::mat33 &N_l)
	Retro project homogeneous 2D line to representation plane normal vector, with Jacobians.
jblas::vec6	retroProjectPlucker (jblas::mat33 const &iK, jblas::vec3 const &l, jblas::vec3 const &v)
	Retro project homogeneous 2D line to Plucker 3D line.
void	retroProjectPlucker (jblas::mat33 const &iK, jblas::vec3 const &l, jblas::vec3 const &v, jblas::vec6 &L, jblas::mat &L_l, jblas::mat &L_v)
	Retro project homogeneous 2D line to Plucker 3D line, with Jacobians.
jblas::vec6	retroProjectPluckerFromBase (jblas::mat33 const &iK, jblas::vec3 const &l, jblas::vec2 const &beta)
	Retro project homogeneous 2D line to Plucker 3D line providing base vector.
void	retroProjectPluckerFromBase (jblas::mat33 const &iK, jblas::vec3 const &l, jblas::vec2 const &beta, jblas::vec6 &L, jblas::mat &L_l, jblas::mat &L_beta)
	Retro project homogeneous 2D line to Plucker 3D line providing base vector, with Jacobians.
jblas::mat	planeVecToBaseMatrix (jblas::vec3 const &n)
	Plane sub-vector to plane base matrix.
jblas::vec3	planeBaseToDirVector (jblas::vec3 const &n, jblas::vec2 const &beta)
	Plane sub-vector 'n' and plane base to direction vector.
void	planeBaseToDirVector (jblas::vec3 const &n, jblas::vec2 const &beta, jblas::vec3 &v, jblas::mat33 &V_n, jblas::mat &V_beta)
	Plane sub-vector 'n' and plane base to direction vector, with Jacobians.
void	planeBaseToDirVector (jblas::vec_range const &n, jblas::vec2 const &beta, jblas::vec_range &v, jblas::mat33 &V_n, jblas::mat &V_beta)
	Plane sub-vector 'n' and plane base to direction vector, with Jacobians.
jblas::vec3	normVec3 (jblas::vec3 const &v)
	Normalize 3-vector.
void	normVec3 (jblas::vec3 const &v, jblas::vec3 &vn, jblas::mat33 &VN_v)
	Normalize 3-vector, with Jacobians.
void	normVec3 (jblas::vec3 const &v, jblas::vec_range &vn, jblas::mat33 &VN_v)
	Normalize 3-vector, with Jacobians.
jblas::vec3	adjustVec3 (jblas::vec3 const &v, jblas::vec3 const &n)
	Bring a vector to the norm of another one.
void	adjustVec3 (jblas::vec3 const &v, jblas::vec3 const &n, jblas::vec3 &w, jblas::mat33 &W_v)
	Bring a vector to the norm of another one - with Jacobian.
jblas::vec3	pluckerOrigin (jblas::vec6 const &L)
	Plucker line's origin, the closest point to the coordinates origin.
void	pluckerOrigin (jblas::vec6 const &L, jblas::vec3 &p0, jblas::mat &P0_l)
	Plucker line's origin, with Jacobian.
jblas::mat33	skewSymmMat33 (jblas::vec3 const &v)
	3-by-3 skew-symmetric matrix.
jblas::vec3	pluckerEndpoint (jblas::vec6 const &L, double const s)
	Compute Plucker line endpoint.
void	pluckerEndpoint (jblas::vec6 const &L, double const s, jblas::vec3 &e, jblas::mat &E_l, jblas::mat &E_s)
	Compute Plucker line endpoint, with Jacobians.
jblas::vec6	pluckerRay (jblas::mat33 const &K, jblas::vec2 const &p)
	Ray from pixel, as a Plucker line.
void	pluckerRay (jblas::mat33 const &K, jblas::vec2 const &p, jblas::vec6 r, jblas::mat R_p)
	Ray as a Plucker line from pixel, with Jacobians.
boost::tuple< double, double, double >	pluckerSoftIntersectionAbscissa (jblas::vec6 const &line1, jblas::vec6 const &line2)
	Abscissas of the imperfect intersection of 2 Plucker lines.
jblas::vec6	pluckerToEuclideanPlucker (jblas::vec6 const &L)
	Normalize Plucker line to convert it to Euclidean Plucker line.
void	pluckerToEuclideanPlucker (jblas::vec6 const &L, jblas::vec6 &eL, jblas::mat66 &EL_l)
	Normalize Plucker line to convert it to Euclidean Plucker line, with Jacobian.
jblas::vec3	extToHomogeneous (jblas::vec2 const &p, jblas::vec2 const &q)
	Two endpoints to homogeneous 2D line.
void	extToHomogeneous (jblas::vec2 const &p, jblas::vec2 const &q, jblas::vec3 &hm, jblas::mat &HM_p, jblas::mat &HM_q)
	Two endpoints to homogeneous 2D line - with Jacobians.
void	points3plane (jblas::vec3 const &a, jblas::vec3 const &b, jblas::vec3 const &c, jblas::vec6 &plane)
	Extract a plane from 3 euclidean points.
jblas::vec6	hmPointsToPlucker (jblas::vec4 &A, jblas::vec4 &B)
	Homogeneous Points to Plucker.
void	hmPointsToPlucker (jblas::vec4 &A, jblas::vec4 &B, jblas::vec6 &L, jblas::mat &L_a, jblas::mat &L_b)
	Homogeneous Point to Plucker Jacobians.
jblas::vec4	uvdToHmPoint (jblas::vec3 const &uvd, jblas::vec4 &k, double const alpha)
	stereo pixel to homogeneous Point
void	uvdToHmPoint (jblas::vec3 const &uvd, jblas::vec4 &k, double const alpha, jblas::vec4 &hmP, jblas::mat &HmPu)
	stereo pixel to homogeneous Point Jacobian
jblas::vec6	projectPluckerToStereo (jblas::vec4 const &k, double alpha, jblas::vec6 const &L)
	Plucker line to two stereo homogeneous lines.
void	projectPluckerToStereo (jblas::vec4 const &k, double alpha, jblas::vec6 const &L, jblas::vec6 &stLine, jblas::mat66 &StLine_l)
	Plucker line to two stereo homogeneous lines - with Jacobians.
jblas::vec2	extToRhoTheta (const jblas::vec2 &ext1, const jblas::vec2 &ext2)
	Segment extremities to rho theta line conversion.
void	extToRhoTheta (const jblas::vec2 &ext1, const jblas::vec2 &ext2, jblas::vec2 &rt, jblas::mat &RT_ee)
	Segment extremities to rho theta line conversion - Jacobians.
jblas::vec4	uvdsToStereoRhoTheta (jblas::vec6 const &uvduvd)
	Convert stereo segment to rho-theta stereo line.
void	uvdsToStereoRhoTheta (jblas::vec6 const &uvduvd, jblas::vec4 &rtrt, jblas::mat &RR_uu)
	Convert stereo segment to rho-theta stereo line - Jacobians.
void	uvdsToStereoRhoTheta (jblas::vec6 const &uvduvd, jblas::mat &RR_uu)
	Convert stereo segment to rho-theta stereo line - only Jacobians.
void	homogenousToExtDistances (const jblas::vec3 &_hm, const jblas::vec2 &ext1, const jblas::vec2 &ext2, jblas::vec2 &dd, jblas::mat &DD_hm, jblas::mat &DD_ext1, jblas::mat &DD_ext2)
void	homogenousToPointDistance (const jblas::vec3 &_hm, const jblas::vec2 &_ext, double &d, jblas::mat_range &D_hm, jblas::mat_range &D_ext)
void	homogenousToExtDistances (const jblas::vec3 &_hm, const jblas::vec2 &ext1, const jblas::vec2 &ext2, jblas::vec2 &dd, jblas::mat &DD_hm)
void	homogenousToPointDistance (const jblas::vec3 &_hm, const jblas::vec2 &_ext, double &d, jblas::mat_range &D_hm)
jblas::vec6	points3DToPointVecLine (const jblas::vec3 &p, const jblas::vec3 &q)
	Points line to point-vector line transform.
void	points3DToPointVecLine (const jblas::vec3 &p, const jblas::vec3 &q, jblas::vec6 &m, jblas::mat66 &M_l)
	Points line to point-vector line transform with Jacobians.
jblas::vec2	intersectPointVecLines (const jblas::vec6 &l1, const jblas::vec6 &l2)
	Intersection two point vector lines.
double	cosVecsAngle (const jblas::vec3 &v1, const jblas::vec3 &v2)
	Cosinus of the angle between two vectors.

Function Documentation

jblas::vec3 jafar::slam::lineTools::adjustVec3	(	jblas::vec3 const &	v,
		jblas::vec3 const &	n
	)

Bring a vector to the norm of another one.

Parameters:

v	the nevtor to adjust
n	the vector used as reference

Returns:: w the adjusted vector

void jafar::slam::lineTools::adjustVec3	(	jblas::vec3 const &	v,
		jblas::vec3 const &	n,
		jblas::vec3 &	w,
		jblas::mat33 &	W_v
	)

Bring a vector to the norm of another one - with Jacobian.

Parameters:

v	the nevtor to adjust
n	the vector used as reference
w	the adjusted vector
W_v	the Jacobian wrt v

jblas::vec2 jafar::slam::lineTools::extToRhoTheta	(	const jblas::vec2 &	ext1,
		const jblas::vec2 &	ext2
	)

Segment extremities to rho theta line conversion.

Parameters:

ext1	one endpoint
ext2	the other endpoint

Returns:: the rho-theta line

void jafar::slam::lineTools::extToRhoTheta	(	const jblas::vec2 &	ext1,
		const jblas::vec2 &	ext2,
		jblas::vec2 &	rt,
		jblas::mat &	RT_ee
	)

Segment extremities to rho theta line conversion - Jacobians.

Parameters:

ext1	one endpoint
ext2	the other endpoint
rt	the rho-theta line
RT_ee	the Jacobian of rt wrt ext1 and ext2

jblas::vec2 jafar::slam::lineTools::homogeneousToRhoTheta ( jblas::vec3 const & l )

Homogeneous 2D line to rho-theta representation.

Parameters:

l	homogeneous 2D line in the image plane

Returns:: the rho-theta representation for the same line

void jafar::slam::lineTools::homogeneousToRhoTheta	(	jblas::vec3 const &	l,
		jblas::vec2 &	rt,
		jblas::mat &	RT_l
	)

homogeneous 2D line to rho-theta, with Jacobian

Parameters:

l	the homogeneous 2D line
rt	the rho-theta representation
RT_l	the Jacobian of rt wrt l

void jafar::slam::lineTools::homogenousToExtDistances	(	const jblas::vec3 &	_hm,
		const jblas::vec2 &	ext1,
		const jblas::vec2 &	ext2,
		jblas::vec2 &	dd,
		jblas::mat &	DD_hm,
		jblas::mat &	DD_ext1,
		jblas::mat &	DD_ext2
	)

Parameters:

_hm	homogenous vector for the line parameters

jblas::vec3 jafar::slam::lineTools::normVec3 ( jblas::vec3 const & v )

Normalize 3-vector.

Parameters:

v	input 3-vector

Returns:: The normalized vector w = v/norm(v)

void jafar::slam::lineTools::normVec3	(	jblas::vec3 const &	v,
		jblas::vec3 &	vn,
		jblas::mat33 &	VN_v
	)

Normalize 3-vector, with Jacobians.

Parameters:

v	input 3-vector
nv	output normalized 3-vector
NV_v	output 3x3 normalization Jacobian

void jafar::slam::lineTools::normVec3	(	jblas::vec3 const &	v,
		jblas::vec_range &	vn,
		jblas::mat33 &	VN_v
	)

Normalize 3-vector, with Jacobians.

Parameters:

v	input 3-vector
nv	output normalized 3-vector
NV_v	output 3x3 normalization Jacobian

jblas::vec3 jafar::slam::lineTools::planeBaseToDirVector	(	jblas::vec3 const &	n,
		jblas::vec2 const &	beta
	)

Plane sub-vector 'n' and plane base to direction vector.

Parameters:

n	the representation plane's normal vector
beta	the line's director 2-vector defined in the local representation plane

Returns:: the line's director 3-vector

void jafar::slam::lineTools::planeBaseToDirVector	(	jblas::vec3 const &	n,
		jblas::vec2 const &	beta,
		jblas::vec3 &	v,
		jblas::mat33 &	V_n,
		jblas::mat &	V_beta
	)

Plane sub-vector 'n' and plane base to direction vector, with Jacobians.

Parameters:

n	the representation plane's normal vector
beta	the line's director 2-vector defined in the local representation plane
v	the line's director 3-vector
V_n	the Jacobian ov V wrt n
V_beta	the Jacobian of V wrt beta

void jafar::slam::lineTools::planeBaseToDirVector	(	jblas::vec_range const &	n,
		jblas::vec2 const &	beta,
		jblas::vec_range &	v,
		jblas::mat33 &	V_n,
		jblas::mat &	V_beta
	)

Plane sub-vector 'n' and plane base to direction vector, with Jacobians.

Parameters:

n	the representation plane's normal vector
beta	the line's director 2-vector defined in the local representation plane
v	the line's director 3-vector
V_n	the Jacobian ov V wrt n
V_beta	the Jacobian of V wrt beta

jblas::mat jafar::slam::lineTools::planeVecToBaseMatrix ( jblas::vec3 const & n )

Plane sub-vector to plane base matrix.

Parameters:

n	the representation plane's normal vector

Returns:: the base matrix transforming vectors in the representation plane to 3D vectors

jblas::vec3 jafar::slam::lineTools::pluckerEndpoint	(	jblas::vec6 const &	L,
		double const	s
	)

Compute Plucker line endpoint.

Parameters:

L	the Plucker line
s	the abcissa of the endpoint in the line's local frame

Returns:: the 3D endpoint

void jafar::slam::lineTools::pluckerEndpoint	(	jblas::vec6 const &	L,
		double const	s,
		jblas::vec3 &	e,
		jblas::mat &	E_l,
		jblas::mat &	E_s
	)

Compute Plucker line endpoint, with Jacobians.

Parameters:

L	Plucker line
s	Endpoint's abscissa in the line's local frame
e	3D Endpoint
E_l	Jacobian of e wrt L
E_s	Jacobian of e wrt s

template<class Mat >

void jafar::slam::lineTools::pluckerIntrinsicMatrix	(	double	u0,
		double	v0,
		double	au,
		double	av,
		Mat &	K
	)

Build Plucker matrix from calibration parameters.

Parameters:

u0	horizontal coordinate of principal point, in pixels
v0	vertical coordinate of principal point
au	focal distance, in horizontal pixel units
av	focal distance, in vertical pixel units
K	Plucker intrinsic matrix

template<class Vec , class Mat >

void jafar::slam::lineTools::pluckerIntrinsicMatrix	(	Vec const &	k,
		Mat &	K
	)

Build Plucker matrix from calibration vector.

Parameters:

k	calibration vector k = (u0, v0, au, av)
K	Plucker intrinsic matrix

void jafar::slam::lineTools::pluckerInverseIntrinsicMatrix	(	double	u0,
		double	v0,
		double	au,
		double	av,
		jblas::mat &	iK
	)

Build inverse Plucker camera matrix from calibration parameters.

Parameters:

u0	horizontal coordinate of principal point, in pixels
v0	vertical coordinate of principal point
au	focal distance, in horizontal pixel units
av	focal distance, in vertical pixel units
iK	inverse Plucker intrinsic matrix

void jafar::slam::lineTools::pluckerInverseIntrinsicMatrix	(	jblas::vec4 const &	k,
		jblas::mat &	iK
	)

Build inverse Plucker camera matrix from calibration vector.

Parameters:

k	calibration vector k = (u0, v0, au, av)
iK	inverse Plucker intrinsic matrix

jblas::vec3 jafar::slam::lineTools::pluckerOrigin ( jblas::vec6 const & L )

Plucker line's origin, the closest point to the coordinates origin.

The point is computed with p0 = cross(n,v)/dot(v,v), where n and v are the sub-vectors of the Plucker line, L = [n;v]. See the function with Jacobians for formulae.

Parameters:

L	Plucker line

Returns:: line origin, defined as the closest point to the global origin

void jafar::slam::lineTools::pluckerOrigin	(	jblas::vec6 const &	L,
		jblas::vec3 &	p0,
		jblas::mat &	P0_l
	)

Plucker line's origin, with Jacobian.

The Plucker line is divided in sub-vectors:
n = subrange(L,0,3) is the plane normal,
v = subrange(L,3,6) is the director vector.
Then the closest point of L to the global origin of coordinates is
p0 = cross(n,v)/dot(v,v).

Parameters:

L	Plucker line
p0	line origin, defined as the closest point to the global origin
P0_l	Jacobian of the origin with respect to the line

jblas::vec6 jafar::slam::lineTools::pluckerRay	(	jblas::mat33 const &	K,
		jblas::vec2 const &	p
	)

Ray from pixel, as a Plucker line.

Parameters:

K	intrinsic matrix
p	a 2-pixel

Returns:: the ray as a Plucker line

void jafar::slam::lineTools::pluckerRay	(	jblas::mat33 const &	K,
		jblas::vec2 const &	p,
		jblas::vec6	r,
		jblas::mat	R_p
	)

Ray as a Plucker line from pixel, with Jacobians.

Parameters:

K	intrinsic matrix
p	a 2-pixel
r	the ray as a Plucker line
R_p	the Jacobian of r wrt p

boost::tuple<double,double,double> jafar::slam::lineTools::pluckerSoftIntersectionAbscissa	(	jblas::vec6 const &	line1,
		jblas::vec6 const &	line2
	)

Abscissas of the imperfect intersection of 2 Plucker lines.

This function computes the distance between two Plucker lines and returns the abscisses of the points on both lines that are closest to the other line. This represents an imperfect intersection.

Parameters:

line1	The first Plucker line
line2	The second Plucker line

Returns:: a 3-tuple with s1, s2 the abscissas of the points in lines 1 and 2 closest to lines 2 and 1, and d the distance between both lines

jblas::vec6 jafar::slam::lineTools::pluckerToEuclideanPlucker ( jblas::vec6 const & L )

Normalize Plucker line to convert it to Euclidean Plucker line.

Parameters:

L	the Plucker line

Returns:: the Euclidean Plucker line

void jafar::slam::lineTools::pluckerToEuclideanPlucker	(	jblas::vec6 const &	L,
		jblas::vec6 &	eL,
		jblas::mat66 &	EL_l
	)

Normalize Plucker line to convert it to Euclidean Plucker line, with Jacobian.

Parameters:

L	the Plucker line
eL	the Euclidean Plucker line
EL_l	the Jacobian

jblas::vec3 jafar::slam::lineTools::projectPlucker	(	jblas::mat33 const &	K,
		jblas::vec6 const &	L
	)

Project Plucker 3D line into homogeneous 2D line.

Parameters:

K	the Plucker intrinsic matrix
L	the Plucker line

Returns:: the homogeneous 2D line in the image plane

void jafar::slam::lineTools::projectPlucker	(	jblas::mat33 const &	K,
		jblas::vec6 const &	L,
		jblas::vec3 &	l,
		jblas::mat &	L_l
	)

Project Plucker 3D line into homogeneous 2D line, with Jacobians.

Parameters:

K	the Plucker intrinsic matrix
L	the Plucker line
l	the homogeneous 2D line in the image plane
L_l	the Jacobian of l wrt L

jblas::vec6 jafar::slam::lineTools::projectPluckerToStereo	(	jblas::vec4 const &	k,
		double	alpha,
		jblas::vec6 const &	L
	)

Plucker line to two stereo homogeneous lines.

Parameters:

k	the intrinsic parameters vector (u0,v0,alphaU,alphaV)
alpha	the baseline factor, alpha = b*alphaU
L	the Plucker line

Returns:: a 6-vector with 2 stacked homogeneous lines, for left and right images

void jafar::slam::lineTools::projectPluckerToStereo	(	jblas::vec4 const &	k,
		double	alpha,
		jblas::vec6 const &	L,
		jblas::vec6 &	stLine,
		jblas::mat66 &	StLine_l
	)

Plucker line to two stereo homogeneous lines - with Jacobians.

Parameters:

k	the intrinsic parameters vector (u0,v0,alphaU,alphaV)
alpha	the baseline factor, alpha = b*alphaU
L	the Plucker line
stLine	a 6-vector with 2 stacked homogeneous lines, for left and right images
StLine_l	the Jacobian of stLine wrt L

jblas::vec6 jafar::slam::lineTools::retroProjectPlucker	(	jblas::mat33 const &	iK,
		jblas::vec3 const &	l,
		jblas::vec3 const &	v
	)

Retro project homogeneous 2D line to Plucker 3D line.

Parameters:

iK	the inverse Plucker intrinsic matrix
l	the 2D homogeneous line
v	the line's director vector, unmeasurable from l

Returns:: the 3D Plucker line

void jafar::slam::lineTools::retroProjectPlucker	(	jblas::mat33 const &	iK,
		jblas::vec3 const &	l,
		jblas::vec3 const &	v,
		jblas::vec6 &	L,
		jblas::mat &	L_l,
		jblas::mat &	L_v
	)

Retro project homogeneous 2D line to Plucker 3D line, with Jacobians.

Parameters:

iK	the inverse Plucker intrinsic matrix
l	the 2D homogeneous line
v	the line's director vector, unmeasurable from l
L	the 3D Plucker line
L_l	the Jacobian of L wrt l
L_v	the Jacobian of L wrt v

jblas::vec6 jafar::slam::lineTools::retroProjectPluckerFromBase	(	jblas::mat33 const &	iK,
		jblas::vec3 const &	l,
		jblas::vec2 const &	beta
	)

Retro project homogeneous 2D line to Plucker 3D line providing base vector.

Parameters:

iK	the inverse Plucker intrinsic matrix
l	the 2D homogeneous line
beta	the line's director 2-vector defined in the local representation plane

Returns:: the 3D Plucker line

void jafar::slam::lineTools::retroProjectPluckerFromBase	(	jblas::mat33 const &	iK,
		jblas::vec3 const &	l,
		jblas::vec2 const &	beta,
		jblas::vec6 &	L,
		jblas::mat &	L_l,
		jblas::mat &	L_beta
	)

Retro project homogeneous 2D line to Plucker 3D line providing base vector, with Jacobians.

Parameters:

iK	the inverse Plucker intrinsic matrix
l	the 2D homogeneous line
beta	the line's director 2-vector defined in the local representation plane
L	the 3D Plucker line
L_l	the Jacobian of L wrt l
L_beta	the Jacobian of L wrt beta

jblas::vec3 jafar::slam::lineTools::retroProjectPluckerPlaneNormal	(	jblas::mat33 const &	iK,
		jblas::vec3 const &	l
	)

Retro project homogeneous 2D line to representation plane normal vector.

Parameters:

iK	the inverse Plucker intrinsic matrix
l	the homogeneous 2D line

Returns:: the 3D Plucker line

void jafar::slam::lineTools::retroProjectPluckerPlaneNormal	(	jblas::mat33 const &	iK,
		jblas::vec3 const &	l,
		jblas::vec3 &	n,
		jblas::mat33 &	N_l
	)

Retro project homogeneous 2D line to representation plane normal vector, with Jacobians.

Parameters:

iK	the inverse plucker camera matrix
l	the homogeneous 2D line
n	the representation plane's normal vector
N_l	the Jacobian of n wrt l.

jblas::vec3 jafar::slam::lineTools::rhoThetaToHomogeneous ( jblas::vec2 const & rt )

Rho-theta to homogeneous 2D line transform.

Parameters:

rt	rho-theta representation of the 2D line

Returns:: the homogeneous 2D line

void jafar::slam::lineTools::rhoThetaToHomogeneous	(	jblas::vec2 const &	rt,
		jblas::vec3 &	l,
		jblas::mat &	L_rt
	)

Rho-theta to homogeneous 2D line transform -- Jacobian.

Parameters:

rt	the rho-theta representation
l	the homogeneous 2D line
L_rt	the Jacobian of l wrt rt

jblas::mat33 jafar::slam::lineTools::skewSymmMat33 ( jblas::vec3 const & v )

3-by-3 skew-symmetric matrix.

Parameters:

v	a 3-vector

Returns:: the skew symmetric matrix (corresponding to the cross product)

jblas::vec4 jafar::slam::lineTools::uvdsToStereoRhoTheta ( jblas::vec6 const & uvduvd )

Convert stereo segment to rho-theta stereo line.

Parameters:

uvduvd a 6-vector with the two stereo endpoints in uvd format.

Returns:: a 4-vector with two rho-theta lines

void jafar::slam::lineTools::uvdsToStereoRhoTheta	(	jblas::vec6 const &	uvduvd,
		jblas::vec4 &	rtrt,
		jblas::mat &	RR_uu
	)

Convert stereo segment to rho-theta stereo line - Jacobians.

Parameters:

uvduvd	a 6-vector with the two stereo endpoints in uvd format.
rtrt	a 4-vector with two rho-theta lines
RR_uu	the Jacobian of rtrt wrt uvduvd

void jafar::slam::lineTools::uvdsToStereoRhoTheta	(	jblas::vec6 const &	uvduvd,
		jblas::mat &	RR_uu
	)

Convert stereo segment to rho-theta stereo line - only Jacobians.

Parameters:

uvduvd	a 6-vector with the two stereo endpoints in uvd format.
RR_uu	the Jacobian of uvdsToStereoRhoTheta() wrt uvduvd