00001
00002
00003
00004
00005 #ifdef __cplusplus
00006 extern "C" {
00007 #endif
00008
00009 #include "afsParticle.h"
00010 #include "afsMotionResample.h"
00011 #include "afsMeasurementUpdate.h"
00012
00013
00014 #include "Configuration.h"
00015
00016 #include <stdio.h>
00017 #include <stdlib.h>
00018 #include <math.h>
00019
00020
00021 afsParticle ParticleSets[2][AFS_NUM_PARTICLES];
00022 afsParticle *Particles, *newParticles;
00023 double Weights[AFS_NUM_PARTICLES];
00024
00025
00026 void afsInit()
00027 {
00028 int i;
00029
00030 Particles = ParticleSets[0];
00031 newParticles = ParticleSets[1];
00032 for(i=0; i<AFS_NUM_PARTICLES; ++i) afsParticleInit(&Particles[i]);
00033 }
00034
00035
00036 void afsMotion(double dx, double dy, double da, unsigned int time)
00037 {
00038
00039 if((dx == 0) && (dy == 0) && (da == 0)) return;
00040
00041
00042 #include "motionReshapeKludge.h"
00043
00044
00045 if(time == 0) return;
00046
00047 int i;
00048 for(i=0; i<AFS_NUM_PARTICLES; ++i)
00049 afsMotionResample(&Particles[i], dx, dy, da, time);
00050 }
00051
00052
00053
00054
00055
00056 void afsSetLandmark(int landmark, double x, double y, double covariance)
00057 {
00058 int i;
00059 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00060 afsLandmarkLoc *l = &Particles[i].landmarks[landmark];
00061
00062 l->mean.x = x;
00063 l->mean.y = y;
00064 l->variance.x = l->variance.y = covariance;
00065 l->variance.xy = 0.0;
00066 l->state = PRIMED;
00067 }
00068 }
00069
00070
00071
00072
00073
00074
00075
00076
00077 void afsDistribute(double lx, double ty, double rx, double by)
00078 {
00079 double dx = rx - lx;
00080 double dy = ty - by;
00081 int i;
00082
00083 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00084 Particles[i].pose.x = lx + (double) rand() * dx / (double) RAND_MAX;
00085 Particles[i].pose.y = by + (double) rand() * dy / (double) RAND_MAX;
00086 Particles[i].pose.theta = (double) rand() * 2.0*M_PI / (double) RAND_MAX;
00087 }
00088 }
00089
00090
00091 void afsMeasurement(int landmark, double theta)
00092 {
00093 int i;
00094
00095 for(i=0; i<AFS_NUM_PARTICLES; ++i)
00096 afsMeasurementUpdate(&Particles[i], landmark, theta);
00097 }
00098
00099
00100 void afsResample()
00101 {
00102 int i, num_weighted=0;
00103 double weight_sum=0.0, prior_weight=0.0, surrogate_weight;
00104
00105 double current_wt_point, wt_increment;
00106 int current_wt_index = 0;
00107
00108
00109 for(i=0; i<AFS_NUM_PARTICLES; ++i)
00110 if(Particles[i].gotweight) {
00111 ++num_weighted;
00112 weight_sum += Particles[i].weight;
00113 Weights[i] = Particles[i].weight;
00114 }
00115
00116
00117 else Weights[i] = -1;
00118
00119
00120
00121
00122 surrogate_weight = num_weighted ? weight_sum / (double) num_weighted
00123 : 1 / (double) AFS_NUM_PARTICLES;
00124
00125
00126 weight_sum += surrogate_weight * (double) (AFS_NUM_PARTICLES - num_weighted);
00127
00128
00129
00130 for(i=0; i<AFS_NUM_PARTICLES; ++i)
00131 if(Weights[i] >= 0.0)
00132 prior_weight = Weights[i] = Weights[i] + prior_weight;
00133 else
00134 prior_weight = Weights[i] = surrogate_weight + prior_weight;
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146 current_wt_point = (double) rand() * weight_sum / (double) RAND_MAX;
00147
00148 wt_increment = weight_sum / (double) AFS_NUM_PARTICLES;
00149
00150
00151 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00152
00153
00154 while(current_wt_point > Weights[current_wt_index])
00155
00156 if(++current_wt_index >= AFS_NUM_PARTICLES) {
00157 current_wt_index = 0;
00158 current_wt_point = fmod(current_wt_point, weight_sum);
00159 }
00160
00161 afsParticleCopy(&Particles[current_wt_index], &newParticles[i]);
00162
00163 current_wt_point += wt_increment;
00164
00165
00166 newParticles[i].weight = 1.0;
00167 }
00168
00169
00170
00171 {
00172 afsParticle* temp = Particles;
00173 Particles = newParticles;
00174 newParticles = temp;
00175 }
00176
00177
00178 }
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188 afsParticle *afsWhatsUp(double *error)
00189 {
00190
00191
00192
00193
00194
00195 int i, j;
00196 static afsParticle avg;
00197 double theta_x, theta_y;
00198
00199
00200
00201
00202 afsParticleInit(&avg);
00203 for(j=0; j<AFS_NUM_LANDMARKS; ++j)
00204 avg.landmarks[j].mean.x = avg.landmarks[j].mean.x = 0.0;
00205
00206
00207
00208
00209 theta_x = theta_y = 0.0;
00210
00211
00212 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00213 avg.pose.x += Particles[i].pose.x;
00214 avg.pose.y += Particles[i].pose.y;
00215
00216 theta_x += cos(Particles[i].pose.theta);
00217 theta_y += sin(Particles[i].pose.theta);
00218 }
00219
00220
00221 avg.pose.x /= (double) AFS_NUM_PARTICLES;
00222 avg.pose.y /= (double) AFS_NUM_PARTICLES;
00223 theta_x /= AFS_NUM_PARTICLES;
00224 theta_y /= AFS_NUM_PARTICLES;
00225
00226 avg.pose.theta = atan2(theta_y, theta_x);
00227 if(avg.pose.theta < 0) avg.pose.theta += 2*M_PI;
00228
00229
00230
00231
00232
00233 for(j=0; j<AFS_NUM_LANDMARKS; ++j) {
00234 int primed_count = 0;
00235 double mx=0.0, my=0.0;
00236 double covx=0.0, covxy=0.0, covy=0.0;
00237 double dcvx=0.0, dcvxy=0.0, dcvy=0.0;
00238
00239
00240
00241
00242
00243
00244
00245
00246 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00247 afsLandmarkLoc *l = &(Particles[i].landmarks[j]);
00248
00249 if(l->state == PRIMED) {
00250 mx += l->mean.x;
00251 my += l->mean.y;
00252
00253 covx += l->variance.x;
00254 covxy += l->variance.xy;
00255 covy += l->variance.y;
00256
00257 ++primed_count;
00258 }
00259 }
00260
00261
00262
00263
00264 if(primed_count == 0) break;
00265
00266
00267 mx /= (double) primed_count;
00268 my /= (double) primed_count;
00269
00270
00271 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00272 afsLandmarkLoc *l = &(Particles[i].landmarks[j]);
00273
00274 if(l->state == PRIMED) {
00275 double dx = l->mean.x - mx;
00276 double dy = l->mean.y - my;
00277
00278 dcvx += dx * dx;
00279 dcvxy += dx * dy;
00280 dcvy += dy * dy;
00281 }
00282 }
00283
00284
00285 covx = (covx + dcvx ) / (double) primed_count;
00286 covxy = (covxy + dcvxy) / (double) primed_count;
00287 covy = (covy + dcvy ) / (double) primed_count;
00288
00289
00290 avg.landmarks[j].state = PRIMED;
00291 avg.landmarks[j].mean.x = mx;
00292 avg.landmarks[j].mean.y = my;
00293 avg.landmarks[j].variance.x = covx;
00294 avg.landmarks[j].variance.xy = covxy;
00295 avg.landmarks[j].variance.y = covy;
00296 }
00297
00298
00299
00300
00301 if(error) {
00302 double dist = 0.0;
00303 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00304 double dx = avg.pose.x - Particles[i].pose.x;
00305 double dy = avg.pose.y - Particles[i].pose.y;
00306 dist += sqrt(dx*dx + dy*dy);
00307 }
00308 *error = dist / (double) AFS_NUM_PARTICLES;
00309 }
00310
00311 return &avg;
00312
00313
00314
00315
00316
00317 #if 0
00318 int i;
00319 afsParticle *closest = NULL;
00320 double best_dist = HUGE_VAL;
00321 afsXY avg_location;
00322
00323
00324 double avg_dist = 0;
00325
00326 afsXY guessmap[AFS_NUM_LANDMARKS];
00327 afsPose guesspose, best_pose;
00328
00329 avg_location.x = avg_location.y = 0;
00330
00331
00332
00333 afsGuessState(Particles, &guesspose, guessmap);
00334
00335
00336 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00337
00338 best_pose = afsFindBestPose(&Particles[i], guesspose, guessmap);
00339
00340
00341
00342 avg_location.x += (rectified_locations[i].x = best_pose.x);
00343 avg_location.y += (rectified_locations[i].y = best_pose.y);
00344 }
00345
00346
00347 avg_location.x /= (double) AFS_NUM_PARTICLES;
00348 avg_location.y /= (double) AFS_NUM_PARTICLES;
00349
00350
00351
00352 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00353 double xdist, ydist, dist;
00354
00355 xdist = rectified_locations[i].x - avg_location.x;
00356 ydist = rectified_locations[i].y - avg_location.y;
00357 dist = sqrt(xdist*xdist + ydist*ydist);
00358
00359
00360 avg_dist += dist;
00361
00362 if(dist < best_dist) {
00363 best_dist = dist;
00364 closest = &Particles[i];
00365 }
00366 }
00367
00368
00369 fprintf(stderr, "AVERAGE DISTANCE: %f\n", avg_dist/(double)AFS_NUM_PARTICLES);
00370
00371
00372 if(error) {
00373 afsParticle work_p;
00374
00375 best_pose = afsFindBestPose(closest, guesspose, guessmap);
00376 afsApplyBestPose(closest, &work_p, best_pose);
00377 *error = afsParticleError(&work_p, guesspose, guessmap);
00378 }
00379
00380 return closest;
00381 #endif
00382 }
00383
00384
00385
00386
00387
00388
00389
00390 double afsCertainty()
00391 {
00392 double error;
00393 afsWhatsUp(&error);
00394 return error;
00395 #if 0
00396 int i;
00397 double total_error = 0;
00398
00399 afsXY guessmap[AFS_NUM_LANDMARKS];
00400 afsPose guesspose;
00401
00402
00403
00404 afsGuessState(Particles, &guesspose, guessmap);
00405
00406
00407 for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00408 afsPose best_pose;
00409 afsParticle work_p;
00410
00411
00412 best_pose = afsFindBestPose(&Particles[i], guesspose, guessmap);
00413 afsApplyBestPose(&Particles[i], &work_p, best_pose);
00414
00415
00416 total_error += afsParticleError(&work_p, guesspose, guessmap);
00417 }
00418
00419 return log(total_error);
00420 #endif
00421 }
00422
00423
00424
00425
00426
00427
00428 afsParticle *afsInvadeFSData()
00429 {
00430 return Particles;
00431 }
00432
00433 #ifdef __cplusplus
00434 }
00435 #endif