Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
00001 /* 00002 * Main interface to the AIBO FastSLAM. 00003 */ 00004 00005 #ifndef _AFS_MAIN_H_ 00006 #define _AFS_MAIN_H_ 00007 00008 #include "afsParticle.h" 00009 00010 #ifdef __cplusplus 00011 extern "C" { 00012 #endif 00013 00014 /* Initialize everything */ 00015 void afsInit(); 00016 00017 /* Manually set a landmark location estimate. landmark is the landmark 00018 * to set, x and y are the location, and covariance is the spherical 00019 * covariance to set about the landmark. You should probably set it to 00020 * something small, like 500. */ 00021 void afsSetLandmark(int landmark, double x, double y, double covariance); 00022 00023 /* Distribute the particle robot location estimates uniformly throughout 00024 * the specified bounding box. Recommended for use immediately after 00025 * afsSetLandmark is used to set all landmark locations and before any 00026 * measurement updates has taken place. The tighter the bounding box, 00027 * the better the performance. This function with the prior function 00028 * essentially reduces FastSLAM to Monte Carlo Localization. */ 00029 /* lx = left x, ty = top y, rx = right x, by = bottom y */ 00030 void afsDistribute(double lx, double ty, double rx, double by); 00031 00032 /* Report a movement */ 00033 void afsMotion(double dx, double dy, double da, unsigned int time); 00034 00035 /* Report a sensor measurement. This routine will not resample the particles! 00036 * Use afsResample() for that. */ 00037 void afsMeasurement(int landmark, double theta); 00038 00039 /* Resamples the particles. */ 00040 void afsResample(); 00041 00042 /* Get the particle that is most representative of where we are and what the 00043 * world looks like. This particle may be synthesized or it may be taken 00044 * directly out of the particle list. Don't modify it, please. The error for 00045 * this particle will be put into the variable pointed to by error, so long 00046 * as the address isn't NULL. */ 00047 afsParticle *afsWhatsUp(double *error); 00048 00049 /* Get an indication of how certain the FastSLAM algorithm is about its 00050 * location. The lower the number, the better. Particles with unprimed 00051 * landmarks will tend to lower the error, but with more priming, things 00052 * become more reasonable */ 00053 double afsCertainty(); 00054 00055 /* Get direct access to the current particle set. Not approved for general 00056 * use--this is only used by the WorldModel2 serializer to send particle 00057 * filter information data for display. Software that makes (illicit) use 00058 * of this routine should not count on the pointer it returns remaining 00059 * accurate after a resampling. */ 00060 afsParticle *afsInvadeFSData(); 00061 00062 #ifdef __cplusplus 00063 } 00064 #endif 00065 #endif 00066
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2 |