00001
00002
00003
00004
00005
00006 #ifdef __cplusplus
00007 extern "C" {
00008 #endif
00009
00010 #include "afsParticle.h"
00011 #include "afsUtility.h"
00012 #include "afsFindBestPose.h"
00013
00014 #include "Configuration.h"
00015
00016 #include <math.h>
00017 #include <stdio.h>
00018
00019
00020
00021
00022
00023 afsPose afsFindBestPose(afsParticle *p, afsPose pose, afsXY *real_landmarks)
00024 {
00025 afsPose Best;
00026
00027 double r_cm_x, r_cm_y, p_cm_x, p_cm_y;
00028
00029 int i, tally = 0;
00030
00031 double dtheta_sum_x, dtheta_sum_y;
00032
00033
00034 double dtheta;
00035
00036
00037 Best = p->pose;
00038
00039
00040
00041 r_cm_x = r_cm_y = p_cm_x = p_cm_y = 0;
00042
00043 for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00044 r_cm_x += real_landmarks[i].x;
00045 r_cm_y += real_landmarks[i].y;
00046
00047 if(p->landmarks[i].state == PRIMING) continue;
00048 p_cm_x += p->landmarks[i].mean.x;
00049 p_cm_y += p->landmarks[i].mean.y;
00050 ++tally;
00051 }
00052
00053
00054 if(tally == 0) return Best;
00055
00056 r_cm_x /= (double) AFS_NUM_LANDMARKS;
00057 r_cm_y /= (double) AFS_NUM_LANDMARKS;
00058 p_cm_x /= (double) tally;
00059 p_cm_y /= (double) tally;
00060
00061
00062
00063 dtheta_sum_x = dtheta_sum_y = 0;
00064 for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00065
00066
00067 double r_dist_x, r_dist_y, p_dist_x, p_dist_y;
00068
00069
00070 double instance_dtheta;
00071
00072
00073 if(p->landmarks[i].state == PRIMING) continue;
00074
00075
00076 r_dist_x = real_landmarks[i].x - r_cm_x;
00077 r_dist_y = real_landmarks[i].y - r_cm_y;
00078 p_dist_x = p->landmarks[i].mean.x - p_cm_x;
00079 p_dist_y = p->landmarks[i].mean.y - p_cm_y;
00080
00081
00082 instance_dtheta =
00083 find_dtheta(atan2(p_dist_y, p_dist_x), atan2(r_dist_y, r_dist_x));
00084
00085 dtheta_sum_x += cos(instance_dtheta);
00086 dtheta_sum_y += sin(instance_dtheta);
00087 }
00088
00089 dtheta_sum_x /= (double) tally;
00090 dtheta_sum_y /= (double) tally;
00091
00092 dtheta = atan2(dtheta_sum_y, dtheta_sum_x);
00093
00094
00095 Best.theta += dtheta;
00096
00097 {
00098 double p_dist_from_cm, p_theta_from_cm, pdfc_x, pdfc_y;
00099
00100 pdfc_x = p->pose.x - p_cm_x;
00101 pdfc_y = p->pose.y - p_cm_y;
00102 p_dist_from_cm = sqrt(pdfc_x*pdfc_x + pdfc_y*pdfc_y);
00103 p_theta_from_cm = atan2(pdfc_y, pdfc_x);
00104
00105 Best.x = r_cm_x + p_dist_from_cm*cos(p_theta_from_cm + dtheta);
00106 Best.y = r_cm_y + p_dist_from_cm*sin(p_theta_from_cm + dtheta);
00107 }
00108
00109 return Best;
00110 }
00111
00112
00113
00114
00115
00116
00117 void afsApplyBestPose(afsParticle *p, afsParticle *new_p, afsPose best_pose)
00118 {
00119 int i;
00120 afsParticle work_p = *p;
00121 double dtheta = find_dtheta(work_p.pose.theta, best_pose.theta);
00122
00123
00124 for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00125 double rel_dist, theta;
00126
00127
00128 afsLandmarkLoc *l = &(work_p.landmarks[i]);
00129
00130
00131 if(l->state == PRIMING) continue;
00132
00133
00134 l->mean.x -= work_p.pose.x;
00135 l->mean.y -= work_p.pose.y;
00136
00137
00138 rel_dist = sqrt(l->mean.x*l->mean.x + l->mean.y*l->mean.y);
00139 theta = atan2(l->mean.y, l->mean.x) + dtheta;
00140 l->mean.x = rel_dist*cos(theta);
00141 l->mean.y = rel_dist*sin(theta);
00142
00143
00144 l->mean.x += best_pose.x;
00145 l->mean.y += best_pose.y;
00146 }
00147
00148
00149 work_p.pose = best_pose;
00150
00151
00152 *new_p = work_p;
00153 }
00154
00155
00156
00157
00158
00159
00160
00161 double afsParticleError(afsParticle *p, afsPose pose, afsXY *real_landmarks)
00162 {
00163 int i;
00164 double xerr, yerr;
00165 double error=0;
00166
00167 for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00168
00169 afsLandmarkLoc *l = &(p->landmarks[i]);
00170
00171
00172 if(l->state == PRIMING) continue;
00173
00174
00175 xerr = real_landmarks[i].x - l->mean.x;
00176 yerr = real_landmarks[i].y - l->mean.y;
00177 error += xerr*xerr + yerr*yerr;
00178 }
00179
00180
00181 xerr = pose.x - p->pose.x;
00182 yerr = pose.y - p->pose.y;
00183 error += xerr*xerr + yerr*yerr;
00184
00185 return error;
00186 }
00187
00188
00189
00190
00191
00192
00193 void afsGuessState(afsParticle *P, afsPose *g_pose, afsXY *g_landmarks)
00194 {
00195 int i, p;
00196 double hdgvect_x, hdgvect_y;
00197
00198
00199 g_pose->x = g_pose->y = 0;
00200
00201
00202
00203
00204
00205 hdgvect_x = hdgvect_y = 0;
00206
00207 for(p=0; p<AFS_NUM_PARTICLES; ++p) {
00208 g_pose->x += P[p].pose.x;
00209 g_pose->y += P[p].pose.y;
00210 hdgvect_x += cos(P[p].pose.theta);
00211 hdgvect_y += sin(P[p].pose.theta);
00212 }
00213
00214 g_pose->x /= (double) AFS_NUM_PARTICLES;
00215 g_pose->y /= (double) AFS_NUM_PARTICLES;
00216 hdgvect_x /= (double) AFS_NUM_PARTICLES;
00217 hdgvect_y /= (double) AFS_NUM_PARTICLES;
00218 g_pose->theta = atan2(hdgvect_y, hdgvect_x);
00219
00220
00221
00222
00223 for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00224 double avg_rel_dist, avg_rel_theta;
00225 int tally=0;
00226
00227 avg_rel_dist = hdgvect_x = hdgvect_y = 0;
00228
00229
00230 for(p=0; p<AFS_NUM_PARTICLES; ++p) {
00231
00232 afsLandmarkLoc *l = &(P[p].landmarks[i]);
00233 double rel_theta, rel_dist_x, rel_dist_y;
00234
00235
00236 if(l->state == PRIMING) continue;
00237
00238
00239 tally++;
00240 rel_dist_x = l->mean.x - P[p].pose.x;
00241 rel_dist_y = l->mean.y - P[p].pose.y;
00242 avg_rel_dist += sqrt(rel_dist_x*rel_dist_x + rel_dist_y*rel_dist_y);
00243
00244
00245
00246
00247 rel_theta = find_dtheta(P[p].pose.theta, atan2(l->mean.y, l->mean.x));
00248 hdgvect_x += cos(rel_theta);
00249 hdgvect_y += sin(rel_theta);
00250 }
00251
00252
00253
00254 if(tally == 0) {
00255 g_landmarks[i].x = g_landmarks[i].y = 0;
00256 continue;
00257 }
00258
00259
00260 avg_rel_dist /= (double) tally;
00261 hdgvect_x /= (double) tally;
00262 hdgvect_y /= (double) tally;
00263
00264 avg_rel_theta = atan2(hdgvect_x, hdgvect_y);
00265
00266
00267 g_landmarks[i].x = avg_rel_dist*cos(avg_rel_theta + g_pose->theta);
00268 g_landmarks[i].y = avg_rel_dist*sin(avg_rel_theta + g_pose->theta);
00269 }
00270 }
00271
00272 #ifdef __cplusplus
00273 }
00274 #endif