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

afsFindBestPose.cc

Go to the documentation of this file.
00001 /*
00002  * Finds the proper orientation of the landmark and position information
00003  * within a particle based on the known locations of landmarks.
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 /* This routine determines the best orientation of the particle p given the
00020  * supplied robot pose and landmark positions. The latter are supplied in an
00021  * array pointed to by the last argument (note: the landmark ID numbers will
00022  * be used to index the array. */
00023 afsPose afsFindBestPose(afsParticle *p, afsPose pose, afsXY *real_landmarks)
00024 {
00025   afsPose Best;
00026   /* Decode as (real|particle) center of mass (x|y) */
00027   double r_cm_x, r_cm_y, p_cm_x, p_cm_y;
00028   /* A tally of how many primed landmarks this particle has */
00029   int i, tally = 0;
00030   /* Vector components we use for angle averaging */
00031   double dtheta_sum_x, dtheta_sum_y;
00032   /* The averaged angle, which indicates how much we should rotate the
00033    * particle's map to align it to the real map. */
00034   double dtheta;
00035 
00036   /* initialize Best to current pose */
00037   Best = p->pose;
00038 
00039   /* First we find the centers of mass of both the real landmarks and the
00040    * ones known to the particle */
00041   r_cm_x = r_cm_y = p_cm_x = p_cm_y = 0;
00042   /* summing step */
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   /* special case: if tally is 0, there's no way to best fit this particle.
00053    * just return the current one. */
00054   if(tally == 0) return Best;
00055   /* division step */
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   /* Now we compute how much the particle's map should be rotated to match
00062    * the real map best */
00063   dtheta_sum_x = dtheta_sum_y = 0;
00064   for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00065     /* x and y distances from the centers of mass to the landmark */
00066     /* use the same decoding scheme as above */
00067     double r_dist_x, r_dist_y, p_dist_x, p_dist_y;
00068     /* The angle difference between the particle and the map account of
00069      * the landmark location */
00070     double instance_dtheta;
00071 
00072     /* skip this landmark if unprimed */
00073     if(p->landmarks[i].state == PRIMING) continue;
00074 
00075     /* find distances */
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     /* find angle distance */
00082     instance_dtheta =
00083       find_dtheta(atan2(p_dist_y, p_dist_x), atan2(r_dist_y, r_dist_x));
00084     /* angle averaging summing step */
00085     dtheta_sum_x += cos(instance_dtheta);
00086     dtheta_sum_y += sin(instance_dtheta);
00087   }
00088   /* angle averaging division step */
00089   dtheta_sum_x /= (double) tally;
00090   dtheta_sum_y /= (double) tally;
00091   /* the angle, at last */
00092   dtheta = atan2(dtheta_sum_y, dtheta_sum_x);
00093 
00094   /* We change the angle in the suggested pose first */
00095   Best.theta += dtheta;
00096   /* Now we rotate the position around the center of mass */
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 /* This routine takes the location and orientation in the particle p and
00113  * places the same information in new_p after applying the translation and
00114  * rotation recommended in best_pose (which you can get from
00115  * afsFindBestPose. You can make p and new_p point to the same thing
00116  * safely, but why would you want to? */
00117 void afsApplyBestPose(afsParticle *p, afsParticle *new_p, afsPose best_pose)
00118 {
00119   int i;
00120   afsParticle work_p = *p; /* copy! */
00121   double dtheta = find_dtheta(work_p.pose.theta, best_pose.theta);
00122 
00123   /* Reposition the landmark locations */
00124   for(i=0; i<AFS_NUM_LANDMARKS; ++i) {
00125     double rel_dist, theta;
00126 
00127     /* shorthand */
00128     afsLandmarkLoc *l = &(work_p.landmarks[i]);
00129 
00130     /* Ignore unprimed landmarks */
00131     if(l->state == PRIMING) continue;
00132 
00133     /* Subtract off the robot x,y from the landmark positions */
00134     l->mean.x -= work_p.pose.x;
00135     l->mean.y -= work_p.pose.y;
00136 
00137     /* Rotate landmark positions */
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     /* Add the new robot x,y */
00144     l->mean.x += best_pose.x;
00145     l->mean.y += best_pose.y;
00146   }
00147 
00148   /* Now just change the robot pose information */
00149   work_p.pose = best_pose;
00150 
00151   /* copy! */
00152   *new_p = work_p;
00153 }
00154 
00155 /* This routine finds the error value for a particle given real map
00156  * information. The function we use to calculate the error here is the
00157  * same one we minimized in afsFindBestPose, only there we were just
00158  * concerned with its derivative. */
00159 /* NOTE: The more unprimed landmarks a particle has, the lower its error is
00160  * likely to be. This does not affect the performance of the algorithm */
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     /* shorthand */
00169     afsLandmarkLoc *l = &(p->landmarks[i]);
00170 
00171     /* Skip this landmark if it's not primed */
00172     if(l->state == PRIMING) continue;
00173 
00174     /* OK, find the error */
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   /* Add location error too. */
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 /* This routine makes an estimated map of landmarks in the environment and
00189  * comes up with an estimated robot pose for situations where ground truth
00190  * is not known. It does all of this by averaging. The first argument is
00191  * a collection of all the particles in the filter. g in the arguments
00192  * stands for "guess". */
00193 void afsGuessState(afsParticle *P, afsPose *g_pose, afsXY *g_landmarks)
00194 {
00195   int i, p;
00196   double hdgvect_x, hdgvect_y;
00197 
00198   /* initialize location to 0, since we're averaging */
00199   g_pose->x = g_pose->y = 0;
00200 
00201   /* Find the average location and heading */
00202   /* Since heading is modular, we instead convert the headings into unit
00203    * vectors, average them, then take the arc tangent (or set it to 0 if
00204    * all of the vectors cancel). */
00205   hdgvect_x = hdgvect_y = 0;
00206   /* averaging summing step */
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   /* averaging division step */
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); /* takes care of 0,0 itself */
00219 
00220   /* That was the easy part. Now we have to find the averaged locations of
00221    * all of the landmarks too. Whee! We transform them to relative radial
00222    * coordinates first, which makes things a lot easier. */
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     /* averaging summing step */
00230     for(p=0; p<AFS_NUM_PARTICLES; ++p) {
00231       /* shorthand */
00232       afsLandmarkLoc *l = &(P[p].landmarks[i]);
00233       double rel_theta, rel_dist_x, rel_dist_y;
00234 
00235       /* Skip this landmark if it's not primed */
00236       if(l->state == PRIMING) continue;
00237 
00238       /* Otherwise, up the tally and start summin'! */
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       /* We do the same vector trick for the bearing that we did before.
00245        * First, though, we have to find the relative bearing of the
00246        * landmark. */
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     /* If none of the particles had landmark i primed, then we can't very
00253      * well compute an average location. Oh well, set it to 0,0 */
00254     if(tally == 0) {
00255       g_landmarks[i].x = g_landmarks[i].y = 0;
00256       continue;
00257     }
00258 
00259     /* averaging division step */
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     /* the location, computed at last! */
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

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