hydronavutil Namespace Reference

libOrcaNavUtil Mobile Robot Navigation Utility Functions More...

Classes
class	Cov2d
	2d covariance matrix More...
class	Cov3d
	3d covariance matrix More...
class	Exception
	Exception thrown by hydronavutil stuff. More...
class	Gaussian
	A single component of a Gmm. More...
class	Gmm
	A Gaussian Mixture Model (GMM) representing a distribution over 2D robot poses. More...
class	OdometryDifferentiator
	Feed in odometry, this will tell you what change each new bit of data represents. More...
class	Pose
class	Velocity
	Velocity of a 2D robot with no side-slip More...
class	Acceleration
	Acceleration of a 2D robot with no side-slip. More...
Functions
double	pochisq (double x, int df)
double	critchi (double p, int df)
double	gauss (double xx, double xy, double xt, double yy, double yt, double tt, double x, double y, double t)
bool	isUncertain (const Gmm &gmm, double maxLinearVar, double maxRotationalVar, int maxComponents)
void	subtractInitialOffset (double &totalOffsetX, double &totalOffsetY, double &totalOffsetTheta, double initialOffsetX, double initialOffsetY, double initialOffsetTheta)
void	subtractInitialOffset (double totalOffsetX, double totalOffsetY, double totalOffsetTheta, double initialOffsetX, double initialOffsetY, double initialOffsetTheta, double &resultX, double &resultY, double &resultTheta)
void	normaliseAngle (double &angle)
	normalise to [-pi,pi).
void	addPoseOffset (double &poseX, double &poseY, double &poseT, double offsetX, double offsetY, double offsetT, bool normaliseHeading)
void	addPoseOffset (double startX, double startY, double startT, double offsetX, double offsetY, double offsetT, double &resultX, double &resultY, double &resultT, bool normaliseHeading)
void	subtractFinalPoseOffset (double startX, double startY, double startT, double offsetX, double offsetY, double offsetT, double &resultX, double &resultY, double &resultT, bool normaliseHeading)
void	subtractFinalPoseOffset (double &poseX, double &poseY, double &poseT, double offsetX, double offsetY, double offsetT, bool normaliseHeading)
void	transformPoint2d (double xIn, double yIn, double offsetX, double offsetY, double offsetT, double &xOut, double &yOut)
void	transformPoint2d (double &x, double &y, double offsetX, double offsetY, double offsetT)
void	addPoseOffset (double *pose, const double *offset, bool normaliseHeading)
	Convenience function for 'double*'s.
void	addPoseOffset (const double *start, const double *offset, double *result, bool normaliseHeading)
	Convenience function for 'double*'s.
void	subtractFinalPoseOffset (double *pose, const double *offset, bool normaliseHeading)
	Convenience function for 'double*'s.
void	subtractFinalPoseOffset (const double *start, const double *offset, double *result, bool normaliseHeading)
	Convenience function for 'double*'s.
void	normaliseHeading (Pose &pose)
	convenience
void	addPoseOffset (Pose &pose, const Pose &offset, bool normaliseHeading)
	convenience
void	addPoseOffset (const Pose &start, const Pose &offset, Pose &result, bool normaliseHeading)
	convenience
void	subtractFinalPoseOffset (Pose &pose, const Pose &offset, bool normaliseHeading)
	convenience
void	subtractFinalPoseOffset (const Pose &start, const Pose &offset, Pose &result, bool normaliseHeading)
	convenience
void	subtractInitialOffset (Pose &pose, const Pose &initialOffset)
	convenience
void	subtractInitialOffset (const Pose &pose, const Pose &initialOffset, Pose &result)
	convenience
Pose	inverseTransform (const Pose &transform)
	convenience

---

Detailed Description

libOrcaNavUtil Mobile Robot Navigation Utility Functions

This file contains functions relating to the chi-square distribution.

The functions in this file were adapted from Javascript functions found at http://www.fourmilab.ch/rpkp/experiments/analysis/chiCalc.html

They were adapted by John Walker from C implementations written by Gary Perlman of Wang Institute, Tyngsboro, MA 01879. Both the original C code and the JavaScript edition are in the public domain.

Author:

Ian Mahon

Date:

20-05-05

See also:

libHydroNavUtil

---

Function Documentation

void hydronavutil::addPoseOffset	(	double	startX,
		double	startY,
		double	startT,
		double	offsetX,
		double	offsetY,
		double	offsetT,
		double &	resultX,
		double &	resultY,
		double &	resultT,
		bool	normaliseHeading
	)			[inline]

Adds an offset (in local coordinates) onto a pose, producing a result

Adds the linear bit, then applies the rotation.

References normaliseAngle().

void hydronavutil::addPoseOffset	(	double &	poseX,
		double &	poseY,
		double &	poseT,
		double	offsetX,
		double	offsetY,
		double	offsetT,
		bool	normaliseHeading
	)			[inline]

Adds an offset (in local coordinates) onto a pose, modifying the pose.

Adds the linear bit, then applies the rotation.

References normaliseAngle().

Referenced by hydrosim2d::VehicleSimulator::act(), and addPoseOffset().

double hydronavutil::critchi	(	double	p,
		int	df
	)

Calculate the critical chi-square value for a given probability.

Parameters:

	p	The probability of the area in the chi-square distribution to the right of the critical value.
	df	The degrees of freedom of the chi-square distribution.

Returns:

The critical value.

double hydronavutil::gauss	(	double	xx,
		double	xy,
		double	xt,
		double	yy,
		double	yt,
		double	tt,
		double	x,
		double	y,
		double	t
	)			[inline]

Calculates the height of a gaussian with this covariance matrix, centred on (0,0,0), evaluated at the point (x,y,t)

References hydronavutil::Cov3d::gauss().

bool hydronavutil::isUncertain	(	const Gmm &	gmm,
		double	maxLinearVar = 5.0,
		double	maxRotationalVar = (M_PI/2.0),
		int	maxComponents = 2
	)

Checks whether the Gmm is uncertain, based on dodgy heuristics. The linear and rotational variances refer to the diagonal elements of the most likely component.

double hydronavutil::pochisq	(	double	x,
		int	df
	)

Calculate the probability of a chi-square value.

Parameters:

	x	The chi-square value.
	df	The degrees of freedom of the chi-square distribution.

Returns:

The probability of the chi-square value.

Adapted from: Hill, I. D. and Pike, M. C. Algorithm 299 Collected Algorithms for the CACM 1967 p. 243 Updated for rounding errors based on remark in ACM TOMS June 1985, page 185

void hydronavutil::subtractFinalPoseOffset	(	double &	poseX,
		double &	poseY,
		double &	poseT,
		double	offsetX,
		double	offsetY,
		double	offsetT,
		bool	normaliseHeading
	)			[inline]

Opposite of addPoseOffset. Subtracts an offset from the end.

References subtractFinalPoseOffset().

void hydronavutil::subtractFinalPoseOffset	(	double	startX,
		double	startY,
		double	startT,
		double	offsetX,
		double	offsetY,
		double	offsetT,
		double &	resultX,
		double &	resultY,
		double &	resultT,
		bool	normaliseHeading
	)			[inline]

Opposite of addPoseOffset. Subtracts an offset from the end.

References normaliseAngle().

Referenced by subtractFinalPoseOffset().

void hydronavutil::subtractInitialOffset	(	double	totalOffsetX,
		double	totalOffsetY,
		double	totalOffsetTheta,
		double	initialOffsetX,
		double	initialOffsetY,
		double	initialOffsetTheta,
		double &	resultX,
		double &	resultY,
		double &	resultTheta
	)

We have some path, defined by an offset. This removes a chunk of it from the front.

void hydronavutil::subtractInitialOffset	(	double &	totalOffsetX,
		double &	totalOffsetY,
		double &	totalOffsetTheta,
		double	initialOffsetX,
		double	initialOffsetY,
		double	initialOffsetTheta
	)

We have some path, defined by an offset. This removes a chunk of it from the front.

Referenced by hydronavutil::OdometryDifferentiator::calcDelta(), and subtractInitialOffset().

void hydronavutil::transformPoint2d	(	double &	x,
		double &	y,
		double	offsetX,
		double	offsetY,
		double	offsetT
	)			[inline]

Transforms a point (x,y) in one coordinate system to a point (x,y) in another coordinate system using the offset between the two coordinate systems (translation: offsetX/Y, rotation:offsetT)

References transformPoint2d().

void hydronavutil::transformPoint2d	(	double	xIn,
		double	yIn,
		double	offsetX,
		double	offsetY,
		double	offsetT,
		double &	xOut,
		double &	yOut
	)			[inline]

Transforms a point (xIn,yIn) in one coordinate system to a point (xOut,yOut) in another coordinate system using the offset between the two coordinate systems (translation: offsetX/Y, rotation:offsetT)

Referenced by transformPoint2d().