#include <ureslocater.h>

Inheritance diagram for UResLocater:

Public Member Functions
void	closelog ()

bool	keepMapPose ()

void	locate (ULaserData *pushData, bool silent)

virtual bool	methodCall (const char name, const char paramOrder, char *strings, const double pars, double value, UDataBase returnStruct=NULL, int returnStructCnt=NULL)

void	openlog ()

void	retrieveData (ULaserData data, RangeData rd)

void	set_poselast ()

void	set_state (char *, double)

void	set_utmstate ()

bool	setUtmPoseToMapPose ()

	UResLocater ()

void	varprint ()

virtual	~UResLocater ()

Public Member Functions inherited from UResVarPool
int	addMethod (const char formalName, const char paramTypes, const char *comment)

int	addMethodV (const char formalName, const char paramTypes, const char *comment)

UVarPool *	addStruct (const char name, const char comment, bool copy=false)

UVariable *	addVar (const char name, const double initialValue, const char type, const char *comment)

UVariable *	addVar (const char name, const char initialValue, const char type, const char comment)

UVariable *	addVarA (const char name, const char initialValue, const char type, const char comment)

bool	callGlobal (const char name, const char paramOrder, char *strings, const double doubles, double value, UDataBase returnStruct, int returnStructCnt)

bool	callGlobalV (const char name, const char paramOrder, UVariable params, UDataBase returnStruct, int *returnStructCnt)
	Call a method from the root var pool scope It may be in the structure direct (no '.') or in any sub-structure that the 'name.subname' indicate. More...

bool	callLocal (const char name, const char paramOrder, char *strings, const double doubles, double value, UDataBase returnStruct, int returnStructCnt)

bool	callVS (const char function, const char stringParam)

int	callVSCD (const char function, const char strPar, UDataBase *data, int cooSys)

void	createBaseVar ()

virtual bool	createVarSpace (const int varCnt, const int structCnt, const int methCnt, const char *note, bool copy=false)

bool	getGlobalString (const char name, const char *value)

bool	getGlobalValue (const char name, double value)

bool	getGlobalValue (const char name, const char *value)

bool	getGlobalValue (const char name, bool value)

bool	getGlobalValue (const char name, UTime value)

bool	getLocalValue (const char name, double value)
	get a value in this variable tree More...

bool	getLocalValue (const char name, bool value)

bool	getLocalValue (const char name, UTime value)

double	getLocalValue (int idx)

UPosition	getLocalValue3D (int idx)

UPosRot	getLocalValue6D (int idx)

bool	getLocalValueBool (int idx)

int	getLocalValueInt (int idx)

UTime	getLocalValueTime (int idx)

const char *	getLogFileName ()

const char *	getVarDescription (int idx)

UVarCalc *	getVarPool ()

virtual bool	gotAllResources (char *missingThese, int missingTheseCnt)

bool	isLogFileOpen ()

bool	isLogFileOpen (const char *structName)

bool	isVarPoolUpdated (int lastCnt, int *newCnt)

void	logFileClose ()

bool	logFileOpen ()

void	logFileStart (const char *structName, bool open)

virtual const char *	print (const char preString, char buff, int buffCnt)

bool	setGlobalVar (const char *name, const double value, bool mayAdd)
	Assigns a value to a variable. The variable may be a part of a structure. More...

bool	setLocalVar (int idx, double value, const int element)

bool	setLocalVar (const char *name, const double value, bool mayAdd)
	Assigns a value to a variable. The variable may be a part of a structure. More...

bool	setLocalVar3D (int idx, UPosition *value)

bool	setLocalVar6D (int idx, UPosRot *value)

bool	setLocalVarAdd (int idx, double value, const int element)

bool	setLocalVarPose (int idx, UPose *value)

virtual bool	setResource (UResBase *resource, bool remove)

void	setVarPool (UVarCalc *sharedVarPool)

int	updateQTvars (int structIdx)

	UResVarPool ()

virtual	~UResVarPool ()

Public Member Functions inherited from UResBase
void	createReplayVar (UVarPool *pool)

virtual void	createResources ()

UCmdExe *	getCorePointer ()

char *	getLogFileName (char *fn, const int fnCnt)

int	getResFuncIdx ()

const char *	getResID ()

int	getResVersion ()

bool	isA (const char *idStr)

bool	isAlsoA (const char *id)

virtual void	print (const char *preString)

virtual void	saveSettings ()

void	setCorePointer (UCmdExe *serverCore)

void	setResFuncIdx (int idx)

virtual void	setResID (const char *id, const int version)

virtual void	stop (bool)

virtual void	updateReplayStatus ()

	UResBase ()

virtual	~UResBase ()

Public Member Functions inherited from UReplay
void	addChild (UReplay *child)

virtual bool	decodeReplayLine (char *)

char *	getLogFileName (char *fn, const int fnCnt)

char *	getReplayFileName (char *fn, const int fnCnt)

int	getReplayLogLine ()

bool	isReplay ()

bool	isReplayFileOpen ()

void	removeChild (UReplay *child)

virtual void	replayAdvanceTime (UTime untilTime)

void	replaySetBaseFileName (const char name, const char preName="")

void	replaySetFileName (const char *name)

virtual bool	replayStep ()

int	replayStep (int steps)

virtual bool	replayToTime (UTime untilTime)

void	setParent (UReplay *parent)

bool	setReplay (bool value)

	UReplay ()

virtual	~UReplay ()

Public Member Functions inherited from ULock
bool	lock ()

void	lockInit ()

void	post ()

bool	tryLock ()

bool	tryWait ()

	ULock ()

void	unlock ()

bool	wait ()

	~ULock ()

Public Member Functions inherited from UVarMethodImplement
virtual bool	methodCallV (const char name, const char paramOrder, UVariable params[], UDataBase returnStruct, int returnStructCnt)

virtual	~UVarMethodImplement ()

Public Attributes
double	axle_dist

double	E_A

double	E_L

double	E_SA

double	ls_offset

char *	mapfile

double	max_angle_deviation

int	min_points

unsigned long	serial

bool	update_allow

double	var_d

double	var_phi

double	var_t

Public Attributes inherited from UReplay
bool	replayTimeAdvancePending

UTime	replayTimeNow

Protected Member Functions
void	correctionUpdate (double dist_meas, double dist_pred, double ang_dev, mapline *line)

void	correctionUpdate3 (double dist_meas, double dist_pred, double ang_dev, double rowEndDev, mapline *line)

void	createBaseVar ()

bool	doLocatorUpdates (bool updateKalman, ULogFile *logUpd, UTime updTime, UPose pose_scan, unsigned int serial)

void	init_matrix ()

void	make_matrix ()

void	makeRowPolygon (regtype reg, mapline mLine)

void	match_points (RangeData rd, matrix x_, double ls_offset, UResMapbase )

void	odoPredict (UPose *poseScan)

bool	polygonToPolyPlugin (UPolygon poly, char name, int coordinateSystem)

float	quality ()

bool	rowEndDetect (UPose mPose, UPose lPose, double maxRange, int lineIdx, mapline line, double rend_dev, const char *lineName, UTime scanTime)

Protected Member Functions inherited from UResVarPool
void	setDescription (const char *note, bool copy=false)

Protected Member Functions inherited from UResBase
UResBase *	getStaticResource (const char *resName, bool mayCreate, bool staticOnly=true)
	get a pointer to a static resource. More...

Protected Attributes
ULogFile	fp

bool	initPoseOK

ULogFile	logEnd
	logfile for updates - matlab style More...

ULogFile	logUpd
	logfile for updates - matlab style More...

UPose	pose_last

ULogFile	testfid

double	ttarget

UVariable *	varDist

UVariable *	varkalmpose

UVariable *	varKeepMapPose
	flag to allow update of mapPose More...

UVariable *	varMaxAngleDeviation

UVariable *	varOdoDistScale
	scale to multiply driven distance - to compensate for soft ground and tire variations More...

UVariable *	varRowEndIn1
	distance inside coverage before row end detect - when expected out of range. More...

UVariable *	varRowEndIn2
	distance inside coverage before row end detect - when expected in range More...

UVariable *	varRowEndMaxDist
	max usable distance movement for row end detection - should be covariance dependent More...

UVariable *	varRowEndMaxGap

UVariable *	varRowExtraLog

UVariable *	varRowToRobotMaxDist
	maximum distance to a row used for line end detection More...

UVariable *	varRowWidthFactor

UVariable *	varUpdcnt

UVariable *	varUseAvgUpdate
	use averaged update of locater, rather than each line individually More...

UVariable *	varUseRowEnd
	flag to allow row-end to be used More...

double	xtarget

double	ytarget

Protected Attributes inherited from UResBase
int	resFuncIdx

char	resID [MAX_RESOURCE_ID_LENGTH]

char	resIsAlso [MAX_RESOURCE_ISALSO_LENGTH]

int	resVersion

Protected Attributes inherited from UReplay
bool	replay

FILE *	replayFile

char	replayFileName [REPLAY_FILE_NAME_LENGTH]
	replay filename More...

char	replayLine [MAX_LOG_LINE_LENGTH]

int	replayLogLine

UReplay *	replayParent

UTime	replayTimeNext

Private Attributes
int	cnt_array [MAPLENGTH]

int	cnte_array [MAPLENGTH]

matrix *	df_u

matrix *	df_u_trans

matrix *	df_x
	odometry relation to covariance More...

matrix *	df_x_trans

matrix *	eye3

matrix *	H
	connection matrix between measurement and state variables More...

matrix *	H3

matrix *	H3_trans

matrix *	H_trans

matrix *	K
	kalman gain More...

matrix *	K3

ULocaterLineResult	locDecs [MAX_LOC_DECS]
	detected lines More...

int	locDecsCnt
	number of detected lines More...

UMatrix4	mPu

UMatrix4	mXu
	updated locater state maintained by averaged update function More...

matrix *	P
	starte and estimated covariance matrix More...

matrix *	P_

matrix *	P_1
	temporary elements of P during odometry update More...

matrix *	P_2

matrix *	P_3

matrix *	P_4

UPolygon *	poly40

UPolygon *	poly400

matrix *	Q
	state noise matrix More...

matrix *	R
	measurement error More...

matrix *	R3

matrix *	S
	sensitiviti matrix for kalman gain calculation More...

matrix *	S3

matrix *	S3_inv

matrix *	S_inv

matrix *	tmp
	temporary matrices More...

matrix *	tmp22

matrix *	tmp23

matrix *	tmp31

matrix *	tmp32

matrix *	tmp33

matrix *	x
	state and estimate state (twice) matrix (3x1) More...

matrix *	x_

double	x_array [MAPLENGTH][POINTS_MAX]

matrix *	x_odo
	odometry based state update More...

matrix *	x_up

double	xe_array [MAPLENGTH][POINTS_MAX]

matrix *	y3_hat

double	y_array [MAPLENGTH][POINTS_MAX]

matrix *	y_hat
	measurement change More...

double	ye_array [MAPLENGTH][POINTS_MAX]

matrix *	z
	measurement vector Z={d; th} Z3 = {d, th, t} More...

matrix *	z3

Static Private Attributes
static const int	MAX_LOC_DECS = 20

Additional Inherited Members
Static Public Member Functions inherited from UResVarPool
static const char *	getResClassID ()

Static Protected Attributes inherited from UReplay
static const int	MAX_LOG_LINE_LENGTH = 10000

static const int	REPLAY_FILE_NAME_LENGTH = 264
	max length of replay filename More...

Detailed Description

Resource to process lasterscans and detectable apriori mapped lines.

Constructor & Destructor Documentation

UResLocater::UResLocater ( )

Constructor

References axle_dist, createBaseVar(), E_A, E_L, E_SA, fp, UResBase::getResVersion(), init_matrix(), initPoseOK, logEnd, logUpd, make_matrix(), max_angle_deviation, min_points, poly40, poly400, UResBase::resVersion, serial, UResVarPool::setDescription(), ULogFile::setLogName(), UResBase::setResID(), testfid, update_allow, var_d, var_phi, and var_t.

UResLocater::~UResLocater ( )

virtual

Destructor

References closelog().

Member Function Documentation

void UResLocater::closelog ( )

References ULogFile::closeLog(), fp, logEnd, logUpd, and testfid.

Referenced by UFunctionLocater::handleLocater(), and ~UResLocater().

void UResLocater::correctionUpdate	(	double	dist_meas,
		double	dist_pred,
		double	ang_dev,
		mapline *	line
	)

protected

update state based on locater measurements of distance from row and row angle

Parameters

dist_meas	is the measured distance to row
dist_pred	the expected distance
ang_dev	difference between measured and expected angle
line	is the mapped row line

References angle_normalize(), eye3, H, H_trans, K, P_, R, S, S_inv, tmp, tmp23, tmp31, tmp32, tmp33, x_, and y_hat.

Referenced by locate().

void UResLocater::correctionUpdate3	(	double	dist_meas,
		double	dist_pred,
		double	ang_dev,
		double	rowEndDev,
		mapline *	line
	)

protected

update state based on locater measurements of distance from row and row angle

Parameters

dist_meas	is the measured distance to row
dist_pred	the expected distance
ang_dev	difference between measured and expected angle
rowEndDev	is the difference is row end position
line	is the mapped row line

References angle_normalize(), eye3, H3, H3_trans, K3, P_, R3, S3, S3_inv, tmp, tmp33, x_, and y3_hat.

Referenced by locate().

void UResLocater::createBaseVar ( )

protected

Make the variables that will be available to other plugins

References UResVarPool::addMethod(), UResVarPool::addVar(), varkalmpose, varKeepMapPose, varMaxAngleDeviation, varOdoDistScale, varRowEndIn1, varRowEndIn2, varRowEndMaxDist, varRowEndMaxGap, varRowExtraLog, varRowToRobotMaxDist, varRowWidthFactor, varUpdcnt, varUseAvgUpdate, and varUseRowEnd.

Referenced by UResLocater().

bool UResLocater::doLocatorUpdates	(	bool	updateKalman,
		ULogFile *	logUpd,
		UTime	updTime,
		UPose	pose_scan,
		unsigned int	serial
	)

protected

Implement the detections of mapped lines. NB! method to add measurements - to an average - is simplified!! no alternative solution is found - yet.

Parameters

updateKalman	skould kalman matrices be updated (else just log)
logUpd	pointer to logfile to use
updTime	is the timestamp for the source data
serial	is the serial number of the laserscan - for log only

Returns: true if updates are usefull.

References UMatrix::add(), ULocaterLineResult::angleDelta, ULocaterLineResult::distMap, ULocaterLineResult::distMeas, UMatrix::get(), UVariable::getBool(), ULogFile::getF(), UTime::getMicrosec(), UTime::getSec(), UVariable::getValued(), UPose::h, ULocaterLineResult::hitCnt, UMatrix::init(), UMatrix::inverse(), UMatrix4::inversed(), ULogFile::isOpen(), ULocaterLineResult::line, ULocaterLineResult::lineEndDelta, ULocaterLineResult::lineEndOK, locDecs, locDecsCnt, mPu, mXu, P_, UMatrix::setCol(), UMatrix::setDiag(), UMatrix::setRC(), UMatrix::setRow(), UMatrix::setSize(), UMatrix::transpose(), var_d, var_phi, var_t, varRowEndMaxDist, varRowToRobotMaxDist, varUseRowEnd, UPose::x, x_up, and UPose::y.

Referenced by locate().

void UResLocater::init_matrix ( )

protected

initialize matrices

Initialize the matrices

References df_u, df_u_trans, df_x, df_x_trans, eye3, H, H3, H3_trans, H_trans, K, K3, mPu, mXu, P, P_, P_1, P_2, P_3, P_4, Q, R, R3, S, S3, S3_inv, S_inv, UMatrix::setSize(), tmp, tmp22, tmp23, tmp31, tmp32, tmp33, var_d, var_phi, var_t, x, x_, x_odo, x_up, y3_hat, y_hat, and z.

Referenced by UResLocater().

bool UResLocater::keepMapPose ( )

inline

Should module keep mapPose from localizer. If keepMapPose=true, then locater updates sets the mapPose. If keepMapPose=false then mapPose can be set bu the utmToMap locater function

Referenced by UFunctionLocater::handleLocater().

void UResLocater::locate	(	ULaserData *	pushData,
		bool	silent
	)

Referenced by UFunctionLocater::handleLocater().

void UResLocater::make_matrix ( )

protected

allocate space for matrices

Author: Christian Andersen Peter Tjell / Søren Hansen

References df_u, df_u_trans, df_x, df_x_trans, eye3, H, H3, H3_trans, H_trans, K, K3, P, P_, P_1, P_2, P_3, P_4, Q, R, R3, S, S3, S3_inv, S_inv, tmp, tmp22, tmp23, tmp31, tmp32, tmp33, x, x_, x_odo, x_up, y3_hat, y_hat, z, and z3.

Referenced by UResLocater().

void UResLocater::makeRowPolygon	(	regtype	reg,
		mapline	mLine
	)

protected

Make a public polygon from the points used in linear regression calculation.

Parameters

reg	is the linear regression points and result.
mLine	is the mapped line - for polygon naming

References regtype::a, UPolygon::add(), regtype::b, regtype::c, UPolygon::clear(), U2Dseg::clear(), UPolygon::color, UPolygon::extractConvexTo(), U2Dlined::getOnLine(), U2Dseg::getPositionOnLine(), U2Dseg::length, regtype::N, poly40, poly400, polygonToPolyPlugin(), UPolygon::setAsPolygon(), UPolygon::setAsPolyline(), regtype::tMax, regtype::tMin, U2Dpos::x, regtype::x_array, U2Dpos::y, and regtype::y_array.

Referenced by locate().

void UResLocater::match_points	(	RangeData	rd,
		matrix *	x_,
		double	ls_offset,
		UResMapbase *	mapbase
	)

protected

Correlates laser scan points to each of the map lines in mapBase. The result is delivered in the array parameters.

Parameters

rd	is the laserscan measurements
x_	matrix, that ???
cnt_array	number of measurements associated to eaxh map point
x_array,y_array	is the associated measurements to each map line
cnte_array	number of measurements associated to eaxh map point
xe_array,ye_array	is the associated measurements to each map line, assuming endless lines
ls_offset	is forward position of laserscanner relative to robot origin (x-offset)
UResMapbase	is a pointer to the map plugin with map line information

References angle_normalize(), cnt_array, cnte_array, dist, dist2line(), U2Dseg::getDistanceSigned(), UVariable::getDouble(), UPose::getPoseToMap(), UVariable::getValued(), UPose::h, ULock::lock(), ls_offset, OUT_OF_RANGE, RangeData::point_r, RangeData::point_x, RangeData::point_y, RangeData::points, pose_last, UPose::set(), U2Dseg::setFromPoints(), UVariable::setValued(), tv, ULock::unlock(), varDist, varRowWidthFactor, x, U2Dpos::x, UPose::x, x_array, xe_array, U2Dpos::y, UPose::y, y_array, and ye_array.

Referenced by locate().

bool UResLocater::methodCall	(	const char *	name,
		const char *	paramOrder,
		char **	strings,
		const double *	pars,
		double *	value,
		UDataBase **	returnStruct = `NULL`,
		int *	returnStructCnt = `NULL`
	)

virtual

The varPool has methods, and a call to one of these is needed. Do the call now and return (a double sized) result in 'value' and return true if the method call is allowed. If the returnStruct and returnStructCnt is not NULL, then a number (no more than initial value of returnStructCnt) of structures based on UDataBase may be returned into returnStruct array of pointers. The returnStructCnt should be modified to the actual number of used pointers (if needed). If the call is allowed, but the result is invalid for some reason, the return value 'value' can be set to som agreed value, (e.g. 0.0 (false) and 1.0 for true).

Reimplemented from UResVarPool.

References set_poselast(), set_utmstate(), and setUtmPoseToMapPose().

void UResLocater::odoPredict ( UPose * poseScan )

protected

update robot state in map coordinates based on odometry movement

Parameters

poseScan pose at current laser scan time.

References angle_normalize(), axle_dist, df_u, df_u_trans, df_x, df_x_trans, E_A, E_L, E_SA, extract_control(), UVariable::getValued(), P, P_, P_1, P_2, P_3, P_4, pose_last, Q, tmp32, varOdoDistScale, x, x_, and x_odo.

Referenced by locate().

void UResLocater::openlog ( )

References fp, logUpd, ULogFile::openLog(), and testfid.

Referenced by UFunctionLocater::handleLocater().

bool UResLocater::polygonToPolyPlugin	(	UPolygon *	poly,
		char *	name,
		int	coordinateSystem
	)

protected

Send this polygon to poly plug-in

Parameters

poly	is the polygon (or polyline to send.
name	is the name of the polygon
coordinatesystem	is 0=odo, 1=utm, 2=map coordinate system

Returns: true if sucessfull.

References UResVarPool::callGlobalV(), UVariable::getBool(), UResBase::getStaticResource(), UVariable::setDouble(), and UVariable::setValues().

Referenced by makeRowPolygon().

float UResLocater::quality ( )

protected

get quality of localization

References UVariable::getInt(), and varUpdcnt.

Referenced by locate().

void UResLocater::retrieveData	(	ULaserData *	data,
		RangeData *	rd
	)

References ULaserData::getAngleRad(), ULaserData::getRangeCnt(), ULaserData::getRangeMeter(), OUT_OF_RANGE, RangeData::point_r, RangeData::point_th, RangeData::points, and RangeData::polarToCart().

Referenced by locate().

bool UResLocater::rowEndDetect	(	UPose	mPose,
		UPose	lPose,
		double	maxRange,
		int	lineIdx,
		mapline *	line,
		double *	rend_dev,
		const char *	lineName,
		UTime	scanTime
	)

protected

estimate a row end locationcompared to current pose.

Parameters

mPose	is current robot pose (in map coordinates)
lPose	is the current laser pose (in map coordinates)
maxRange	is max range of laser scanner (twice that distance is searched)
lineIdx	is the index to the row to test.
line	is the associated map line
rend_dev	is the measured deviation from old row end
lineName	is the descriptive name of the line
scantime	for logging

Returns: true if a row end is estimated

if line end is out of range, then detected end must at lest 3 m inside coverage

if line expected in range, then at least (1m) past end must be visible

References ULogFile::closeLog(), cnte_array, UVariable::getBool(), ULogFile::getF(), UPose::getMapToPose(), UTime::getMicrosec(), UTime::getSec(), UVariable::getValued(), UPose::h, ULogFile::isOpen(), logEnd, ULogFile::openLog(), roundi(), serial, UPosition::set(), ULogFile::setLogName(), ULogFile::setLogNameNumber(), sqr(), varRowEndIn1, varRowEndIn2, varRowEndMaxGap, varRowExtraLog, UPose::x, x_array, xe_array, UPose::y, y_array, and ye_array.

Referenced by locate().

void UResLocater::set_poselast ( )

References UResPoseHist::getNewest(), UResBase::getStaticResource(), ULock::lock(), pose_last, and ULock::unlock().

Referenced by UFunctionLocater::handleLocater(), and methodCall().

void UResLocater::set_state	(	char *	var,
		double	val
	)

void UResLocater::set_utmstate ( )

bool UResLocater::setUtmPoseToMapPose ( )

Set the map pose from utmPose - if mapPose is not maintained by locater update

Returns: true if mapPose is updated by call.

References UResPoseHist::addIfNeeded(), UVariable::getBool(), UPose::getMapToPosePose(), UResPoseHist::getNewest(), UResBase::getStaticResource(), UPoseTVQ::q, UPoseTime::t, URESLOCATER_ID_SET, varKeepMapPose, and UPoseTVQ::vel.

Referenced by UFunctionLocater::handleLocater(), and methodCall().

void UResLocater::varprint ( )

References UVariable::getPose(), UPose::h, ULock::lock(), ULock::unlock(), varkalmpose, x, UPose::x, and UPose::y.

Referenced by UFunctionLocater::handleLocater().

Member Data Documentation

double UResLocater::axle_dist

Referenced by UFunctionLocater::handleLocater(), odoPredict(), and UResLocater().

int UResLocater::cnt_array[MAPLENGTH]

private

number of points for each map line

Referenced by locate(), and match_points().

int UResLocater::cnte_array[MAPLENGTH]

private

Referenced by match_points(), and rowEndDetect().

matrix * UResLocater::df_u

private

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix * UResLocater::df_u_trans

private

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix* UResLocater::df_x

private

odometry relation to covariance

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix * UResLocater::df_x_trans

private

Referenced by init_matrix(), make_matrix(), and odoPredict().

double UResLocater::E_A

Noise variables for control signals

Referenced by UFunctionLocater::handleLocater(), odoPredict(), and UResLocater().

double UResLocater::E_L

Referenced by UFunctionLocater::handleLocater(), odoPredict(), and UResLocater().

double UResLocater::E_SA

Referenced by UFunctionLocater::handleLocater(), odoPredict(), and UResLocater().

matrix* UResLocater::eye3

private

Referenced by correctionUpdate(), correctionUpdate3(), init_matrix(), and make_matrix().

ULogFile UResLocater::fp

protected

logfiles

Referenced by closelog(), locate(), openlog(), and UResLocater().

matrix* UResLocater::H

private

connection matrix between measurement and state variables

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

matrix * UResLocater::H3

private

Referenced by correctionUpdate3(), init_matrix(), and make_matrix().

matrix * UResLocater::H3_trans

private

Referenced by correctionUpdate3(), init_matrix(), and make_matrix().

matrix * UResLocater::H_trans

private

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

bool UResLocater::initPoseOK

protected

is the startup pose initiated OK This is to avoid the map pose to be initialized from a non-valid GPS pose.

Referenced by locate(), set_state(), set_utmstate(), and UResLocater().

matrix* UResLocater::K

private

kalman gain

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

matrix * UResLocater::K3

private

Referenced by correctionUpdate3(), init_matrix(), and make_matrix().

ULocaterLineResult UResLocater::locDecs[MAX_LOC_DECS]

private

detected lines

Referenced by doLocatorUpdates(), and locate().

int UResLocater::locDecsCnt

private

number of detected lines

Referenced by doLocatorUpdates(), and locate().

ULogFile UResLocater::logEnd

protected

logfile for updates - matlab style

Referenced by closelog(), locate(), rowEndDetect(), and UResLocater().

ULogFile UResLocater::logUpd

protected

logfile for updates - matlab style

Referenced by closelog(), locate(), openlog(), and UResLocater().

double UResLocater::ls_offset

Robot parameters

Referenced by locate(), and match_points().

char* UResLocater::mapfile

Path to mapfile

double UResLocater::max_angle_deviation

Update limits - angle limit

Referenced by UFunctionLocater::handleLocater(), locate(), and UResLocater().

const int UResLocater::MAX_LOC_DECS = 20

staticprivate

detection results maximum number of line detections in one scan

Referenced by locate().

int UResLocater::min_points

Update limits - number of correlated points

Referenced by UFunctionLocater::handleLocater(), locate(), and UResLocater().

UMatrix4 UResLocater::mPu

private

matrix declarations updated covariance maintained by averaged update function

Referenced by doLocatorUpdates(), init_matrix(), and locate().

UMatrix4 UResLocater::mXu

private

updated locater state maintained by averaged update function

Referenced by doLocatorUpdates(), init_matrix(), and locate().

matrix* UResLocater::P

private

starte and estimated covariance matrix

Referenced by init_matrix(), locate(), make_matrix(), odoPredict(), and set_utmstate().

matrix * UResLocater::P_

private

Referenced by correctionUpdate(), correctionUpdate3(), doLocatorUpdates(), init_matrix(), locate(), make_matrix(), odoPredict(), and set_utmstate().

matrix* UResLocater::P_1

private

temporary elements of P during odometry update

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix * UResLocater::P_2

private

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix * UResLocater::P_3

private

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix * UResLocater::P_4

private

Referenced by init_matrix(), make_matrix(), and odoPredict().

UPolygon * UResLocater::poly40

private

Referenced by makeRowPolygon(), set_state(), and UResLocater().

UPolygon* UResLocater::poly400

private

temporary polygon for debug

Referenced by makeRowPolygon(), set_state(), and UResLocater().

UPose UResLocater::pose_last

protected

Referenced by locate(), match_points(), odoPredict(), and set_poselast().

matrix* UResLocater::Q

private

state noise matrix

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix* UResLocater::R

private

measurement error

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

matrix * UResLocater::R3

private

Referenced by correctionUpdate3(), init_matrix(), and make_matrix().

matrix* UResLocater::S

private

sensitiviti matrix for kalman gain calculation

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

matrix * UResLocater::S3

private

Referenced by correctionUpdate3(), init_matrix(), and make_matrix().

matrix * UResLocater::S3_inv

private

Referenced by correctionUpdate3(), init_matrix(), and make_matrix().

matrix * UResLocater::S_inv

private

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

unsigned long UResLocater::serial

Referenced by locate(), rowEndDetect(), and UResLocater().

ULogFile UResLocater::testfid

protected

Referenced by closelog(), locate(), openlog(), and UResLocater().

matrix* UResLocater::tmp

private

temporary matrices

Referenced by correctionUpdate(), correctionUpdate3(), init_matrix(), make_matrix(), and set_state().

matrix * UResLocater::tmp22

private

Referenced by init_matrix(), and make_matrix().

matrix * UResLocater::tmp23

private

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

matrix * UResLocater::tmp31

private

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

matrix * UResLocater::tmp32

private

Referenced by correctionUpdate(), init_matrix(), make_matrix(), and odoPredict().

matrix * UResLocater::tmp33

private

Referenced by correctionUpdate(), correctionUpdate3(), init_matrix(), and make_matrix().

double UResLocater::ttarget

protected

bool UResLocater::update_allow

Flag indicating whether updates are allowed or not:

Referenced by UFunctionLocater::handleLocater(), locate(), and UResLocater().

double UResLocater::var_d

Measurement noise intensities

Referenced by doLocatorUpdates(), UFunctionLocater::handleLocater(), init_matrix(), and UResLocater().

double UResLocater::var_phi

Referenced by doLocatorUpdates(), UFunctionLocater::handleLocater(), init_matrix(), and UResLocater().

double UResLocater::var_t

Referenced by doLocatorUpdates(), init_matrix(), and UResLocater().

UVariable* UResLocater::varDist

protected

Referenced by match_points().

UVariable* UResLocater::varkalmpose

protected

Referenced by createBaseVar(), locate(), set_state(), set_utmstate(), and varprint().

UVariable* UResLocater::varKeepMapPose

protected

flag to allow update of mapPose

Referenced by createBaseVar(), locate(), set_state(), set_utmstate(), and setUtmPoseToMapPose().

UVariable* UResLocater::varMaxAngleDeviation

protected

Referenced by createBaseVar(), and locate().

UVariable* UResLocater::varOdoDistScale

protected

scale to multiply driven distance - to compensate for soft ground and tire variations

Referenced by createBaseVar(), and odoPredict().

UVariable* UResLocater::varRowEndIn1

protected

distance inside coverage before row end detect - when expected out of range.

Referenced by createBaseVar(), and rowEndDetect().

UVariable* UResLocater::varRowEndIn2

protected

distance inside coverage before row end detect - when expected in range

Referenced by createBaseVar(), and rowEndDetect().

UVariable* UResLocater::varRowEndMaxDist

protected

max usable distance movement for row end detection - should be covariance dependent

Referenced by createBaseVar(), and doLocatorUpdates().

UVariable* UResLocater::varRowEndMaxGap

protected

Referenced by createBaseVar(), and rowEndDetect().

UVariable* UResLocater::varRowExtraLog

protected

Referenced by createBaseVar(), locate(), and rowEndDetect().

UVariable* UResLocater::varRowToRobotMaxDist

protected

maximum distance to a row used for line end detection

Referenced by createBaseVar(), and doLocatorUpdates().

UVariable* UResLocater::varRowWidthFactor

protected

Referenced by createBaseVar(), and match_points().

UVariable* UResLocater::varUpdcnt

protected

Referenced by createBaseVar(), locate(), quality(), and set_utmstate().

UVariable* UResLocater::varUseAvgUpdate

protected

use averaged update of locater, rather than each line individually

Referenced by createBaseVar(), and locate().

UVariable* UResLocater::varUseRowEnd

protected

flag to allow row-end to be used

Referenced by createBaseVar(), doLocatorUpdates(), and locate().

matrix* UResLocater::x

private

state and estimate state (twice) matrix (3x1)

Referenced by init_matrix(), locate(), make_matrix(), match_points(), odoPredict(), set_state(), set_utmstate(), and varprint().

matrix * UResLocater::x_

private

Referenced by correctionUpdate(), correctionUpdate3(), init_matrix(), locate(), make_matrix(), and odoPredict().

double UResLocater::x_array[MAPLENGTH][POINTS_MAX]

private

array of measurement points for each of the lines in the map - for line estimate

Referenced by locate(), match_points(), and rowEndDetect().

matrix* UResLocater::x_odo

private

odometry based state update

Referenced by init_matrix(), make_matrix(), and odoPredict().

matrix * UResLocater::x_up

private

Referenced by doLocatorUpdates(), init_matrix(), locate(), and make_matrix().

double UResLocater::xe_array[MAPLENGTH][POINTS_MAX]

private

array of measurement points for each of the lines in the map - without line end test for line end test

Referenced by match_points(), and rowEndDetect().

double UResLocater::xtarget

protected

matrix * UResLocater::y3_hat

private

Referenced by correctionUpdate3(), init_matrix(), and make_matrix().

double UResLocater::y_array[MAPLENGTH][POINTS_MAX]

private

Referenced by locate(), match_points(), and rowEndDetect().

matrix* UResLocater::y_hat

private

measurement change

Referenced by correctionUpdate(), init_matrix(), and make_matrix().

double UResLocater::ye_array[MAPLENGTH][POINTS_MAX]

private

Referenced by match_points(), and rowEndDetect().

double UResLocater::ytarget

protected

matrix* UResLocater::z

private

measurement vector Z={d; th} Z3 = {d, th, t}

Referenced by init_matrix(), and make_matrix().

matrix * UResLocater::z3

private

Referenced by make_matrix().

The documentation for this class was generated from the following files:

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation