Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
00001 #ifndef _AFS_PARTICLE_H_ 00002 #define _AFS_PARTICLE_H_ 00003 00004 #ifdef __cplusplus 00005 extern "C" { 00006 #endif 00007 00008 /* 00009 * Defines the particle data structures used for AIBO FastSLAM 00010 */ 00011 00012 #include "Configuration.h" 00013 00014 /** This structure contains a robot pose. Pretty simple. */ 00015 typedef struct _afsPose { 00016 double x; //!< x value 00017 double y; //!< y value 00018 double theta; //!< theta value 00019 } afsPose; 00020 00021 /** This structure is used within afsLandmarkLoc during the initialization 00022 * phase, when it is necessary to triangulate the location of the 00023 * landmark before using regular FastSLAM techniques. */ 00024 typedef struct _afsLastObservation { 00025 int empty; //!< TSS_TODO 00026 double x; //!< x value 00027 double y; //!< y value 00028 double theta; //!< theta value 00029 } afsLastObservation; 00030 00031 /** This structure is used within afsParticle structures to encode 00032 * information about landmark location */ 00033 typedef struct _afsLandmarkLoc { 00034 /** This indicates whether the landmark has been found yet (PRIMED if so, 00035 * PRIMING if not). */ 00036 enum { PRIMING, PRIMED } state; 00037 00038 /** This element is used when the inital location of the landmark must 00039 * be triangulated. */ 00040 afsLastObservation priming; 00041 00042 /** Mean of Gaussian landmark location estimate */ 00043 struct { 00044 double x; //!< x value 00045 double y; //!< y value 00046 } mean; 00047 00048 /** Variance of Gaussian landmark location estimate */ 00049 struct { 00050 double x; //!< x value 00051 double xy; //!< TSS_TODO 00052 double y; //!< y value 00053 } variance; 00054 00055 } afsLandmarkLoc; 00056 00057 /* This is cheating with the namespaces to make C++ work like C code. */ 00058 #ifdef __cplusplus 00059 #define PRIMING afsLandmarkLoc::PRIMING 00060 #define PRIMED afsLandmarkLoc::PRIMED 00061 #endif 00062 00063 /** This structure contains data for each particle used by the 00064 * particle filter. Since we have a fixed number of landmarks, we 00065 * simply fix the number of afsLandmarkLoc structures inside. */ 00066 typedef struct _afsParticle { 00067 /** The robot pose represented by this particle */ 00068 afsPose pose; 00069 00070 /** The estimated poses of all landmarks in the map */ 00071 afsLandmarkLoc landmarks[AFS_NUM_LANDMARKS]; 00072 00073 //@{ 00074 /** These two variables reflect the weight assigned to this particle 00075 * from the last sensor measurement. While the particle is still 00076 * triangulating, there is no weight calculated and gotweight will 00077 * be false. Otherwise, gotweight is true and the new weight is 00078 * placed into weight */ 00079 int gotweight; 00080 double weight; 00081 //@} 00082 00083 } afsParticle; 00084 00085 /** Initialize a new afsParticle */ 00086 void afsParticleInit(afsParticle *p); 00087 /** Copy an afsParticle */ 00088 void afsParticleCopy(afsParticle *old, afsParticle *neu); 00089 00090 #ifdef __cplusplus 00091 } 00092 #endif 00093 #endif
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2 |