ArLocalizationTask Class Reference

#include <ArLocalizationTask.h>

Inheritance diagram for ArLocalizationTask:

List of all members.

---

Detailed Description

Task that performs continuous localization of the robot with a laser range sensor in a seperate asynchronous thread.

The localization software can be used to localize a robot in a given map using the laser rangefinder. The system is meant to be used along with ARIA.

The localization system uses Monte Carlo Localization Algorithm to accurately localize the robot in the given map using its laser data. The localization task is meant to be initialized and run in a separate thread in ARIA. It can be used with a real robot or with a simulator.

In order to get the localization task thread going, the ArLocalizationTask class needs an instantiated ArRobot, ArRangeDevice and a map file of the robot's environment. The output of the localization will be reflected directly in the pose of the ArRobot. (unless explicitly set to not do so)

The basic MCL localization system has also been augmented with a Kalman filter which will fuse the wheel encoder data with the reflections from known laser reflecting landmarks (these are constructed with special reflective plastic manufactured by laser) in the map. To use this functionality you will need a map with reflector positions in it, and will have to configure the laser to return reflectance information. ARNL will incrementally incorporate the reflections from the landmarks using a EKF formulation. See the main ARNL overview page for details on this extra feature, including how to enable reflectance data in the laser.

ArLocalizationTask automatically creates and runs its background thread when constructed.

Examples:

arnlJustLocalization.cpp, and arnlServer.cpp.

ArNetworking callback methods
(used by server classes in ArServerClasses.cpp)
void	drawRangePoints (ArServerClient *client, ArNetPacket *packet)
	Draws range data used for localization.
void	drawReflectorRays (ArServerClient *client, ArNetPacket *packet)
	Draw rays from the robot to the reflector.
void	drawSamplePoses (ArServerClient *client, ArNetPacket *packet)
	Draws debugging objects if filled.
void	drawSampleBounds (ArServerClient *client, ArNetPacket *packet)
	Draws the sample bounds.
void	drawKalmanVariance (ArServerClient *client, ArNetPacket *packet)
	Draws the sample bounds.
void	drawMCLVariance (ArServerClient *client, ArNetPacket *packet)
	Draws the sample bounds.
virtual int	getPoseInterpPosition (ArTime timeStamp, ArPose *position)
	See also: ArInterpolation::getPose
void	setMultiRobotCallback (ArRetFunctor< std::list< ArMultiRobotPoseAndRadius > > *func)
	Sets the callback function for localization to know about other robots.
Public Member Functions
	ArLocalizationTask (ArRobot *robot, ArRangeDevice *laser, char *mapName)
	Base constructor with all the necessary inputs.
	ArLocalizationTask (ArRobot *robot, ArRangeDevice *laser, ArMapInterface *ariaMap, bool noReflectors=false)
	Base constructor with all the necessary inputs.
virtual	~ArLocalizationTask (void)
	Base destructor.
bool	localizeRobotInMapInit (ArPose given, int numSamples, double stdX, double stdY, double stdT, double thresFactor, bool warn=true, bool setInitializedToFalse=true, bool rayTrace=true)
	Localization function mainly for initialization of robot at given pose.
bool	localizeRobotInMapMoved (int numSamples, double distFactor, double angFactor, double thresFactor)
	Function used to do the localization after motion (normally called automatically by the background task).
bool	localizeRobotAtHomeBlocking (double distSpread, double angleSpread, double probThreshold)
	Try initial localization at each home point at the map, and set the robot pose to the point with best score, using given parameter values instead of values previously configured.
bool	localizeRobotAtHomeBlocking (double spreadX, double spreadY, double angleSpread, double probThreshold)
	instead of values previously configured
virtual bool	localizeRobotAtHomeBlocking (double distSpread, double angleSpread)
	The common function call from above.
virtual bool	localizeRobotAtHomeBlocking ()
	Try initial localization at each home point at the map, \ and set the robot pose to the point with best score.
bool	localizeRobotAtHomeNonBlocking (void)
	Request that the task later localize the robot at a map home \ position, then return immediately.
virtual ArPose	getRobotHome (void)
	Gets the pose that robot was localized to.
void	setForceUpdateParams (int numSamples, double xStd, double yStd, double tStd)
	Sets the force update parameters.
void	forceUpdatePose (ArPose forcePose, bool rayTrace=true)
	Force an update in thread instead of waiting for distance-angle trigger.
void	addFailedLocalizationCB (ArFunctor1< int > *functor)
	Adds a callback which will be called when loca fails.
void	remFailedLocalizationCB (ArFunctor1< int > *functor)
	Removes a callback.
bool	setGridResolution (double res, ArMapInterface *ariaMap)
	Sets the occupancy grid with the new map.
void	setFailedCallBack (ArFunctor1< int > *fcb)
	Sets tracking failed Callback.
virtual LocalizationState	getState (void)
	Gets the state of localization.
bool	getIdleFlag (void)
	Gets the idle flag.
bool	getReloadingMapFlag (void)
	Gets the map reloading flag.
ArMapInterface *	readMapFromFile (char *mapName)
	Read the Map data from a map file.
ArMapInterface *	readAriaMap (ArMapInterface *ariaMap)
	Read the Map data from ArMap.
bool	loadParamFile (const char *file)
	Load parameters from the given filename into ArConfig.
bool	saveParams (char *filename)
	Saves all parameter values from ArConfig to the given file.
Modifiers for configuration values.
Normally these values are automatically set via ArConfig (e.g. by loading them from a file or other source) in the "Localization" section, and these methods would only be used internally by ARNL. However, they can be used if you are not using ArConfig or wish to override a setting.
void	setTriggerDelR (double tr)
	Sets the motion trigger value for distance.
void	setTriggerDelT (double tt)
	Sets the motion trigger value for angle.
void	setTriggerTimeFlag (bool tt)
	Sets the flag to trigger on idle.
void	setTriggerTime (double tt)
	Sets the idle trigger time in millisecs.
void	setTriggerTimeX (double tt)
	Sets the X range when localization triggers due to idle time.
void	setTriggerTimeY (double tt)
	Sets the Y range when localization triggers due to idle time.
void	setTriggerTimeTh (double tt)
	Sets the Theta range when localization triggers due to idle time.
void	setNumSamples (int n)
	Set the number of samples to use in the MCL.
void	setNumSamplesAtInit (int n)
	Set the number of samples to use during initialization.
void	setRayTraceAtInit (bool f)
	Set the flag which decides on ray tracing for localize at init.
void	setCurrentNumSamples (int n)
	Set the variable number of samples (usually adjusted automatically).
void	setPassThreshold (double f)
	Sets the pass threshold for fraction of laser points matched to env.
void	setSensorBelief (double sensorBelief)
	Sets the sensor belief.
void	setCurrentLocaPose (double x, double y, double th)
	Sets the current pose.
void	setCurrentLocaPose (ArPose p)
	Sets the current pose.
void	setVerboseFlag (bool a)
	Sets the verbose flag value. (for debugging).
void	setAngleIncrement (double f)
	Sets the laser increments.
void	setKillThreshold (double f)
void	setRecoverOnFailedFlag (bool f)
	Sets the recoverOnFailed flag.
void	setIdleFlag (bool f)
	Sets the idle flag.
void	setReloadingMapFlag (bool f)
	Sets the map reloading flag.
void	setEnableReflectorLocalizationFlag (bool f)
	Sets the flag to fuse all sensors.
void	setReflectorVar (double f)
	Sets the variance in the XY for the reflector centers.
void	setReflectorMatchDist (double f)
	Sets the radius to search for a reflector to match a given reflection.
void	setReflectorMatchAngle (double f)
	Sets the angle to search for a reflector to match a given reflection.
void	setReflectorMaxRange (double f)
	Sets the maximum distance the reflector can be seen.
void	setReflectorMaxAngle (double f)
	Sets the maximum incident angle the reflector can be seen.
void	setReflectorSize (double f)
	Sets the reflector size.
void	setReflectanceThreshold (int f)
	Sets the reflectance threshold.
void	setBypassMCLFlag (bool f)
	Sets the flag which decides to bypasses MCL and use reflectors.
Accessors for current configuration values.
These values are normally set via ArConfig (the "Localization" section) by loading a file or other means, or by calling the modifier functions above.
void	getForceUpdateParams (ArPose &forcePose, int &numSamples, double &xStd, double &yStd, double &tStd)
	Get the params to do the localization from the class.
bool	getVerboseFlag (void)
	Gets the verbose flag. (for debugging only).
bool	getInitializedFlag (void)
	Gets the initialized flag indicating if localization thread is on.
int	getNumSamples (void)
	Get the maximum number of samples used in the MCL.
int	getNumSamplesAtInit (void)
	Get the number of samples used in the MCL during initialization.
bool	getRayTraceAtInit (void)
	Get the flag which decides on ray tracing for localize at init.
int	getCurrentNumSamples (void)
	Get the variable number of samples if adjusted during move.
ArPose	getRobotMaxProbPose (void)
	Get the current computed best robot pose.
double	getTriggerDelR (void)
	Gets the min distance to localize.
double	getTriggerDelT (void)
	Gets the min angle to localize.
bool	getTriggerTimeFlag (void)
	Gets the flag indicating if localization should trigger on idle.
double	getTriggerTime (void)
	Gets the min time in millisecs to be idle.
double	getTriggerTimeX (void)
	Gets the X range of samples when localization triggers on idle.
double	getTriggerTimeY (void)
	Gets the Y range of samples when localization triggers on idle.
double	getTriggerTimeTh (void)
	Gets the Theta range of samples when localization triggers on idle.
double	getPassThreshold (void)
	Gets the Pass threshold for localization success.
double	getUsingPassThreshold (void)
	Gets the Pass threshold for localization success being used right now.
void	setTempPassThreshold (double passThreshold)
	Sets the Pass threshold to use until it is reset to config.
double	getTempPassThreshold (void)
	Gets the temporary pass threshold to use until it is reset to config.
void	clearTempPassThreshold (void)
	Resets the Pass threshold to config.
virtual double	getLocalizationScore (void)
	Gets the higher of MCL and Reflector based localization.
virtual double	getLocalizationThreshold (void)
	Gets the localization threshold.
double	getMCLocalizationScore (void)
	Gets the localization score based on Monte Carlo only.
double	getRefLocalizationScore (void)
	Gets the localization score based on reflectors only.
double	getSensorBelief (void)
	Gets the sensor belief.
ArPose	getCurrentLocaPose (void)
	Gets the last successful MCLocalized pose.
double	getStdX (void) const
double	getStdY (void) const
double	getStdTh (void) const
double	getErrorMmPerMm (void) const
double	getErrorDegPerDeg (void) const
double	getErrorDegPerMm (void) const
double	getSensorBelief (void) const
double	getPeakFactor (void) const
double	getOccThreshold (void) const
double	getGridRes (void) const
char *	getMapName (void)
double	getPeturbRangeX (void) const
double	getPeturbRangeY (void) const
double	getPeturbRangeTh (void) const
double	getFailedRangeX (void) const
double	getFailedRangeY (void) const
double	getFailedRangeTh (void) const
double	getPeakStdX (void) const
double	getPeakStdY (void) const
double	getPeakStdTh (void) const
double	getAngleIncrement (void) const
double	getKillThreshold (void) const
ArMapInterface *	getAriaMap (void)
int	getBufferSize (void)
	Scan Buffer size. (internal laser scan buffer).
std::vector< ArPose >	getXYBuffer (void)
	Scan Buffer XY.
ArPose	getBufferPose (void)
	Scan Buffer pose taken. (internal laser scan buffer).
ArOccGrid *	getOccGridPtr (void)
	Returns pointer to the occgrid.
virtual std::list< ArPose >	getCurrentSamplePoses (void)
	Gets the pose samples for client.
bool	getRecoverOnFailedFlag (void)
	Gets the recoverOnFailed flag.
bool	getIgnoreIllegalPoseFlag (void)
	Gets the flag which allows poses overlaping occupied points on map.
bool	getAdjustNumSamplesFlag (void)
	Gets the flag which allows for changing numSamples with loca score.
int	getMinNumSamples (void)
	Gets the minimum no of samples the localization will drop to.
double	getNumSamplesAngleFactor (void)
	Gets the rotation factor for adjusting no of samples with score.
bool	getSensorSetFlag (void)
	Gets the value of the sensor set flag which indicates new cycle.
bool	getEnableReflectorLocalizationFlag (void)
	Gets the flag to fuse all sensors.
double	getReflectorVar (void)
	Gets the variance in the XY for the reflector centers.
double	getReflectorMatchDist (void)
	Gets the radius to search for a reflector to match a given reflection.
double	getReflectorMatchAngle (void)
	Sets the angle to search for a reflector to match a given reflection.
double	getReflectorMaxRange (void)
	Gets the maximum distance the reflector can be seen.
double	getReflectorMaxAngle (void)
	Gets the maximum incident angle the reflector can be seen.
double	getReflectorSize (void)
	Gets the reflector size param.
int	getReflectanceThreshold (void)
	Gets the reflectance threshold.
bool	getBypassMCLFlag (void)
	Gets the flag which decides to bypasses MCL and use reflectors.
double	getReflectorTriDistLimit (void)
	Gets the triangulation dist limit.
double	getReflectorTriAngLimit (void)
	Gets the triangulation angle limit.
double	getBadReflectorFactor (void)
	Gets the bad reflector factor.
ArMatrix	getQParams (void)
	Gets the bad reflector factor.
Functions used internally by ARNL
bool	fillHistogram (double *&hist, double *&cumSum, int &numSamples)
	Fill prob distribution histogram for debugging.
bool	scanToGlobalCoords (ArPose robPose, std::vector< ArPose > &xyLrf)
	Convert the laser data to x and y coord array.
ArTime	getLastLocaTime (void)
	Gets the last loca time to now.
bool	setLocaParams (double xStd, double yStd, double tStd, double kMmPerMm, double kDegPerDeg, double kDegPerMm, double sensorBelief)
	Set all the parameters for localization.
void	setIgnoreIllegalPoseFlag (bool f)
	Sets the flag which allows poses overlaping occupied points on map.
void	setAdjustNumSamplesFlag (bool f)
	Sets the flag which changes numSamples with the localization score.
void	setMinNumSamples (int n)
	Sets the minimum no of samples the localization will drop to.
void	setNumSamplesAngleFactor (double f)
	Sets the rotation factor for adjusting no of samples with score.
void	setLastLocaTimeToNow (void)
	Sets the last loca time to now.
void	setSensorSetFlag (bool p)
	Sets the value of the sensor set flag which indicates new cycle.
bool	computeLastLocaMeanVar (ArPose &mean, ArMatrix &Var)
	The compute the mean and var as soon as MCL finds a pose.
bool	findMCLMeanVar (ArPose &mean, ArMatrix &Var)
	Finds the mean and var of MCL after moving to present pose.
virtual bool	findLocalizationMeanVar (ArPose &mean, ArMatrix &Var)
	The actual mean var calculator for the virtual in the base class.
virtual void	setCorrectRobotFlag (bool f)
	Sets the flag deciding whether to reflect localized pose onto the robot.
virtual void	setRobotPose (ArPose pose, ArPose spread=ArPose(0, 0, 0), int nSam=0)
	Used to set the robot pose usually at the start of localization.
virtual bool	getRobotIsLostFlag (void)
	Gets the lost flag.
virtual void	setLocalizationIdle (bool f)
	Set the localization idle.
virtual ArTransform	getEncoderToLocalizationTransform (void)
	Gets the transform from the encoder to the laser.
Protected Member Functions
void	basicInitialization (ArRobot *robot, ArRangeDevice *laser)
	Basic initializer (internal use only).
virtual void *	runThread (void *ptr)
	Function used to run the task as a thread.
bool	correctPoseFromSensor (double increment, bool rayTrace, bool ignoreBadProb)
	Function which does the main eqn for the actual MCL localization.
bool	getLocalizedFlag (void)
	Gets the localized flag. (do not use).
bool	getLocalizingFlag (void)
	Gets the localizing flag value. To avoid multiple localizing threads.
bool	getRobotMovedFlag (void)
	Gets the robot moved flag value. (Moved after last localization).
bool	getCorrectRobotFlag (void)
	Gets the flag indicating if localization result will be set on robot.
bool	getLogFlag (void)
	Gets the flag indicating if logging.
void	setLocalizedFlag (bool a)
	Sets the localized flag value. (do not use).
void	setInitializedFlag (bool a)
	Sets the initialized flag value indicating robot was localized or not.
void	setRobotMovedFlag (bool a)
	Sets the robot moved flag to set off MCL.
bool	initializeSamples (int numSamples)
	Initialize the MCL samples.
bool	scanLaserIntoArray (void)
	Get the laser buffer into data structure.
ArOccGrid *	initializeOccGrid (double res, ArMapInterface *ariaMap, bool lockMap=true)
	Set resolution of the occupancy grid and fill it.
void	killBadSamples (double obsThreshold)
	Legalize samples to eliminate illegal location such as on obstacles.
void	saveSamples (bool saveFile=false)
	Save samples for backup.
int	findBestPoses (ArRobotPoseSamples *mrsp, double factor)
	Find the best poses by looking at prob distribution peaks.
int	getNumBestPoses (ArRobotPoseSamples *mrsp)
	Get the number of peaks.
bool	findSamplesStatistics (double &xMean, double &yMean, double &thMean, double &xStd, double &yStd, double &tStd)
	Compute statistics of samples after localization.
bool	findKalmanMeanVar (ArPose &mean, ArMatrix &var)
	Return the state.
bool	fuseTwoDistributions (ArPose m1, ArMatrix V1, ArPose m2, ArMatrix V2, ArPose &mean, ArMatrix &Var)
	Fuses the stats of two uniform dists into one.
unsigned int	getLocaTime ()
	Return time of last good localization.
void	setMotionErrorParam (int index, double value)
	Set motion error params.
double	getMotionErrorParam (int index)
	Get motion error params.
void	robotCallBack (void)
	The sensor interpretation callback. Called every 100msec.
bool	configureLaser (void)
	Needed if the laser does not connect first.
bool	reconfigureLocalization (void)
	Needed if the params are changed or loaded again.
void	setupLocalizationParams (bool noReflectors=false)
	Setup the path planing params with values from ArConfig.
void	failedLocalization (int times)
	Actual thing that gets called when localization failed.
int	dynamicallyAdjustNumSamples (double dr, double dt)
	Change the sample size to reflect localization score.
void	mapChanged (void)
	Function for the things to do if map changes.
bool	findPoseFromLandmarks (const std::vector< ArPose > &global, const std::vector< ArPose > &local, ArPose &rPose, ArMatrix &Var)
	Solve the triangulation eqn.
bool	kalmanFilter (const std::vector< ArPose > &global, const std::vector< ArPose > &local, ArPose &pose, ArPose &deltaPose, ArPose &delta, bool failedLocalization, ArPose &mean, ArPose &stdDev, ArPose &maxInnov, double &refScore)
	Solve the Kalman thing.
int	getReflectionCoords (ArPose &poseTaken, ArPose &rPose, std::vector< ArPose > &gList, std::vector< ArPose > &lList)
	Find the reflectors in local and global coords.
int	getReflectorCenterCoords (ArPose &poseTaken, ArPose &rPose, std::vector< ArPose > &gList, std::vector< ArPose > &lList)
	Find the reflector centers in local and global coords.
bool	fuseSensorWithOdometry (bool failedLocalization)
	Fuse all the sensor information.
void	setFlagsAndCallbacksOnFail (int ntimes)
	Set the flags and call the right callbacks when loca fails ntimes.

---

Constructor & Destructor Documentation

ArLocalizationTask::ArLocalizationTask	(	ArRobot *	robot,
		ArRangeDevice *	laser,
		char *	mapname
	)

Base constructor with all the necessary inputs.

Constructor.

Parameters:

	robot,:	Robot data structure.
	laser,:	Laser data structure.
	mapname,:	Map file containing environment map.

ArLocalizationTask::ArLocalizationTask	(	ArRobot *	robot,
		ArRangeDevice *	laser,
		ArMapInterface *	ariaMap,
		bool	noReflectors = false
	)

Base constructor with all the necessary inputs.

Constructor.

Parameters:

	robot	Robot data structure.
	laser	Laser data structure.
	ariaMap	Aria map containing environment map.
	noReflectors	If true, ignore reflector objects in the map and don't try to include them in localization

ArLocalizationTask::~ArLocalizationTask

(

void

)

[virtual]

Base destructor.

Destructor.

---

Member Function Documentation

bool ArLocalizationTask::localizeRobotInMapInit	(	ArPose	given,
		int	numSamples,
		double	stdX,
		double	stdY,
		double	stdT,
		double	thresFactor,
		bool	warn = true,
		bool	setInitializedToFalse = true,
		bool	rayTraceFlag = true
	)

Localization function mainly for initialization of robot at given pose.

Perform an initial map localization. This function is meant to find a robot in the map without motion. The error parameters fed to the localization are temporary and will be reset to the old values after the localization.

This is mostly for internal use, or to do a localization after forcing a change in robot pose with alternative values for parameters given (used instead of values from ArConfig).

See also:

localizeRobotAtHomeBlocking()

localizeRobotAtHomeNonBlocking()

Parameters:

	given,:	Pose to start localization at.
	numSamples,:	No of samples to be used in the MCL algorithm.
	stdX,:	Standard deviation in the X coords in mm.
	stdY,:	Standard deviation in the Y coords in mm.
	stdT,:	Standard deviation in the Theta coords in degs.
	thresFactor,:	Factor deciding the threshold for a match. +ve(0-1). Generally must allow at least .1 (10%).
	warn,:	Flag to warning if loca score is too low.

Returns:

true if successful false if not.

Interpolating the pose at any given time.

bool ArLocalizationTask::localizeRobotInMapMoved	(	int	numSamples,
		double	distFactor,
		double	angFactor,
		double	thresFactor
	)

Function used to do the localization after motion (normally called automatically by the background task).

This is the actual function doing the Monte Carlo Markov Localization during most of the time once the localization is initialized and the robot is moving. This function calls the appropriate functions in the ArRobotPoseSamples to compute the most likely pose given a current robot pose and the sensor reading.

Interpolating the pose at any given time.

bool ArLocalizationTask::localizeRobotAtHomeBlocking	(	double	distSpread,
		double	angleSpread,
		double	probThreshold
	)			[inline]

Try initial localization at each home point at the map, and set the robot pose to the point with best score, using given parameter values instead of values previously configured.

See also:

localizeRobotAtHomeBlocking()

Examples:

arnlJustLocalization.cpp.

bool ArLocalizationTask::localizeRobotAtHomeBlocking	(	double	spreadX,
		double	spreadY,
		double	angleSpread,
		double	probThreshold
	)

instead of values previously configured

Attempt to localize the robot in the most likely home position: first it checks the robot's current pose and then checks all of the home positions in the map. The position with the highest score is then used. Call this function after the localization thread has been initialized but needs to be reset, or an initial localization (i.e. at program start up) is needed. This function blocks while the localization takes place, and returns after it either succeeds (Either localization at a home position succeeds, or all fail.)

Note:

Localization will fail if no sensor data has yet been obtained when this function is called (e.g. if called before or immediately after connecting to the robot and/or laser sensor).

Parameters:

	distSpread	Use this distance in mm to spread x and y in search of position instead of configured through ArConfig
	angleSpread	Use this number of degrees to spread in search of position instead of configuration's value
	locScoreThreshold	Lowest allowed score to consider a localization to have succeeded, overriding configuration's value.

See also:

localizeRobotAtHomeBlocking()

virtual bool ArLocalizationTask::localizeRobotAtHomeBlocking

(

)

[inline, virtual]

Try initial localization at each home point at the map, \ and set the robot pose to the point with best score.

Attempt to localize the robot in the most likely home position: first it checks the robot's current pose and then checks all of the home positions in the map. The position with the highest score is then used. Call this function after the localization thread has been initialized but needs to be reset, or an initial localization (i.e. at program start up) is needed. This function blocks while the localization takes place, and returns after it either succeeds (Either localization at a home position succeeds, or all fail.)

Note:

Localization will fail if no sensor data has yet been obtained when this function is called (e.g. if called before or immediately after connecting to the robot and/or laser sensor).

Reimplemented from ArBaseLocalizationTask.

bool ArLocalizationTask::localizeRobotAtHomeNonBlocking

(

void

)

Request that the task later localize the robot at a map home \ position, then return immediately.

Initiate background localization of the robot at the first home pose as defined in the Aria Map. Call this function after the localization thread has been initialized, and needs to be reset, or an initial localization (e.g. at program start) needs to be performed. This function moves the robot to the first home point in the map, and requests that the localization thread perform a new localization, then returns immediately The localization thread will then later attept to localize, starting at the home point.

Note:

Localization will fail if no sensor data has yet been obtained.

Returns:

false if there is no map, true in all other cases; use a callback or monitor the localization state to determine whether localization later fails or succeeds.

MPL this can't know the score yet... so it just has to set it and continue with life

ArPose ArLocalizationTask::getRobotHome

(

void

)

[virtual]

Gets the pose that robot was localized to.

Function to get stored home pose (used internally during reset etc.)

Returns:

"Home" pose determined by localizeRobotAtHomeBlocking() or localizeRobotAtHomeNonBlocking() (or (0,0,0) if these functions have not been called).

Reimplemented from ArBaseLocalizationTask.

void ArLocalizationTask::setForceUpdateParams	(	int	numSamples,
		double	xStd,
		double	yStd,
		double	tStd
	)

Sets the force update parameters.

Sets new localization parameters used after an pose update is forced by forceUpdatePose().

See also:

forceUpdatePose()

Parameters:

	numSamples,:	no of samples to be used.
	xStd,:	X axis std deviation in mm.
	yStd,:	Y axis std deviation in mm.
	tStd,:	Theta axis std deviation in degs.

void ArLocalizationTask::forceUpdatePose	(	ArPose	forcePose,
		bool	rayTrace = true
	)

Force an update in thread instead of waiting for distance-angle trigger.

Forces the thread to go from move localization to set localization with a new pose. Force a change in robot position, and a stationary relocalization at that point (instead of moving localization). This function is neccesary to cleanly resume localization after forcing a change in robot position (or at initial localization if neither localizeRobotAtHomeBlocking() or localizeRobotAtHomeNonBlocking() are used). You can use setForceUpdateParams() to explictly net new values used for parameters.

See also:

localizeRobotAtHomeBlocking()

localizeRobotAtHomeNonBlocking()

Parameters:

forcePose

New starting pose to be used for next localization

void ArLocalizationTask::addFailedLocalizationCB

(

ArFunctor1< int > *

functor

)

Adds a callback which will be called when loca fails.

After localization has failed these callbacks will be called. This function adds the a pointer to a function to this list.

Parameters:

functor

the functor to call.

void ArLocalizationTask::remFailedLocalizationCB

(

ArFunctor1< int > *

functor

)

Removes a callback.

Removes a FailedLocalizationCB, see addFailedLocalizationCB for details.

Parameters:

functor,:

The pointer to the function to be taken off.

void ArLocalizationTask::getForceUpdateParams	(	ArPose &	forcePose,
		int &	numSamples,
		double &	xStd,
		double &	yStd,
		double &	tStd
	)

Get the params to do the localization from the class.

Fills the 5 relevant parameters from the force update set.

ArPose ArLocalizationTask::getRobotMaxProbPose

(

void

)

Get the current computed best robot pose.

Gets the most probable pose of the robot as computed by the last localization.

Returns:

The best pose among the samples..

double ArLocalizationTask::getLocalizationScore

(

void

)

[virtual]

Gets the higher of MCL and Reflector based localization.

Returns the best of monte carlo localization and kalman filter based localization using reflectors.

Returns:

A measure of the goodness of the map seen by the sensor and the robots pose. (0-1)

Reimplemented from ArBaseLocalizationTask.

Examples:

arnlServer.cpp.

double ArLocalizationTask::getLocalizationThreshold

(

void

)

[virtual]

Gets the localization threshold.

Returns the localization threshold.

Returns:

threshold.

Reimplemented from ArBaseLocalizationTask.

double ArLocalizationTask::getMCLocalizationScore

(

void

)

Gets the localization score based on Monte Carlo only.

Returns the goodness of the last localization result. No of matched laser points in the environment to the total no of laser points.

Returns:

A measure of the goodness of the map seen by the sensor and the robots pose. (0-1)

double ArLocalizationTask::getRefLocalizationScore

(

void

)

Gets the localization score based on reflectors only.

Returns the localization score based on reflectors alone.

Returns:

A measure of the goodness of the map seen by the sensor and the robots pose. (0-1)

std::list< ArPose > ArLocalizationTask::getCurrentSamplePoses

(

void

)

[virtual]

Gets the pose samples for client.

Gets the Robot pose samples before sensor correction.

Returns:

list of poses.

Reimplemented from ArBaseLocalizationTask.

ArMatrix ArLocalizationTask::getQParams

(

void

)

Gets the bad reflector factor.

Gets the variance values that decay with time.

ArMapInterface * ArLocalizationTask::readMapFromFile

(

char *

mapName

)

Read the Map data from a map file.

Read the map given the file name.

Parameters:

mapName,:

Name of the map file.

Returns:

1 if successful else 0.

ArMapInterface * ArLocalizationTask::readAriaMap

(

ArMapInterface *

ariaMap

)

Read the Map data from ArMap.

Read the map given the Aria map.

Parameters:

ariaMap,:

The Aria map class ptr.

Returns:

1 if successful else 0.

bool ArLocalizationTask::loadParamFile

(

const char *

file

)

Load parameters from the given filename into ArConfig.

It Aria::getConfig()->parseFile(file, false, true), and also stores the ArConfig object returned by Aria::getConfig() as a (possibly new) ArConfig object for ArLocalizationTask to use in the future.

Returns:

true if the file could be loaded, false otherwise

bool ArLocalizationTask::saveParams

(

char *

filename

)

Saves all parameter values from ArConfig to the given file.

Returns:

true if the save works, false if error, or ArLocalizationTask has no ArConfig object (that object is set in the constructor or loadParamFile()).

bool ArLocalizationTask::fillHistogram	(	double *&	hist,
		double *&	cumSum,
		int &	numSamples
	)

Fill prob distribution histogram for debugging.

Fill histogram arrays. For display purposes during debugging.

Parameters:

	hist,:	Histogram array.
	cumSum,:	Cumulative sum array reference.
	numSamples,:	No of samples.

Returns:

true if successful.

bool ArLocalizationTask::scanToGlobalCoords	(	ArPose	robPose,
		std::vector< ArPose > &	xyLrf
	)

Convert the laser data to x and y coord array.

Converts the range points into X and Y coord arrays.

Parameters:

	robPose,:	Robots pose.
	xLrf,:	Pointer to the X coords.
	xLrf,:	Pointer to the Y coords.

Returns:

1 if successful else 0.

bool ArLocalizationTask::setLocaParams	(	double	xStd,
		double	yStd,
		double	tStd,
		double	kMmPerMm,
		double	kDegPerDeg,
		double	kDegPerMm,
		double	sensorBelief
	)

Set all the parameters for localization.

Sets all error parameters needed to do the MCL localization.

Parameters:

	xStd,:	Standard deviation in the X coords in mm.
	yStd,:	Standard deviation in the Y coords in mm.
	tStd,:	Standard deviation in the Theta coords in deg.
	kMmPerMm,:	Error factor for distance per distance traveled. mm/mm.
	kDegPerDeg,:	Error factor for angle drift per heading change. deg/deg.
	kDegPerMm,:	Error factor for angle drift per dist travelled. deg/mm.
	sensorBelief,:	Sensor Belief. (0-1)

Returns:

1 if successful else 0.

bool ArLocalizationTask::computeLastLocaMeanVar	(	ArPose &	mean,
		ArMatrix &	Var
	)

The compute the mean and var as soon as MCL finds a pose.

Compute the mean and variance from the MCL samples.

Parameters:

	mean,:	The computed mean pose.
	Var,:	The computed variance matrix.

Returns:

true if successful else false.

bool ArLocalizationTask::findMCLMeanVar	(	ArPose &	mean,
		ArMatrix &	Var
	)

Finds the mean and var of MCL after moving to present pose.

Finds the MCL mean and variance at the current pose.

Parameters:

	mean,:	The computed mean pose.
	Var,:	The computed variance matrix.

Returns:

true if successful else false.

bool ArLocalizationTask::findLocalizationMeanVar	(	ArPose &	mean,
		ArMatrix &	Var
	)			[virtual]

The actual mean var calculator for the virtual in the base class.

Virtual function which will pull the current state of the loca.

Parameters:

	mean,:	The computed mean pose.
	Var,:	The computed variance matrix.

Returns:

true if successful else false.

Reimplemented from ArBaseLocalizationTask.

void ArLocalizationTask::setRobotPose	(	ArPose	pose,
		ArPose	spread = ArPose(0, 0, 0),
		int	nSam = 0
	)			[virtual]

Used to set the robot pose usually at the start of localization.

This one with a spread around the set pose.

Reimplemented from ArBaseLocalizationTask.

void ArLocalizationTask::drawRangePoints	(	ArServerClient *	client,
		ArNetPacket *	packet
	)

Draws range data used for localization.

ArNetworking callback providing sample points to draw in a GUI for troubleshooting or debugging.

Examples:

arnlServer.cpp.

void ArLocalizationTask::drawReflectorRays	(	ArServerClient *	client,
		ArNetPacket *	packet
	)

Draw rays from the robot to the reflector.

Draws the rays to the landmarks.

Parameters:

	client,:	Stuff handed down from client prgram.
	packet,:	Stuff handed down from client program.

MPL changes so kathleen always gets packet

void ArLocalizationTask::drawSamplePoses	(	ArServerClient *	client,
		ArNetPacket *	packet
	)

Draws debugging objects if filled.

MPL changes so kathleen always gets packet

void ArLocalizationTask::drawSampleBounds	(	ArServerClient *	client,
		ArNetPacket *	packet
	)

Draws the sample bounds.

Draws the boundary around the samples.

Parameters:

	client,:	Stuff handed down from client program.
	packet,:	Stuff handed down from client program.

MPL changes so kathleen always gets packet

void ArLocalizationTask::drawKalmanVariance	(	ArServerClient *	client,
		ArNetPacket *	packet
	)

Draws the sample bounds.

Draws the Reflector Kalman mean pose and var position rectangle with angle cone.

void ArLocalizationTask::drawMCLVariance	(	ArServerClient *	client,
		ArNetPacket *	packet
	)

Draws the sample bounds.

Draws the MCL's mean pose and var position rectangle with angle cone.

void ArLocalizationTask::basicInitialization	(	ArRobot *	robot,
		ArRangeDevice *	laser
	)			[protected]

Basic initializer (internal use only).

Basic initializer to avoid duplication in the two constructors.

void * ArLocalizationTask::runThread

(

void *

ptr

)

[protected, virtual]

Function used to run the task as a thread.

The main loop for the background localization thread.

bool ArLocalizationTask::correctPoseFromSensor	(	double	increment,
		bool	rayTrace,
		bool	ignoreBadProb
	)			[protected]

Function which does the main eqn for the actual MCL localization.

Correct the sample poses by using the data from the sensor. This is the second of the two important steps in Localization. Correlation window is only 1 pixel wide per ray.

Parameters:

	increment,:	Angle increment by which the laser readings differ.
	rayTrace,:	Flag which will enable ray tracing.
	ignoreBadProb,:	Will return tru even when maxProb is low.

Returns:

true if successful else 0.

bool ArLocalizationTask::initializeSamples

(

int

ns

)

[protected]

Initialize the MCL samples.

Initialize the robot samples for MCL to start.

Parameters:

ns,:

Number of samples.

Returns:

1 if successful else 0.

bool ArLocalizationTask::scanLaserIntoArray

(

void

)

[protected]

Get the laser buffer into data structure.

Moves the laser range buffer into the robot and laser class.

Returns:

1 if successful else 0.

ArOccGrid * ArLocalizationTask::initializeOccGrid	(	double	res,
		ArMapInterface *	ariaMap,
		bool	lockMap = true
	)			[protected]

Set resolution of the occupancy grid and fill it.

Fills the occupancy grid with data from the map.

Parameters:

	res,:	The new resolution in mm.
	ariaMap,:	Aria map ptr.
	lockMap,:	Flag to enable locking of the map.

Returns:

1 if successful else 0.

void ArLocalizationTask::killBadSamples

(

double

occThreshold

)

[protected]

Legalize samples to eliminate illegal location such as on obstacles.

Kill all bad samples by setting their probs to zero.

Parameters:

occThreshold,:

Value at which the occupancy grid is considered an obstacle.

void ArLocalizationTask::saveSamples

(

bool

saveFile = false

)

[protected]

Save samples for backup.

Backup samples before they are resampled. Resampling usually flattens the probability distribution to a single location.

Parameters:

saveFile,:

Flag to make it save it as a file for debugging.

int ArLocalizationTask::findBestPoses	(	ArRobotPoseSamples *	mrsp,
		double	factor
	)			[protected]

Find the best poses by looking at prob distribution peaks.

Finds the most probable poses from the results of the MCL by looking at the samples and their characteristics. Uses the fact that after resampling, the probabilties must be non zero for only one or a few poses.

Parameters:

	mrsp,:	Pointer to the robot samples.
	factor,:	Scale factor from the peak prob to consider.

Returns:

no of peaks found if successful else 0.

bool ArLocalizationTask::findSamplesStatistics	(	double &	xMean,
		double &	yMean,
		double &	thMean,
		double &	xStd,
		double &	yStd,
		double &	tStd
	)			[protected]

Compute statistics of samples after localization.

Compute the statistics from the samples.

Parameters:

	xMean,:	Reference to the mean X of the sample poses.
	yMean,:	Reference to the mean Y of the sample poses.
	thMean,:	Reference to the mean Theta of the sample poses.
	xStd,:	Reference to the computed std deviation on X.
	yStd,:	Reference to the computed std deviation on Y.
	tStd,:	Reference to the computed std deviation on Theta.

Returns:

true if successful else false.

bool ArLocalizationTask::findKalmanMeanVar	(	ArPose &	mean,
		ArMatrix &	Var
	)			[protected]

Return the state.

Return the current mean pose of the kalman and its variance.

Parameters:

	mean,:	The computed mean pose.
	Var,:	The computed variance matrix.

Returns:

true if successful else false.

bool ArLocalizationTask::fuseTwoDistributions	(	ArPose	m1,
		ArMatrix	V1,
		ArPose	m2,
		ArMatrix	V2,
		ArPose &	mean,
		ArMatrix &	Var
	)			[protected]

Fuses the stats of two uniform dists into one.

Combines two gaussian distributions into one.

Parameters:

	m1,:	Mean of first distribution.
	V1,:	Variance matrix of the first distribution.
	m2,:	Mean of second distribution.
	V2,:	Variance matrix of the second distribution.
	mean,:	Mean of the combine.
	Var,:	Variance matrix of the combine.

Returns:

true if successful else false.

void ArLocalizationTask::setMotionErrorParam	(	int	index,
		double	value
	)			[protected]

Set motion error params.

Sets all error parameters needed to do the MCL localization. [Kr, Kt, Kd] treated as a vector.

Parameters:

	index,:	Index of error param.
	value,:	Value to set it to.

Returns:

1 if successful else 0.

double ArLocalizationTask::getMotionErrorParam

(

int

index

)

[protected]

Get motion error params.

Gets all error parameters needed to do the MCL localization. [Kr, Kt, Kd] treated as a vector.

Parameters:

index,:

Index of error param.

Returns:

value of the error param.

void ArLocalizationTask::robotCallBack

(

void

)

[protected]

The sensor interpretation callback. Called every 100msec.

Updates the true current robot pose. Called every 100msec from ArRobot.

bool ArLocalizationTask::configureLaser

(

void

)

[protected]

Needed if the laser does not connect first.

A function to setup the laser if it wasnt connected first time around.

Returns:

true if successful else false.

bool ArLocalizationTask::reconfigureLocalization

(

void

)

[protected]

Needed if the params are changed or loaded again.

A function to setup the laser if it wasnt connected first time around.

Returns:

true if successful else false.

void ArLocalizationTask::failedLocalization

(

int

times

)

[protected]

Actual thing that gets called when localization failed.

Things to do when localization fails.

int ArLocalizationTask::dynamicallyAdjustNumSamples	(	double	dr,
		double	dt
	)			[protected]

Change the sample size to reflect localization score.

Dynamically reduce the number of samples when the localization score is good and increase it when the score is lower.

Mostly for internal use only.

Parameters:

	dr,:	Change in distance.
	dt,:	Change in angle.

Returns:

: No of samples.

void ArLocalizationTask::mapChanged

(

void

)

[protected]

Function for the things to do if map changes.

Things to do if map changes.

bool ArLocalizationTask::findPoseFromLandmarks	(	const std::vector< ArPose > &	global,
		const std::vector< ArPose > &	local,
		ArPose &	rPose,
		ArMatrix &	Var
	)			[protected]

Solve the triangulation eqn.

Solve the pose equation from the landmark triangulation. Unlike the earlier method, this uses no matrix inversion and hence is a lot more stable and accurate.

Parameters:

	global	The global coords of the landmarks.
	local	The local coords of the landmarks.
	The	best pose which minimizes the error the most.

int ArLocalizationTask::getReflectionCoords	(	ArPose &	poseTaken,
		ArPose &	rPose,
		std::vector< ArPose > &	gList,
		std::vector< ArPose > &	lList
	)			[protected]

Find the reflectors in local and global coords.

Function to get all the reflections in global and local coords at the current robot pose.

Parameters:

	poseTaken,:	Pose at which laser taken.
	rPose,:	Pose of the robot now.
	gList,:	Reference to the list of reflections seen, in global coords.
	lList,:	Reference to the list of reflections seen, in local coords.

Returns:

No of reflectors.

int ArLocalizationTask::getReflectorCenterCoords	(	ArPose &	poseTaken,
		ArPose &	rPose,
		std::vector< ArPose > &	gList,
		std::vector< ArPose > &	lList
	)			[protected]

Find the reflector centers in local and global coords.

Function to get the reflectors centers in global and local coords if any.

Parameters:

	poseTaken,:	Pose at which laser taken.
	rPose,:	Pose of the robot now.
	gList,:	Reference to the list of reflectors seen, in global coords.
	lList,:	Reference to the list of reflectors seen, in local coords.

Returns:

no of reflectors.

bool ArLocalizationTask::fuseSensorWithOdometry

(

bool

failedLocalizationFlag

)

[protected]

Fuse all the sensor information.

Fuse the odometry into the current observations about the reflectors. using kalman filtering.

Parameters:

failedLocalizationFlag,:

Tells whether MCL localization failed.

Returns:

False only if it is not initialized or if timing messed up.

Interpolating the light pose at any given time.

void ArLocalizationTask::setFlagsAndCallbacksOnFail

(

int

ntimes

)

[protected]

Set the flags and call the right callbacks when loca fails ntimes.

One stop for all failed localization in runThread.

---