Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
#include "afsParticle.h"
#include "afsUtility.h"
#include "afsFindBestPose.h"
#include "Configuration.h"
#include <math.h>
#include <stdio.h>
Include dependency graph for afsFindBestPose.cc:
Go to the source code of this file.
Functions | |
afsPose | afsFindBestPose (afsParticle *p, afsPose pose, afsXY *real_landmarks) |
void | afsApplyBestPose (afsParticle *p, afsParticle *new_p, afsPose best_pose) |
double | afsParticleError (afsParticle *p, afsPose pose, afsXY *real_landmarks) |
void | afsGuessState (afsParticle *P, afsPose *g_pose, afsXY *g_landmarks) |
|
This routine takes the location and orientation in the particle p and places the same information in new_p after applying the translation and rotation recommended in best_pose (which you can get from afsFindBestPose. You can make p and new_p point to the same thing safely, but why would you want to? Definition at line 117 of file afsFindBestPose.cc. References AFS_NUM_LANDMARKS, find_dtheta(), _afsParticle::landmarks, _afsLandmarkLoc::mean, _afsParticle::pose, _afsLandmarkLoc::state, _afsPose::theta, _afsPose::x, and _afsPose::y. |
|
This routine determines the best orientation of the particle p given the supplied robot pose and landmark positions. The latter are supplied in an array pointed to by the last argument (note: the landmark ID numbers will be used to index the array. Definition at line 23 of file afsFindBestPose.cc. References AFS_NUM_LANDMARKS, find_dtheta(), _afsParticle::landmarks, _afsLandmarkLoc::mean, _afsParticle::pose, _afsLandmarkLoc::state, _afsPose::theta, _afsXY::x, _afsPose::x, _afsXY::y, and _afsPose::y. |
|
This routine makes an estimated map of landmarks in the environment and comes up with an estimated robot pose for situations where ground truth is not known. It does all of this by averaging. The first argument is a collection of all the particles in the filter. g in the arguments stands for "guess". Definition at line 193 of file afsFindBestPose.cc. References AFS_NUM_LANDMARKS, AFS_NUM_PARTICLES, find_dtheta(), _afsParticle::landmarks, _afsLandmarkLoc::mean, _afsParticle::pose, _afsLandmarkLoc::state, _afsPose::theta, _afsPose::x, _afsLandmarkLoc::x, _afsXY::x, _afsPose::y, _afsLandmarkLoc::y, and _afsXY::y. |
|
This routine finds the error value for a particle given real map information. The function we use to calculate the error here is the same one we minimized in afsFindBestPose, only there we were just concerned with its derivative. NOTE: The more unprimed landmarks a particle has, the lower its error is likely to be. This does not affect the performance of the algorithm Definition at line 161 of file afsFindBestPose.cc. References AFS_NUM_LANDMARKS, _afsParticle::landmarks, _afsLandmarkLoc::mean, _afsParticle::pose, _afsLandmarkLoc::state, _afsXY::x, _afsPose::x, and _afsPose::y. |
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:33 2003 by Doxygen 1.3.2 |