#include <ArLocalizationManager.h>
Inheritance diagram for ArLocalizationManager:
Public Member Functions | |
| ArLocalizationManager (ArRobot *robot, ArMapInterface *ariaMap) | |
| Base constructor with all the necessary inputs. | |
| virtual | ~ArLocalizationManager (void) |
| Destructor. | |
| bool | fuseTwoDistributions (ArPose m1, ArMatrix V1, double s1, double threshold1, ArPose m2, ArMatrix V2, double s2, double threshold2, ArPose &mean, ArMatrix &Var, double &s, double &threshold) |
| Fuse two distributions to make one mean pose and one variance matrix. | |
| bool | addLocalizationTask (ArBaseLocalizationTask *loca) |
| Adds a localization to be managed. | |
| bool | removeLocalizationTask (ArBaseLocalizationTask *loca) |
| Removes a localization from the managing. | |
| virtual bool | findLocalizationMeanVar (ArPose &mean, ArMatrix &Var) |
| The actual mean var calculator for the virtual in the base class. | |
| virtual void | setCorrectRobotFlag (bool a) |
| 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 | localizeRobotAtHomeBlocking (double distSpread, double angleSpread) |
| Localize robot at start or while initialization. | |
| virtual double | getLocalizationScore (void) |
| Returns the weighted average of the scores from the localizations in its list. | |
| virtual void | setLocalizationScore (double f) |
| Function to set the score. | |
| virtual double | getLocalizationThreshold (void) |
| Get the localization threshold. | |
| virtual void | setLocalizationThreshold (double t) |
| Set the localization threshold. | |
| virtual ArBaseLocalizationTask * | getLightLocalizationPtr (void) |
| Function to get the light loca pointer. | |
Protected Member Functions | |
| virtual void * | runThread (void *ptr) |
| Function used to run the task as a thread. | |
| bool | reconfigureLocalization (void) |
| Needed if the params are changed or loaded again. | |
| bool ArLocalizationManager::fuseTwoDistributions | ( | ArPose | m1, | |
| ArMatrix | V1, | |||
| double | s1, | |||
| double | threshold1, | |||
| ArPose | m2, | |||
| ArMatrix | V2, | |||
| double | s2, | |||
| double | threshold2, | |||
| ArPose & | mean, | |||
| ArMatrix & | Var, | |||
| double & | s, | |||
| double & | threshold | |||
| ) |
Fuse two distributions to make one mean pose and one variance matrix.
Combines two gaussian distributions into one. Also interpolates the score as the weighted average of the two dists.
| m1,: | Mean of first distribution. | |
| V1,: | Variance matrix of the first distribution. | |
| s1,: | Score of the first localization. | |
| m2,: | Mean of second distribution. | |
| V2,: | Variance matrix of the second distribution. | |
| s2,: | Score of the second localization. | |
| mean,: | Mean of the combine. | |
| Var,: | Variance matrix of the combine. | |
| s,: | Score of the combined localization. |
| bool ArLocalizationManager::addLocalizationTask | ( | ArBaseLocalizationTask * | loca | ) |
Adds a localization to be managed.
Adds a localization to be managed.
| The | localization task to be added. |
| bool ArLocalizationManager::removeLocalizationTask | ( | ArBaseLocalizationTask * | loca | ) |
Removes a localization from the managing.
Removes a localization to be managed.
| The | localization task to be added. |
| bool ArLocalizationManager::findLocalizationMeanVar | ( | ArPose & | mean, | |
| ArMatrix & | var | |||
| ) | [virtual] |
The actual mean var calculator for the virtual in the base class.
Return the last mean and variance from all the localization threads.
| mean | Reference to the pose to be filled on return. | |
| var | Reference to the matrix to be filled on return. |
Reimplemented from ArBaseLocalizationTask.
| void ArLocalizationManager::setCorrectRobotFlag | ( | bool | f | ) | [virtual] |
Sets the flag deciding whether to reflect localized pose onto the robot.
Sets the correct robot flag, this flag decides if the localization results will be poked into the robot.
| f,: | Value to be set to. |
Reimplemented from ArBaseLocalizationTask.
| void ArLocalizationManager::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.
Do the fusing.
Reimplemented from ArBaseLocalizationTask.
| bool ArLocalizationManager::localizeRobotAtHomeBlocking | ( | double | distSpread, | |
| double | angleSpread | |||
| ) | [virtual] |
Localize robot at start or while initialization.
Sets robot pose for localization init.
| pose,: | The pose to initialize to. | |
| spread,: | The spread about the mean pose. | |
| nSum,: | Unused. |
Do the fusing.
Reimplemented from ArBaseLocalizationTask.
| virtual double ArLocalizationManager::getLocalizationScore | ( | void | ) | [inline, virtual] |
Returns the weighted average of the scores from the localizations in its list.
Only the XY pose uncertainity is used for now compute the weight. The theta uncertainity is ignored.
Reimplemented from ArBaseLocalizationTask.
| virtual ArBaseLocalizationTask* ArLocalizationManager::getLightLocalizationPtr | ( | void | ) | [inline, virtual] |
Function to get the light loca pointer.
Do the fusing.
| void * ArLocalizationManager::runThread | ( | void * | ptr | ) | [protected, virtual] |
Function used to run the task as a thread.
The main loop for the background localization thread.
Do the fusing.
MPL commenting this out since it causes a deadlock and isn't needed
MPL commenting this out since it causes a deadlock and isn't needed
| bool ArLocalizationManager::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.
1.5.1