Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
00001 #ifndef _AFS_FIND_BEST_POSE_H_ 00002 #define _AFS_FIND_BEST_POSE_H_ 00003 00004 /*! @file 00005 * 00006 * Finds the proper orientation of the landmark and position information 00007 * within a particle based on the known locations of landmarks. 00008 */ 00009 00010 #include "afsParticle.h" 00011 00012 #ifdef __cplusplus 00013 extern "C" { 00014 #endif 00015 00016 /*! A structure that contains the relative radial position of a landmark 00017 * (the robot is at the origin). */ 00018 typedef struct _afsRRP { 00019 double theta; //!< angle 00020 double distance; //!< distance 00021 } afsRRP; 00022 00023 /*! A structure that simply contains the x,y coordinates of landmarks. */ 00024 typedef struct _afsXY { 00025 double x; //!< distance along x 00026 double y; //!< distance along y 00027 } afsXY; 00028 00029 /*! This routine determines the best orientation of the particle p given the 00030 * supplied robot pose and landmark positions. The latter are supplied in an 00031 * array pointed to by the last argument (note: the landmark ID numbers will 00032 * be used to index the array. */ 00033 afsPose afsFindBestPose(afsParticle *p, afsPose pose, afsXY *real_landmarks); 00034 00035 /*! This routine takes the location and orientation in the particle p and 00036 * places the same information in new_p after applying the translation and 00037 * rotation recommended in best_pose (which you can get from 00038 * afsFindBestPose. You can make p and new_p point to the same thing 00039 * safely, but why would you want to? */ 00040 void afsApplyBestPose(afsParticle *p, afsParticle *new_p, afsPose best_pose); 00041 00042 /*! This routine finds the error value for a particle given real map 00043 * information. The function we use to calculate the error here is the 00044 * same one we minimized in afsFindBestPose, only there we were just 00045 * concerned with its derivative. 00046 * NOTE: The more unprimed landmarks a particle has, the lower its error is 00047 * likely to be. This does not affect the performance of the algorithm */ 00048 double afsParticleError(afsParticle *p, afsPose pose, afsXY *real_landmarks); 00049 00050 /*! This routine makes an estimated map of landmarks in the environment and 00051 * comes up with an estimated robot pose for situations where ground truth 00052 * is not known. It does all of this by averaging. The first argument is 00053 * a collection of all the particles in the filter. g in the arguments 00054 * stands for "guess". */ 00055 void afsGuessState(afsParticle *P, afsPose *g_pose, afsXY *g_landmarks); 00056 00057 #ifdef __cplusplus 00058 } 00059 #endif 00060 #endif
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2 |