ArSonarLocalizationTask Class Reference

#include <ArSonarLocalizationTask.h>

Inheritance diagram for ArSonarLocalizationTask:

ArBaseLocalizationTask 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

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.

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::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 (  )  [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::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.

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.

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.

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

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

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.

Examples:
sonarnlServer.cpp.

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.

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.

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.

bool ArSonarLocalizationTask::saveParams ( char *  filename  ) 

Saves all default params to the param file.

Saves the params.

Returns:
true if the save works.

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

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.

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.

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.

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::drawRangePoints ( ArServerClient *  client,
ArNetPacket *  packet 
)

Draws range data used for localization.

Draws the stuff.

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

Draws the closest map lines to robot.

Draws the closest lines.

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

Draws the sonar rays.

Draws the closest lines.

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.

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

Draws intersection to closest lines.

Draws the intersection points.

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

Draws the histogram.

Draws the histogram

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

Draws the sample bounds.

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

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

Basic initializer to avoid duplication in the two constructors.

Parameters:
robot,: Robot data structure.
laser,: Laser data structure.

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

One stop for all failed localization in runThread.

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

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.

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.

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.

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.

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.

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.

void ArSonarLocalizationTask::robotCallBack ( void   )  [protected]

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

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

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

Returns:
true if successful else false.

void ArSonarLocalizationTask::setupLocalizationParams ( void   )  [protected]

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

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

Things to do when localization fails.

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

Things to do if map changes.

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.

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

Gets the closest lines.

Returns:
value.


Generated on Thu Apr 23 10:19:39 2009 for SONARNL by  doxygen 1.5.1