Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

afsFindBestPose.h

Go to the documentation of this file.
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