ArSonarLocalizationTask Class Reference

#include <ArSonarLocalizationTask.h>

Inheritance diagram for ArSonarLocalizationTask:

List of all members.

---

Detailed Description

Task that performs localization of the robot with sonar range sensors in a seperate asynchronous thread.

The sonar localization task can be used to localize a robot in a given line map using the ring of sonar rangefinders.

ARNL's sonar localization uses Monte Carlo Localization algorithm to accurately localize the robot in the given line map using its sonar 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 ArSonarLocalizationTask class needs an instantiated ArRobot, ArSonarDevice and a line map of the robot's environment in an ArMap object (or loaded from file). The output of the localization will be reflected directly in the pose of the ArRobot. (unless explicitly set to not do so)

ArSonarLocalizationTask automatically creates and runs its background thread when constructed.

Examples:

sonarnlJustLocalization.cpp, and sonarnlServer.cpp.

Public Member Functions
	ArSonarLocalizationTask (ArRobot *robot, ArSonarDevice *sonar, char *mapName)
	Base constructor with all the necessary inputs.
	ArSonarLocalizationTask (ArRobot *robot, ArSonarDevice *sonar, ArMapInterface *ariaMap)
	Base constructor with all the necessary inputs.
virtual	~ArSonarLocalizationTask (void)
	Base destructor.
bool	localizeRobotInMapInit (ArPose given, int numSamples, double stdX, double stdY, double stdT, double thresFactor, bool warn=true, bool setInitializedToFalse=true)
	Localization function mainly for initialization of robot at a given pose.
bool	localizeRobotInMapMoved (int numSamples, double distFactor, double angFactor, double thresFactor)
	Function used to do the localization after motion (normally automatic).
bool	localizeRobotAtHomeBlocking (double distSpread, double angleSpread, double probThreshold)
	Try initial localization at each home point in the map, 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)
	Try initial localization at each home point in the map, set the robot pose to the point with best score, using given parameter values instead of values previously configured.
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 home position, and 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)
	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.
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 an ArMap object.
bool	loadParamFile (const char *file)
	load param file when Aria config sets it off.
bool	saveParams (char *filename)
	Saves all default params to the param file.
Modifiers for configuration values.
Normally thes evalues are set via ArConfig (e.g. from a loaded 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 parameter.
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	setCurrentNumSamples (int n)
	Set the variable number of samples (usually adjusted automatically).
void	setNumSamplesAngleFactor (double f)
	Sets the rotation factor for adjusting no of samples with score.
void	setPassThreshold (double f)
	Sets the pass threshold for fraction of sonar 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 f)
	Sets the verbose flag value. (for debugging).
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	setSonarMaxRange (double f)
	Sets the sonar range.
Accessors for confiuration values.
These values are normally set via ArConfig (the "Localization" section) by loading a file or other means, or by manually 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.
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 score for the localization as fraction of matched sonar pts.
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	getPeakFactor (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
ArMapInterface *	getAriaMap (void)
int	getBufferSize (void)
	Scan Buffer size. (internal sonar scan buffer).
std::vector< ArPose >	getXYBuffer (void)
	Scan Buffer XY.
ArPose	getBufferPose (void)
	Scan Buffer pose taken. (internal sonar scan buffer).
std::list< ArPose >	getCurrentSamplePoses (void)
	Gets the pose samples for client.
bool	getRecoverOnFailedFlag (void)
	Gets the recoverOnFailed flag.
double	getNumSamplesAngleFactor (void)
	Gets the angle factor when rotating which multiplies the no of samples.
double	getSonarMaxRange (void)
	Gets the sonar range.
double	getSonarMinLineSize (void)
	Gets the min line size;.
bool	getAdjustNumSamplesFlag (void)
	Gets the flag which allows for changing numSamples with loca score.
int	getMinNumSamples (void)
	Gets the min no of samples if they can be adjusted.
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 sonar data to x and y coord array.
bool	setLocaParams (double xStd, double yStd, double tStd, double kMmPerMm, double kDegPerDeg, double kDegPerMm, double sensorBelief)
	Set all the parameters for localization.
void	setAdjustNumSamplesFlag (bool f)
	Sets the flag which changes numSamples with the localization score.
void	setMinNumSamples (int f)
	Sets the min no of samples if they are adjustable with loca score.
void	setLastLocaTimeToNow (void)
	Sets the last loca time to now.
ArTime	getLastLocaTime (void)
	Gets the last loca time to now.
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 bool	localizeRobotAtHomeBlocking (double distSpread, double angleSpread)
	Localize the robot at known homes.
virtual void	setLocalizationIdle (bool f)
	Set the localization idle.
ArNetworking callback methods
(used by server classes in ArServerClasses.cpp)
void	drawRangePoints (ArServerClient *client, ArNetPacket *packet)
	Draws range data used for localization.
void	drawClosestLines (ArServerClient *client, ArNetPacket *packet)
	Draws the closest map lines to robot.
void	drawBestRays (ArServerClient *client, ArNetPacket *packet)
	Draws the sonar rays.
void	drawSampleRays (ArServerClient *client, ArNetPacket *packet)
	Draws the sonar rays.
void	drawSonarRays (ArServerClient *client, ArNetPacket *packet)
	Draws the sonar rays.
void	drawIntersections (ArServerClient *client, ArNetPacket *packet)
	Draws intersection to closest lines.
void	drawHistogram (ArServerClient *client, ArNetPacket *packet)
	Draws the histogram.
void	drawBestPoses (ArServerClient *client, ArNetPacket *packet)
	Draws the histogram.
void	drawSamplePoses (ArServerClient *client, ArNetPacket *packet)
	Draws the samples.
void	drawMCLVariance (ArServerClient *client, ArNetPacket *packet)
	Draws the sample bounds.
Protected Member Functions
std::vector< ArPose >	makeMeanVarPoints (ArPose &mean, ArMatrix &Var)
void	basicInitialization (ArRobot *robot, ArSonarDevice *sonar)
void	setFlagsAndCallbacksOnFail (int ntimes)
virtual void *	runThread (void *ptr)
bool	correctPoseFromSensor (int sonarStart, int sonarEnd, double maxRange, std::vector< ArLineSegment > lines)
bool	initializeSamples (int numSamples)
void	killBadSamples (double obsThreshold)
void	saveSamples (bool saveFile=false)
int	findBestPoses (ArRobotPoseSamples *mrsp, double factor)
bool	findAllStatistics (double &xMean, double &yMean, double &thMean, double &xStd, double &yStd, double &tStd)
void	setMotionErrorParam (int index, double value)
double	getMotionErrorParam (int index)
void	robotCallBack (void)
bool	configureSonar (void)
bool	reconfigureLocalization (void)
void	setupLocalizationParams (void)
void	failedLocalization (int times)
int	dynamicallyAdjustNumSamples (double dr, double dt)
void	mapChanged (void)
std::vector< ArLineSegment >	findClosestLines (ArPose robotPose, double range)
std::vector< ArLineSegment >	getClosestLines (void)

---

Constructor & Destructor Documentation

ArSonarLocalizationTask::ArSonarLocalizationTask (  ArRobot *  robot, ArSonarDevice *  sonar, char *  mapname )

Base constructor with all the necessary inputs. Constructor. Parameters: robot:  Robot data structure. sonar:  Sonar data structure. mapname:  Map file containing environment map.

ArSonarLocalizationTask::ArSonarLocalizationTask (  ArRobot *  robot, ArSonarDevice *  sonar, ArMapInterface *  ariaMap )

Base constructor with all the necessary inputs. Constructor. Parameters: robot:  Robot data structure. sonar:  Sonar data structure. ariaMap:  Aria map containing environment map.

ArSonarLocalizationTask::~ArSonarLocalizationTask (  void   )  [virtual]

Base destructor. Destructor.

---

Member Function Documentation

void ArSonarLocalizationTask::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 ArSonarLocalizationTask::basicInitialization (  ArRobot *  robot, ArSonarDevice *  sonar )  [protected]

Basic initializer to avoid duplication in the two constructors. Parameters: robot:  Robot data structure. laser:  Laser data structure.

bool ArSonarLocalizationTask::configureSonar (  void   )  [protected]

A function to setup the sonar if it wasnt connected first time around. Returns: true if successful else false.

bool ArSonarLocalizationTask::correctPoseFromSensor (  int  sonarStart, int  sonarEnd, double  maxRange, std::vector< ArLineSegment >  lines )  [protected]

Correct the sample poses by using the data from the sensor. This is the second of the two important steps in sonar Localization. Sensor information is correlated with the lines in the sonar FOV. Parameters: sonarStart  Sonar start number. sonarEnd  Sonar end number. maxRange  Maximum range to work with. lines  The set of lines to work with Returns: true if successful else 0.

void ArSonarLocalizationTask::drawBestRays (  ArServerClient *  client, ArNetPacket *  packet )

Draws the sonar rays. Draws the closest lines.

void ArSonarLocalizationTask::drawClosestLines (  ArServerClient *  client, ArNetPacket *  packet )

Draws the closest map lines to robot. Draws the closest lines.

void ArSonarLocalizationTask::drawHistogram (  ArServerClient *  client, ArNetPacket *  packet )

Draws the histogram. Draws the histogram

void ArSonarLocalizationTask::drawIntersections (  ArServerClient *  client, ArNetPacket *  packet )

Draws intersection to closest lines. Draws the intersection points.

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

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

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

Draws range data used for localization. Draws the stuff.

void ArSonarLocalizationTask::drawSampleRays (  ArServerClient *  client, ArNetPacket *  packet )

Draws the sonar rays. Draws the closest lines.

void ArSonarLocalizationTask::drawSonarRays (  ArServerClient *  client, ArNetPacket *  packet )

Draws the sonar rays. Draws the closest lines.

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

Dynamically reduce the number of samples when the localization score is good and increase it when the score is lower. Parameters: dr:  Change in distance. dt:  Change in angle. Returns: : No of samples.

void ArSonarLocalizationTask::failedLocalization (  int  times  )  [protected]

Things to do when localization fails.

bool ArSonarLocalizationTask::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 ArSonarLocalizationTask::findAllStatistics (  double &  xMean, double &  yMean, double &  thMean, double &  xStd, double &  yStd, double &  tStd )  [protected]

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 succesful else false.

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

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.

std::vector< ArLineSegment > ArSonarLocalizationTask::findClosestLines (  ArPose  robotPose, double  range )  [protected]

Find the closest lines to a robot pose. Parameters: robotPose:  The current robot pose. range:  Only those within this distance. Returns: Vector of line segments.

bool ArSonarLocalizationTask::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.

bool ArSonarLocalizationTask::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.

void ArSonarLocalizationTask::forceUpdatePose (  ArPose  forcePose  )

Force an update in thread instead of waiting for distance-angle trigger. Forces the thread to go from move localization to set localization. This function is neccesary to cleanly enter the localization thread without messing things up. Parameters: forcePose:  Pose to be used to center localization about.

std::vector< ArLineSegment > ArSonarLocalizationTask::getClosestLines (  void   )  [protected]

Gets the closest lines. Returns: value.

std::list< ArPose > ArSonarLocalizationTask::getCurrentSamplePoses (  void   )  [virtual]

Gets the pose samples for client. Gets the Robot pose samples before sensor correction. Returns: list of poses. Reimplemented from ArBaseLocalizationTask.

void ArSonarLocalizationTask::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. Parameters: forcePose:  Pose to be used to center localization about. 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 mm.

double ArSonarLocalizationTask::getLocalizationScore (  void   )  [virtual]

Gets the score for the localization as fraction of matched sonar pts. 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) Reimplemented from ArBaseLocalizationTask.

double ArSonarLocalizationTask::getMotionErrorParam (  int  index  )  [protected]

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.

ArPose ArSonarLocalizationTask::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..

bool ArSonarLocalizationTask::initializeSamples (  int  ns  )  [protected]

Initialize the robot samples for MCL to start. Parameters: ns:  Number of samples. Returns: 1 if successful else 0.

void ArSonarLocalizationTask::killBadSamples (  double  occThreshold  )  [protected]

Kill all bad samples by setting their probs to zero. Parameters: occThreshold:  Value at which the occupancy grid is considered an obstacle.

bool ArSonarLocalizationTask::loadParamFile (  const char *  file  )

load param file when Aria config sets it off. Returns: true if the file could be loaded, false otherwise

bool ArSonarLocalizationTask::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 ArSonarLocalizationTask::localizeRobotAtHomeBlocking (  double  spreadX, double  spreadY, double  angleSpread, double  probThreshold )

Try initial localization at each home point in the map, set the robot pose to the point with best score, using given parameter values instead of values previously configured. This function is specifically 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 one with the highest score is where the robot is localized (the home is set to the first home or the home the robot is at if it is at a home). This is to be called when the localization thread has been initialized and needs to be reset. This function blocks while the localization takes place. Parameters: distSpread  the distance in mm to spread x and y in search of position angleSpread  the number of degrees to spread in search of position locScoreThreshold  the score for localization to say is good enough

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

Try initial localization at each home point in the map, set the robot pose to the point with best score, using given parameter values instead of values previously configured. See also: localizeRobotAtHomeBlocking()

bool ArSonarLocalizationTask::localizeRobotAtHomeNonBlocking (  void   )

Request that the task later localize the robot at a home position, and return immediately. This function is specifically to localize the robot in the first home pose as defined in the Aria Map. This is to be called when the localization thread has been initialized and needs to be reset. This function returns immediately. Returns: true if successful false if Aria map is NULL.

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

Localization function mainly for initialization of robot at a given pose. This is the first call to the main localization function. 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. 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.

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

Function used to do the localization after motion (normally automatic). 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. Parameters: numSamples:  No of samples to be used in the MCL algorithm. distFactor:  Scale factor for scaling the linear std deviation.(+ve). angFactor:  Scale factor for scaling the angle std deviation.(+ve). thresFactor:  Factor deciding the threshold for a match. +ve(0-1). Generally must allow at least .1 (10%). Returns: true if successful false if not.

std::vector< ArPose > ArSonarLocalizationTask::makeMeanVarPoints (  ArPose &  mean, ArMatrix &  Var )  [protected]

Makes the list of points needed to draw the variance and mean. The points will form a rectangle to frame the position and a cone to frame the angle. Parameters: mean:  Return holder for the computed mean. Var:  Return holder for the variance matrix. Returns: List of points when drawn as segments indicating mean and var.

void ArSonarLocalizationTask::mapChanged (  void   )  [protected]

Things to do if map changes.

ArMapInterface * ArSonarLocalizationTask::readAriaMap (  ArMapInterface *  ariaMap  )

Read the Map data from an ArMap object. Read the map given the Aria map. Parameters: ariaMap:  The Aria map class ptr. Returns: 1 if successful else 0.

ArMapInterface * ArSonarLocalizationTask::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.

bool ArSonarLocalizationTask::reconfigureLocalization (  void   )  [protected]

A function to setup the laser if it wasnt connected first time around. Returns: true if successful else false.

void ArSonarLocalizationTask::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 ArSonarLocalizationTask::robotCallBack (  void   )  [protected]

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

void * ArSonarLocalizationTask::runThread (  void *  ptr  )  [protected, virtual]

This is the function loop which is called when the localization is run as a thread. Parameters: ptr:  Some kind of pointer needed by ARIA. Returns: Some kind of pointer decided by ARIA.

bool ArSonarLocalizationTask::saveParams (  char *  filename  )

Saves all default params to the param file. Saves the params. Returns: true if the save works.

void ArSonarLocalizationTask::saveSamples (  bool  saveFile = false  )  [protected]

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.

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

Convert the sonar 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.

void ArSonarLocalizationTask::setFlagsAndCallbacksOnFail (  int  ntimes  )  [protected]

One stop for all failed localization in runThread.

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

Sets the force update parameters. Sets the force update params. This function is neccesary to cleanly enter the localization thread without messing things up. 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.

bool ArSonarLocalizationTask::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.

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

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.

void ArSonarLocalizationTask::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 ArSonarLocalizationTask::setupLocalizationParams (  void   )  [protected]

Sets all the default parameters for the localization and sets up the ArConfig struct for handling Aria configuration.

---