00001
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
00025 int main(int argc, char **argv)
00026 {
00027 int i, iteration;
00028 double theta, dtheta, dist_x, dist_y;
00029 afsParticle p;
00030 afsParticle *q;
00031 FILE *pfh, *lfh, *mfh, *sfh, *efh;
00032
00033 srand48(time(NULL));
00034 srand(time(NULL));
00035
00036
00037 efh = fopen("err.mat", "w");
00038 if(!efh) {
00039 fputs("couldn't open err.mat\n", stderr);
00040 return -1;
00041 }
00042
00043
00044 pfh = fopen("particle.m", "w");
00045 if(!pfh) {
00046 fputs("couldn't open particle.m\n", stderr);
00047 return -1;
00048 }
00049
00050
00051 lfh = fopen("estimate.m", "w");
00052 if(!lfh) {
00053 fputs("couldn't open estimate.m\n", stderr);
00054 return -1;
00055 }
00056
00057
00058 mfh = fopen("measurement.m", "w");
00059 if(!mfh) {
00060 fputs("couldn't open measurement.m\n", stderr);
00061 return -1;
00062 }
00063
00064
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
00072 for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00073 landmarks[i].x = (2*drand48() - 1) * 4000.0;
00074 landmarks[i].y = (2*drand48() - 1) * 4000.0;
00075 }
00076
00077
00078 sfh = fopen("scene.m", "w");
00079 if(!sfh) {
00080 fputs("couldn't open scene.m\n", stderr);
00081 return -1;
00082 }
00083
00084
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
00097 afsInit();
00098
00099
00100 afsParticleInit(&p);
00101
00102
00103
00104
00105
00106 if((argc > 1) && !strcmp(argv[1], "mclmode")) {
00107 puts("Operating in Monte Carlo Localization mode");
00108
00109 for(i=0; i<AFS_NUM_LANDMARKS; ++i)
00110 afsSetLandmark(i, landmarks[i].x, landmarks[i].y, 500);
00111
00112 afsDistribute(-6000, 6000, 6000, -6000);
00113
00114 p.pose.x = (2*drand48() - 1) * 4000.0;
00115 p.pose.y = (2*drand48() - 1) * 4000.0;
00116 }
00117
00118
00119
00120 for(iteration=0; iteration<50; ++iteration) {
00121 afsParticle work_p;
00122 afsPose work_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
00129
00130 {
00131 double goal_theta = atan2(1 - p.pose.y, 1 - p.pose.x);
00132
00133 goal_theta -= p.pose.theta;
00134
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
00143 afsMotionResample(&p, 70, 0, dtheta, 20000);
00144
00145 afsMotion(70, 0, dtheta, 20000);
00146
00147
00148
00149 for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00150 if(drand48() < 0.4) continue;
00151
00152
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
00157 theta += normRand()*AFS_MEASURE_VARIANCE;
00158
00159 fprintf(mfh, "plot_line(%f,%f,%f,%f)\n", p.pose.x, p.pose.y,
00160
00161 1200.0, theta);
00162
00163 theta = find_dtheta(p.pose.theta, theta);
00164
00165
00166 afsMeasurement(i, theta);
00167 afsResample();
00168 }
00169
00170
00171
00172 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00173
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
00184 fprintf(pfh, "plot(%f,%f,'r+')\n", p.pose.x, p.pose.y);
00185
00186
00187 q = afsWhatsUp(NULL);
00188
00189 work_pose = afsFindBestPose(q, p.pose, landmarks);
00190 afsApplyBestPose(q, &work_p, work_pose);
00191
00192 fprintf(pfh, "plot(%f,%f,'g+')\n", work_p.pose.x, work_p.pose.y);
00193
00194
00195
00196
00197
00198
00199
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
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