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

Test.c

Go to the documentation of this file.
00001 /* The Aibo FastSLAM test suite. */
00002 
00003 #ifdef __cplusplus
00004 extern "C" {
00005 #endif
00006 
00007 #include "afsMain.h"
00008 #include "afsParticle.h"
00009 #include "afsMotionResample.h"
00010 #include "afsFindBestPose.h"
00011 #include "afsUtility.h"
00012 #include "Configuration.h"
00013 
00014 #include <stdio.h>
00015 #include <stdlib.h>
00016 #include <string.h>
00017 #include <math.h>
00018 #include <time.h>
00019 
00020 extern afsParticle *Particles;
00021 
00022 afsXY landmarks[AFS_NUM_LANDMARKS];
00023 
00024 /* JUST ONE ENORMOUS MAIN FUNCTION. Hooray!!! */
00025 int main(int argc, char **argv)
00026 {
00027   int i, iteration;
00028   double theta, dtheta, dist_x, dist_y;
00029   afsParticle p; /* the robot location */
00030   afsParticle *q; /* where we think we are */
00031   FILE *pfh, *lfh, *mfh, *sfh, *efh;
00032 
00033   srand48(time(NULL));  /* initialize random number generators */
00034   srand(time(NULL));
00035 
00036   /* open matlab file for errors */
00037   efh = fopen("err.mat", "w");
00038   if(!efh) {
00039     fputs("couldn't open err.mat\n", stderr);
00040     return -1;
00041   }
00042 
00043   /* open matlab file for particles */
00044   pfh = fopen("particle.m", "w");
00045   if(!pfh) {
00046     fputs("couldn't open particle.m\n", stderr);
00047     return -1;
00048   }
00049 
00050   /* open matlab file for landmark estimates and ellipses */
00051   lfh = fopen("estimate.m", "w");
00052   if(!lfh) {
00053     fputs("couldn't open estimate.m\n", stderr);
00054     return -1;
00055   }
00056 
00057   /* open matlab file for measurements */
00058   mfh = fopen("measurement.m", "w");
00059   if(!mfh) {
00060     fputs("couldn't open measurement.m\n", stderr);
00061     return -1;
00062   }
00063 
00064   /* print headers for these three files */
00065   fputs("function particle(i)\nhold on\nswitch i\n", pfh);
00066   fputs("function estimate(i)\nhold on\nswitch i\n", lfh);
00067   fputs("function measurement(i)\nhold on\nswitch i\n", mfh);
00068 
00069 
00070 
00071   /* Randomly distribute landmarks over an 8-meter square */
00072   for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00073     landmarks[i].x = (2*drand48() - 1) * 4000.0; /* we measure in mm */
00074     landmarks[i].y = (2*drand48() - 1) * 4000.0;
00075   }
00076 
00077   /* open matlab file for the basic scene */
00078   sfh = fopen("scene.m", "w");
00079   if(!sfh) {
00080     fputs("couldn't open scene.m\n", stderr);
00081     return -1;
00082   }
00083 
00084   /* Print landmarks out for Matlab */
00085   fputs("hold off\n", sfh);
00086   fprintf(sfh, "plot(%f,%f,'k*')\n", landmarks[0].x, landmarks[0].y);
00087   fputs("hold on\n", sfh);
00088   for(i=1; i<AFS_NUM_LANDMARKS; ++i)
00089     fprintf(sfh, "plot(%f,%f,'k*')\n", landmarks[i].x, landmarks[i].y);
00090 
00091   fputs("axis([-6000 6000 -6000 6000])\n", sfh);
00092   fclose(sfh);
00093 
00094 
00095 
00096   /* ignition! */
00097   afsInit();
00098 
00099   /* The real robot starts at 0,0,0. */
00100   afsParticleInit(&p);
00101 
00102   /* See if the user wanted to test landmark location initialization
00103    * with afsSetLandmark and distributed start with afsDistribute.
00104    * We'll call this "Monte Carlo Localization mode" or "mclmode" for
00105    * short */
00106   if((argc > 1) && !strcmp(argv[1], "mclmode")) {
00107     puts("Operating in Monte Carlo Localization mode");
00108     /* preset landmark guesses */
00109     for(i=0; i<AFS_NUM_LANDMARKS; ++i)
00110       afsSetLandmark(i, landmarks[i].x, landmarks[i].y, 500);
00111     /* distribute particle guesses all over */
00112     afsDistribute(-6000, 6000, 6000, -6000);
00113     /* oh, and make our location tough to guess, too--not 0,0 at first. */
00114     p.pose.x = (2*drand48() - 1) * 4000.0;
00115     p.pose.y = (2*drand48() - 1) * 4000.0;
00116   }
00117 
00118   /* now have the robot run around in circles forever, and print out the
00119    * global certainty value at each step */
00120   for(iteration=0; iteration<50; ++iteration) {
00121     afsParticle work_p; /* handy scratchpad particle */
00122     afsPose work_pose;  /* handy scratchpad pose */
00123 
00124     fprintf(pfh, "case %d\n", iteration);
00125     fprintf(lfh, "case %d\n", iteration);
00126     fprintf(mfh, "case %d\n", iteration);
00127 
00128     /* First we move ourselves. We try to hover around the point 1,1, but
00129      * we can only make very shallow turns. */
00130     {
00131       double goal_theta = atan2(1 - p.pose.y, 1 - p.pose.x);
00132       /* make robot relative */
00133       goal_theta -= p.pose.theta;
00134       /* constrain to [0, 2pi) */
00135       while(goal_theta < 0) goal_theta += 2*M_PI;
00136       while(goal_theta >= 2*M_PI) goal_theta -= 2*M_PI;
00137 
00138       if(goal_theta < M_PI) dtheta = 0.03;
00139       else dtheta = -0.03;
00140     }
00141 
00142     /* move the 'real' particle */
00143     afsMotionResample(&p, 70, 0, dtheta, 20000);
00144     /* Now we move the particles. */
00145     afsMotion(70, 0, dtheta, 20000);
00146 
00147     /* Now we take sensor measurements. Some of them we'll miss this time
00148      * around. */
00149     for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00150       if(drand48() < 0.4) continue;
00151 
00152       /* compute bearing to landmark */
00153       dist_x = landmarks[i].x - p.pose.x;
00154       dist_y = landmarks[i].y - p.pose.y;
00155       theta = atan2(dist_y, dist_x);
00156       /* add measurement error */
00157       theta += normRand()*AFS_MEASURE_VARIANCE;
00158       /* show measurement in matlab */
00159       fprintf(mfh, "plot_line(%f,%f,%f,%f)\n", p.pose.x, p.pose.y,
00160       //sqrt(dist_x*dist_x + dist_y*dist_y), theta);
00161       1200.0, theta);
00162       /* make bearing relative to robot pose */
00163       theta = find_dtheta(p.pose.theta, theta);
00164 
00165       /* register landmark observation */
00166       afsMeasurement(i, theta);
00167       afsResample();
00168     }
00169 
00170     /* what have we found? */
00171     /* first, plot all particle xy locations in yellow. */
00172     for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00173       /* fit it to our map of the world */
00174       work_pose = afsFindBestPose(&Particles[i], p.pose, landmarks);
00175       work_p = Particles[i];
00176       afsApplyBestPose(&Particles[i], &work_p, work_pose);
00177 
00178       fprintf(pfh, "%% %f %f %f\n", work_pose.x, work_pose.y, work_pose.theta);
00179 
00180       fprintf(pfh, "plot(%f, %f, 'y+')\n", work_p.pose.x,
00181             work_p.pose.y);
00182     }
00183     /* now plot where we *should* be */
00184     fprintf(pfh, "plot(%f,%f,'r+')\n", p.pose.x, p.pose.y);
00185 
00186     /* now find and plot our best location guess */
00187     q = afsWhatsUp(NULL);
00188 
00189     work_pose = afsFindBestPose(q, p.pose, landmarks);
00190     afsApplyBestPose(q, &work_p, work_pose);
00191     //work_p = *q;
00192     fprintf(pfh, "plot(%f,%f,'g+')\n", work_p.pose.x, work_p.pose.y);
00193 
00194     /*
00195     for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00196       if(q->landmarks[i].state == PRIMING) continue;
00197 
00198       fprintf(fh, "plot(%f,%f,'c*')\n", q->landmarks[i].mean.x,
00199           q->landmarks[i].mean.y);
00200     }
00201     */
00202 
00203     for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00204       if(work_p.landmarks[i].state == PRIMING) continue;
00205 
00206       fprintf(lfh, "plot(%f,%f,'b*')\n", work_p.landmarks[i].mean.x,
00207           work_p.landmarks[i].mean.y);
00208       fprintf(lfh, "plot_ellipse([%f %f; %f %f], %f, %f)\n",
00209       work_p.landmarks[i].variance.x,
00210       work_p.landmarks[i].variance.xy,
00211       work_p.landmarks[i].variance.xy,
00212       work_p.landmarks[i].variance.y,
00213       work_p.landmarks[i].mean.x,
00214       work_p.landmarks[i].mean.y);
00215     }
00216 
00217     {
00218       double error = afsCertainty();
00219       fprintf(stderr, "Iter. %d Certainty: %f\n", iteration, error);
00220       fprintf(efh, "%f\n", error);
00221     }
00222   }
00223 
00224 
00225   /* close up MATLAB files */
00226   fputs("end\naxis([-6000 6000 -6000 6000])\n", pfh);
00227   fputs("end\naxis([-6000 6000 -6000 6000])\n", lfh);
00228   fputs("end\naxis([-6000 6000 -6000 6000])\n", mfh);
00229 
00230   fclose(efh);
00231   fclose(pfh);
00232   fclose(lfh);
00233   fclose(mfh);
00234 
00235   return 0;
00236 }
00237 
00238 #ifdef __cplusplus
00239 }
00240 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2