#include <ufuncloca2.h>

Inheritance diagram for UFuncLoca2:

Public Member Functions
virtual bool	handleCommand (UServerInMsg msg, void extra)

virtual bool	setResource (UResBase *resource, bool remove)

	UFuncLoca2 ()

virtual	~UFuncLoca2 ()

Public Member Functions inherited from UFuncLaserBase
	UFuncLaserBase ()

virtual	~UFuncLaserBase ()

Public Member Functions inherited from UFuncPlugBase
	UFuncPlugBase ()

virtual	~UFuncPlugBase ()

Public Member Functions inherited from UFunctionBase
bool	addResource (UResBase resource, UFunctionBase owner)

void	closeLogfile ()

virtual const char *	commandList ()

virtual void	createResources ()

const char *	getAliasName ()

const char *	getLoadedFileName ()

void *	getLoadedModuleRef ()

virtual bool	gotAllResources (char *missingThese, int missingTheseCnt)

bool	isMine (const char *cmdName)

bool	isStatic ()
	is the module loaded as static A module is static if the ldFilename has a length of zero. More...

virtual const char *	name ()

bool	newCmd (UServerInMsg newMsg, UDataBase extra)

bool	newCommand (UServerInMsg newMsg, void extra)

bool	openLogfile (const char *name)

void	print (const char *preString)

virtual void	resourceUpdated ()

bool	sendEndTag (UServerInMsg *msg)

bool	sendEndTag ()

bool	sendFullTag (const char *attributes)

bool	sendHelpDone (UServerInMsg *msg)

bool	sendHelpDone ()

bool	sendHelpStart (UServerInMsg msg, const char subject)

bool	sendHelpStart (const char *subject)

bool	sendHelpStart ()

bool	sendMsg (UServerInMsg msg, const char message, int size)

bool	sendMsg (const char *message)

bool	sendMsg (UServerInMsg msg, const char message)

bool	sendMsg (int clientIdx, const char *message, int size)

bool	sendMsgInt (int clientIdx, const char *message)

bool	sendStartTag (const char *attributes)

bool	sendText (UServerInMsg msg, const char text)

bool	sendText (const char *text)

virtual void	setAliasName (const char *name)

virtual void	setCommand (const char cmdList, const char name, const char *note)

void	setLoadedModuleRef (void module, const char moduleFileName)

virtual void	setLoadTime (UTime)

void	setName (const char *name)

void	setVerbose (bool value)

	UFunctionBase ()

virtual	~UFunctionBase ()

Public Member Functions inherited from UCallBack
bool	addOnEvent (UOnEvent *dataObject)

virtual bool	onEvent (const char interface, const char dataType, void *data)

	UCallBack ()

virtual	~UCallBack ()

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 ()

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 bool	methodCall (const char name, const char paramOrder, char *strings, const double doubles, double value, UDataBase returnStruct=NULL, int returnStructCnt=NULL)

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)

void	setVarPool (UVarCalc *sharedVarPool)

int	updateQTvars (int structIdx)

	UResVarPool ()

virtual	~UResVarPool ()

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

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	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 ()

Protected Attributes
ULogFile	innoLog

ULocaMatchStat *	jointMatch [MJM]

UDataBase *	laserLines [MAX_LAS_CNT]

int	laserLinesCnt

ULaserMatches *	laserMatches [MML]

int	laserMatchesCnt

UPosRot	lasPose

UPoseTime	lastOdoPose

list< LEL_ARLine >	lineList

double	mahaMean

ULineMatch *	matchList [MML]

int	matchListCnt
	number of entries in match list More...

int	matchMiss

Matrix< double, 3, 1 >	pose

Matrix< double, 3, 3 >	poseCov

UResPoseHist *	poseHist

int	poseIndex

double	transTh

double	transX

double	transY

UVariable *	varCovar
	value of current covariance matrix More...

UVariable *	varCovarA
	eigenvalues of x,y part of current covariance matrix More...

UVariable *	varCovarV
	eigenvectors of x,y part of current covariance matrix More...

UVariable *	varDefinedLines
	number of defined lines in localizer More...

UVariable *	varFailed
	number of failed matches since last successful match More...

UVariable *	varKL
	distance varaince for left wheel each moved meter More...

UVariable *	varKR
	distance varaince for right wheel each moved meter More...

UVariable *	varLaserAlpha
	The noise covariances for laser readings. More...

UVariable *	varLaserR

UVariable *	varMahaDistNoCorr
	mahalanobi distance cost, when using no correlation More...

UVariable *	varMaxThVariance

UVariable *	varMaxXYVariance
	The maximum allowed variances for each hypothesis. More...

UVariable *	varMinOverlap
	number of points that must be inside line segment to correlate More...

UVariable *	varOdoB
	robot base (differential drive?) More...

UVariable *	varOffsetGain
	gain for filtering offset to odometry, default is 0.01 (100 updates) More...

UVariable *	varOffsetValue
	automatic calculated offset to odometry updates to get a bias-free update - distance and heading More...

UVariable *	varUpdates
	number of successful match updates More...

Protected Attributes inherited from UFuncLaserBase
ULaserData	dataBuff

int	lastDevice

unsigned long	lastSerial

Protected Attributes inherited from UFunctionBase
char	aliasName [MAX_ID_LENGTH]

UCmdExe *	cmdHandler

char *	cmdToks [20]

int	cmdToksCnt

char	commandsHandled [MAX_RESOURCE_LIST_SIZE]

UServerInMsg *	msg

char	pluginName [MAX_RESOURCE_LIST_SIZE]

bool	verboseMessages

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

Static Protected Attributes
static const int	MAX_LAS_CNT = 200

static const int	MJM = 50

static const int	MML = 200
	list of valid matches (Mahalanobi distance less than some limit) More...

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...

Private Member Functions
void	addLine (UPosition p1, UPosition p2)

int	correlateLines (UPoseTime uNewPose)

int	correlateLines_old (list< LEL_GFLine > GFLL, UPoseTime uNewPose)

void	createBaseVar ()

bool	handleAddLine (UServerInMsg msg, int added)

bool	handleLocalize (UServerInMsg msg, void extra)

void	markBestJointMatches ()

void	projectToLaser (LEL_ARLine worldLine, Matrix< double, 3, 1 > &pose, Matrix< double, 3, 3 > &poseCov, LEL_ARLine &projLine, Matrix< double, 2, 2 > &lineCov, Matrix< double, 2, 3 > &delH_delP)

void	updateCovStatus ()

void	updateDisplacement (UTime &timeOfPose, Matrix< double, 3, 1 > &pose, Matrix< double, 3, 3 > &poseCov, UTime scanTime, UResPoseHist *poseHist, int silent, double odoB, double KR, double KL)

void	updateStatus (double mapX, double mapY, double mapTh, UTime time)

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

Public Attributes inherited from UFunctionBase
bool	silent
	is reply to client and console to be limited More...

Public Attributes inherited from UReplay
bool	replayTimeAdvancePending

UTime	replayTimeNow

Static Public Attributes inherited from UFunctionBase
static const int	MAX_ID_LENGTH = 30

static const int	MAX_RESOURCE_LIST_SIZE = 250

Protected Member Functions inherited from UFuncLaserBase
ULaserDevice *	getDevice (UServerInMsg msg, ULaserData pushData)

ULaserData *	getScan (UServerInMsg msg, ULaserData pushData, bool any=false, ULaserDevice **dev=NULL)

Protected Member Functions inherited from UFunctionBase
int	getCmdIndex ()

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

bool	isClientAlive (int clientIdx, double holdOffTime)

bool	sendDebug (UServerInMsg msg, const char debugText)

bool	sendDebug (const char *debugText)

bool	sendError (UServerInMsg msg, const char errorText)

bool	sendError (const char *errorText)

bool	sendHelp (UServerInMsg msg, const char infoText)

bool	sendHelp (const char *infoText)

bool	sendInfo (UServerInMsg msg, const char infoText)

bool	sendInfo (const char *infoText)

bool	sendWarning (UServerInMsg msg, const char warningText)

bool	sendWarning (const char *warningText)

UResBase *	setThisResource (const char ID, UResBase resource, bool remove, bool changed, UResBase currentResPtr, bool *isLocal)

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...

Detailed Description

Localizer based on Enis implementation of kalman localizer filter and - in unknown state - a multi hypothesis filter. This version is modified for more consensus based correlation by

Author: Christian Andersen

Constructor & Destructor Documentation

UFuncLoca2::UFuncLoca2 ( )

Constructor

References createBaseVar(), ULogFile::getF(), innoLog, laserMatches, laserMatchesCnt, lastOdoPose, matchList, matchListCnt, matchMiss, MML, ULogFile::openLog(), poseIndex, UFunctionBase::setCommand(), ULogFile::setLogName(), UPoseTime::t, transTh, transX, transY, and UTime::valid.

UFuncLoca2::~UFuncLoca2 ( )

virtual

Destructor

References ULogFile::closeLog(), innoLog, laserLines, laserMatches, matchList, MAX_LAS_CNT, and MML.

Member Function Documentation

void UFuncLoca2::addLine	(	UPosition	p1,
		UPosition	p2
	)

private

Add a detectable line from 3D position p1 to 3D position p2

int UFuncLoca2::correlateLines ( UPoseTime uNewPose )

private

Correlate laser lines and map lines using 2D-line segment laser lines.

Parameters

uNewPose is robot pose at time of data lines

Returns: number of matches

References a, ULineMatch::addToPoly(), LEL_ARLine::alpha, ULineMatch::delH_delP, ULineMatch::difA, ULineMatch::difR, fswap(), U2Dlined::getARLine(), UVariable::getDouble(), UResPoly::getItem(), UResPoseHist::getOdoPoseOrigin(), U2Dseg::getOtherEnd(), UFunctionBase::getStaticResource(), UVariable::getValued(), UResPoly::gotNewData(), ULineMatch::laserLine, ULineMatch::laserLineMatch, laserLines, laserLinesCnt, LEL_ARLine::lb, LEL_ARLine::le, LEL_ARLine::limited, limitToPi(), ULineMatch::lineDifCovAR, ULineMatch::lineDifCovARinv, lineList, ULock::lock(), ULineMatch::mapLine, matchList, matchListCnt, ULineMatch::minMahDist, MML, ULineMatch::pe1, ULineMatch::pe2, pose, poseCov, LEL_ARLine::positionAlongLine(), ULineMatch::print(), projectToLaser(), LEL_ARLine::r, UPolygon::setColor(), ULineMatch::setExpectedPose(), UPolygon::setPointsCnt(), UPolyItem::setUpdated(), UFunctionBase::silent, sqr(), ULock::unlock(), ULineMatch::valid, varLaserAlpha, varLaserR, varMinOverlap, U2Dpos::x, and U2Dpos::y.

Referenced by handleLocalize().

int UFuncLoca2::correlateLines_old	(	list< LEL_GFLine >	GFLL,
		UPoseTime	uNewPose
	)

private

Correlate laser lines and map lines - using old LEL_GFline laser line list

Parameters

GFLL	is detected lines in laser scanner coordinates
uNewPose	is robot pose at time of data lines

Returns: number of matches

void UFuncLoca2::createBaseVar ( )

private

Create local variables for manipulating parameters

distance varaince for right wheel each moved meter

distance varaince for left wheel each moved meter

References UResVarPool::addVar(), UResVarPool::addVarA(), varCovar, varCovarA, varCovarV, varDefinedLines, varFailed, varKL, varKR, varLaserAlpha, varLaserR, varMahaDistNoCorr, varMinOverlap, varOdoB, varOffsetGain, varOffsetValue, and varUpdates.

Referenced by UFuncLoca2().

bool UFuncLoca2::handleAddLine	(	UServerInMsg *	msg,
		int *	added
	)

private

Handle addition of map lines.

Parameters

msg	is a struct with further parameters
added	is number of lines added.

Returns: true if no parameter error

References UPosition::dist(), USmlTagIn::getAttDouble(), USmlTagIn::getAttValue(), UResPoly::getNext(), UPolygon::getPoints(), UPolygon::getPointsCnt(), UFunctionBase::getStaticResource(), lineList, LEL_ARLine::MNL, UFunctionBase::name(), UPolyItem::name, UServerInMsg::tag, UPosition::x, and UPosition::y.

Referenced by handleCommand().

bool UFuncLoca2::handleCommand	(	UServerInMsg *	msg,
		void *	extra
	)

virtual

Handle incomming command (intended for command separation) Must return true if the function is handled - otherwise the client will get a failed - reply

Reimplemented from UFunctionBase.

References UPolygon::add(), UResPoly::add(), UPolygon::clear(), UPolygon::color, UPolyItem::cooSys, UPosition::dist(), USmlTagIn::getAttBool(), USmlTagIn::getAttDouble(), USmlTagIn::getAttValue(), UResPoly::getItem(), UPolygon::getPointsCnt(), UFunctionBase::getStaticResource(), handleAddLine(), handleLocalize(), lastOdoPose, lineList, pose, poseCov, poseIndex, UFunctionBase::sendHelpDone(), UFunctionBase::sendHelpStart(), UFunctionBase::sendInfo(), UFunctionBase::sendMsg(), UFunctionBase::sendText(), UFunctionBase::sendWarning(), UPosition::set(), UVariable::setDouble(), UFunctionBase::silent, UPoseTime::t, UServerInMsg::tag, transTh, transX, transY, updateCovStatus(), updateStatus(), UTime::valid, and varDefinedLines.

bool UFuncLoca2::handleLocalize	(	UServerInMsg *	msg,
		void *	extra
	)

private

get robot pose at laser time

prepare match array with laser lines

update since last localize

correlate map lines and laser lines - in laser coordinates

SMRDEMO reply format

References UVariable::add(), ULaserMatches::best, UResVarPool::callGlobalV(), UServerInMsg::client, correlateLines(), ULineMatch::delH_delP, ULineMatch::difA, ULineMatch::difR, dist, UVariable::getDouble(), ULogFile::getF(), UVariable::getInt(), UTime::GetMicrosec(), UTime::getSec(), UVariable::getValued(), UPose::h, innoLog, ULogFile::isOpen(), laserLines, laserLinesCnt, laserMatches, laserMatchesCnt, lastOdoPose, limitd(), limitToPi(), ULineMatch::lineDifCovAR, lineList, ULineMatch::mapLine, ULaserMatches::mapLines, ULaserMatches::mapLinesCnt, markBestJointMatches(), matchListCnt, matchMiss, MAX_LAS_CNT, ULineMatch::minMahDist, LEL_ARLine::name, pose, poseCov, poseHist, UFunctionBase::sendInfo(), UFunctionBase::sendMsg(), UFunctionBase::sendWarning(), UVariable::setValued(), UFunctionBase::silent, sqr(), UPoseTime::t, transTh, transX, transY, updateDisplacement(), updateStatus(), ULineMatch::valid, varKL, varKR, varOdoB, varOffsetGain, varOffsetValue, varUpdates, UPose::x, and UPose::y.

Referenced by handleCommand().

void UFuncLoca2::markBestJointMatches ( )

private

Use list of laser line matches to mark best match for each of the candidate correlations

Test innovation distance for all correlations based on one of the correlations are right. The series of best correlations are then used - some may be false

First build laser line list with all possible map correlations
then seed an update bast on one of these correlations, and
for each seed, test all other correlations
what should be out? more than 1 sigma mahalanobi?
what about no correlation as alternative, even if within 1 sigma

References ULaserMatches::best, ULocaMatchStat::best, bool2str(), UVariable::getDouble(), U2Dseg::getFirstEnd(), U2Dseg::getOtherEnd(), ULocaMatchStat::init(), jointMatch, ULineMatch::laserLineMatch, laserLines, laserMatches, laserMatchesCnt, U2Dseg::length, UResBase::line, ULocaMatchStat::mahaScore, ULineMatch::mapLine, ULaserMatches::mapLines, ULaserMatches::mapLinesCnt, matchList, matchListCnt, ULaserMatches::MLM, LEL_ARLine::name, pose, ULocaMatchStat::poseCovOrg, ULocaMatchStat::poseOrg, ULocaMatchStat::seedLaserLine, ULocaMatchStat::seedMapMatch, UFunctionBase::silent, ULocaMatchStat::skippedCorrelationsCnt, sqr(), ULocaMatchStat::testMatches(), ULocaMatchStat::usedCorrelationsCnt, varLaserAlpha, varLaserR, varMahaDistNoCorr, U2Dpos::x, and U2Dpos::y.

Referenced by handleLocalize().

void UFuncLoca2::projectToLaser	(	LEL_ARLine	worldLine,
		Matrix< double, 3, 1 > &	pose,
		Matrix< double, 3, 3 > &	poseCov,
		LEL_ARLine &	projLine,
		Matrix< double, 2, 2 > &	lineCov,
		Matrix< double, 2, 3 > &	delH_delP
	)

private

References LEL_ARLine::alpha, fswap(), URotation::Kappa, lasPose, LEL_ARLine::lb, LEL_ARLine::le, LEL_ARLine::limited, pose, LEL_ARLine::positionAlongLine(), LEL_ARLine::r, UPosition::x, and UPosition::y.

Referenced by correlateLines().

bool UFuncLoca2::setResource	(	UResBase *	resource,
		bool	remove
	)

virtual

Called by the server core, when a new resource is available (or is removed), local pointers to the resource should be updated as appropriate.

Reimplemented from UFunctionBase.

References UResPoseHist::getOdoPoseID(), UResBase::isA(), poseHist, and UFunctionBase::setResource().

void UFuncLoca2::updateCovStatus ( )

private

Update covariance status in global variables.

References UMatrix::cols(), UMatrix4::eig2x2(), UMatrix::get(), UMatrix::getData(), poseCov, UMatrix::rows(), UMatrix::set(), UMatrix::setMat(), UMatrix::setSize(), UVariable::setValueM(), varCovar, varCovarA, and varCovarV.

Referenced by handleCommand(), and updateStatus().

void UFuncLoca2::updateDisplacement	(	UTime &	timeOfPose,
		Matrix< double, 3, 1 > &	pose,
		Matrix< double, 3, 3 > &	poseCov,
		UTime	scanTime,
		UResPoseHist *	poseHist,
		int	silent,
		double	odoB,
		double	KR,
		double	KL
	)

private

References dist, UTime::getDecSec(), UVariable::getDouble(), UResPoseHist::getNewest(), UPoseTVQ::getPose(), UResPoseHist::getPoseNearTime(), UPose::h, interpolatePoses(), limitToPi(), pose, sqr(), UPoseTime::t, varOffsetValue, UPose::x, and UPose::y.

Referenced by handleLocalize().

void UFuncLoca2::updateStatus	(	double	mapX,
		double	mapY,
		double	mapTh,
		UTime	time
	)

private

Update module status (global variables)

Parameters

mapX	is the localized position in X
mapY	is the localized position in Y
mapTh	is the localized position in radians
time	is the valid time for the position
match	was the last update a match.

References UResPoseHist::addIfNeeded(), UFunctionBase::getStaticResource(), matchMiss, UPoseTVQ::q, UVariable::setDouble(), updateCovStatus(), and varFailed.

Referenced by handleCommand(), and handleLocalize().

Member Data Documentation

ULogFile UFuncLoca2::innoLog

protected

Innovation logfile

Referenced by handleLocalize(), UFuncLoca2(), and ~UFuncLoca2().

ULocaMatchStat* UFuncLoca2::jointMatch[MJM]

protected

Referenced by markBestJointMatches().

UDataBase* UFuncLoca2::laserLines[MAX_LAS_CNT]

protected

laser line list - first element is robot pose at scantime (UPoseTime*) the rest is 2D segments (U2Dseg*)

Referenced by correlateLines(), handleLocalize(), ULocaMatchStat::init(), markBestJointMatches(), and ~UFuncLoca2().

int UFuncLoca2::laserLinesCnt

protected

number of valid elements in laser line list

Referenced by correlateLines(), handleLocalize(), and ULocaMatchStat::init().

ULaserMatches* UFuncLoca2::laserMatches[MML]

protected

Referenced by handleLocalize(), markBestJointMatches(), UFuncLoca2(), and ~UFuncLoca2().

int UFuncLoca2::laserMatchesCnt

protected

Referenced by handleLocalize(), markBestJointMatches(), and UFuncLoca2().

UPosRot UFuncLoca2::lasPose

protected

Referenced by projectToLaser().

UPoseTime UFuncLoca2::lastOdoPose

protected

Referenced by handleCommand(), handleLocalize(), and UFuncLoca2().

list<LEL_ARLine> UFuncLoca2::lineList

protected

Referenced by correlateLines(), handleAddLine(), handleCommand(), and handleLocalize().

double UFuncLoca2::mahaMean

protected

the mean Mahalanobi distance in all matches in matchList, to be used as distance value for no-match test.

ULineMatch* UFuncLoca2::matchList[MML]

protected

Referenced by correlateLines(), markBestJointMatches(), UFuncLoca2(), and ~UFuncLoca2().

int UFuncLoca2::matchListCnt

protected

number of entries in match list

Referenced by correlateLines(), handleLocalize(), markBestJointMatches(), and UFuncLoca2().

int UFuncLoca2::matchMiss

protected

Referenced by handleLocalize(), UFuncLoca2(), and updateStatus().

const int UFuncLoca2::MAX_LAS_CNT = 200

staticprotected

maximum number of laser lines in one svan

Referenced by handleLocalize(), and ~UFuncLoca2().

const int UFuncLoca2::MJM = 50

staticprotected

array of best joint correlations

const int UFuncLoca2::MML = 200

staticprotected

list of valid matches (Mahalanobi distance less than some limit)

Referenced by correlateLines(), UFuncLoca2(), and ~UFuncLoca2().

Matrix<double,3,1> UFuncLoca2::pose

protected

Referenced by correlateLines(), handleCommand(), handleLocalize(), markBestJointMatches(), projectToLaser(), and updateDisplacement().

Matrix<double,3,3> UFuncLoca2::poseCov

protected

Referenced by correlateLines(), handleCommand(), handleLocalize(), and updateCovStatus().

UResPoseHist* UFuncLoca2::poseHist

protected

Referenced by handleLocalize(), and setResource().

int UFuncLoca2::poseIndex

protected

Referenced by handleCommand(), and UFuncLoca2().

double UFuncLoca2::transTh

protected

Referenced by handleCommand(), handleLocalize(), and UFuncLoca2().

double UFuncLoca2::transX

protected

Referenced by handleCommand(), handleLocalize(), and UFuncLoca2().

double UFuncLoca2::transY

protected

Referenced by handleCommand(), handleLocalize(), and UFuncLoca2().

UVariable* UFuncLoca2::varCovar

protected

value of current covariance matrix

Referenced by createBaseVar(), and updateCovStatus().

UVariable* UFuncLoca2::varCovarA

protected

eigenvalues of x,y part of current covariance matrix

Referenced by createBaseVar(), and updateCovStatus().

UVariable* UFuncLoca2::varCovarV

protected

eigenvectors of x,y part of current covariance matrix

Referenced by createBaseVar(), and updateCovStatus().

UVariable* UFuncLoca2::varDefinedLines

protected

number of defined lines in localizer

Referenced by createBaseVar(), and handleCommand().

UVariable* UFuncLoca2::varFailed

protected

number of failed matches since last successful match

Referenced by createBaseVar(), and updateStatus().

UVariable* UFuncLoca2::varKL

protected

distance varaince for left wheel each moved meter

Referenced by createBaseVar(), and handleLocalize().

UVariable* UFuncLoca2::varKR

protected

distance varaince for right wheel each moved meter

Referenced by createBaseVar(), and handleLocalize().

UVariable* UFuncLoca2::varLaserAlpha

protected

The noise covariances for laser readings.

Referenced by correlateLines(), createBaseVar(), and markBestJointMatches().

UVariable* UFuncLoca2::varLaserR

protected

Referenced by correlateLines(), createBaseVar(), and markBestJointMatches().

UVariable* UFuncLoca2::varMahaDistNoCorr

protected

mahalanobi distance cost, when using no correlation

Referenced by createBaseVar(), and markBestJointMatches().

UVariable* UFuncLoca2::varMaxThVariance

protected

UVariable* UFuncLoca2::varMaxXYVariance

protected

The maximum allowed variances for each hypothesis.

UVariable* UFuncLoca2::varMinOverlap

protected

number of points that must be inside line segment to correlate

Referenced by correlateLines(), and createBaseVar().

UVariable* UFuncLoca2::varOdoB

protected

robot base (differential drive?)

Referenced by createBaseVar(), and handleLocalize().

UVariable* UFuncLoca2::varOffsetGain

protected

gain for filtering offset to odometry, default is 0.01 (100 updates)

Referenced by createBaseVar(), and handleLocalize().

UVariable* UFuncLoca2::varOffsetValue

protected

automatic calculated offset to odometry updates to get a bias-free update - distance and heading

Referenced by createBaseVar(), handleLocalize(), and updateDisplacement().

UVariable* UFuncLoca2::varUpdates

protected

number of successful match updates

Referenced by createBaseVar(), and handleLocalize().

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

Public Member Functions

Protected Attributes

Static Protected Attributes

Private Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation