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

afsTriangulator.cc

Go to the documentation of this file.
00001 /*
00002  * Triangulator code for AIBO FastSLAM. Takes in the locations and
00003  * values of two bearings-only measurements and constructs a Gaussian for
00004  * the probable location of the landmark.
00005  */
00006 
00007 #ifdef __cplusplus
00008 extern "C" {
00009 #endif
00010 
00011 #include "afsParticle.h"
00012 #include "afsUtility.h"
00013 
00014 #include <math.h>
00015 
00016 #ifdef UNIT_TEST
00017 #include <stdio.h>
00018 #endif
00019 
00020 /* To actually generate the gaussians, we need a way to perform triangulations
00021  * based on two bearing measurements from different locations. The arguments
00022  * to this routine consist of the measurements (first six args) and the
00023  * place to store the triangulated location information (last two).
00024  * We assume that the calling function has already ensured that the angles
00025  * given to us will be easy to triangulate. */
00026 void triangulate(double x1, double y1, double theta1,
00027      double x2, double y2, double theta2,
00028      double *pos_x, double *pos_y)
00029 {
00030   double ux1, uy1, ux2, uy2, t;
00031 
00032   /* The first thing we need to do is convert the ray form of these lines
00033    * into vector form. The u(x|y)(1|2) variables define linear subspaces
00034    * along our lines of sight. */
00035   ux1 = cos(theta1);
00036   uy1 = sin(theta1);
00037   ux2 = cos(theta2);
00038   uy2 = sin(theta2);
00039 
00040   /* Paul Bourke rules. His webpage can tell you how to find out anything
00041    * interesting. Go to http://astronomy.swin.edu.au/~pbourke/ and see.
00042    * This code computes how far from x1, y1 in terms of ux1, uy1 the
00043    * intersection is. */
00044   t = (ux2*(y1-y2) - uy2*(x1-x2)) / (uy2*ux1 - ux2*uy1);
00045 
00046   /* From this, we get the actual intersection */
00047   *pos_x = x1 + t*ux1;
00048   *pos_y = y1 + t*uy1;
00049 }
00050 
00051 /* The whole triangulator routine itself. See the header file for more
00052  * information */
00053 int afsTriangulator(double x1, double y1, double theta1,
00054         double x2, double y2, double theta2,
00055         afsLandmarkLoc *landmark)
00056 {
00057   /* Our stack frame is BIG. Uh-uh, it ain't ashamed... */
00058   double dtheta;
00059   double mean_x, mean_y;
00060   double variance_x, variance_y, variance_xy;
00061   double corner_x, corner_y;
00062 
00063   /* Make certain the two observations are far enough apart for us to
00064    * triangulate meaningfully. */
00065   dtheta = find_dtheta(theta1, theta2);
00066   if(dtheta < AFS_MIN_TRIANG_ANGLE) return 0;
00067   if(dtheta > AFS_MAX_TRIANG_ANGLE) return 0;
00068 
00069   /* Now we do that thing that we do. Find the mean of the Gaussian made
00070    * by the two measurements. */
00071   triangulate(x1,y1,theta1, x2,y2,theta2, &mean_x, &mean_y);
00072 
00073   /* The variance of this Gaussian is derived from the intersections of
00074    * the variance lines of the two measurements. We take the points of
00075    * these intersections and use them as training data for a maximum
00076    * likelihood Gaussian with a mean at the intersection calculated above.
00077    * This is a fancy way of saying that we take the x and y locations of
00078    * these points relative to the mean, multiply them by themselves to
00079    * make a 2x2 matrix, and averages the matrices made for all four points.
00080    * to get the final covariance matrix. */
00081   triangulate(x1,y1,theta1+AFS_MEASURE_VARIANCE,
00082         x2,y2,theta2-AFS_MEASURE_VARIANCE, &corner_x, &corner_y);
00083   corner_x -= mean_x;
00084   corner_y -= mean_y;
00085   variance_x = corner_x * corner_x;
00086   variance_y = corner_y * corner_y;
00087   variance_xy = corner_x * corner_y;
00088 #ifdef UNIT_TEST
00089 printf("plot(%f, %f, 'bx')\n", corner_x, corner_y);
00090 #endif
00091 
00092   triangulate(x1,y1,theta1-AFS_MEASURE_VARIANCE,
00093         x2,y2,theta2+AFS_MEASURE_VARIANCE, &corner_x, &corner_y);
00094   corner_x -= mean_x;
00095   corner_y -= mean_y;
00096   variance_x += corner_x * corner_x; /* note addition, not assignment */
00097   variance_y += corner_y * corner_y;
00098   variance_xy += corner_x * corner_y;
00099 #ifdef UNIT_TEST
00100 printf("plot(%f, %f, 'bx')\n", corner_x, corner_y);
00101 #endif
00102 
00103   triangulate(x1,y1,theta1+AFS_MEASURE_VARIANCE,
00104         x2,y2,theta2+AFS_MEASURE_VARIANCE, &corner_x, &corner_y);
00105   corner_x -= mean_x;
00106   corner_y -= mean_y;
00107   variance_x += corner_x * corner_x;
00108   variance_y += corner_y * corner_y;
00109   variance_xy += corner_x * corner_y;
00110 #ifdef UNIT_TEST
00111 printf("plot(%f, %f, 'bx')\n", corner_x, corner_y);
00112 #endif
00113 
00114   triangulate(x1,y1,theta1-AFS_MEASURE_VARIANCE,
00115         x2,y2,theta2-AFS_MEASURE_VARIANCE, &corner_x, &corner_y);
00116   corner_x -= mean_x;
00117   corner_y -= mean_y;
00118   variance_x += corner_x * corner_x;
00119   variance_y += corner_y * corner_y;
00120   variance_xy += corner_x * corner_y;
00121 #ifdef UNIT_TEST
00122 printf("plot(%f, %f, 'bx')\n", corner_x, corner_y);
00123 #endif
00124 
00125   /* Check for degeneracy and fail if we see it. */
00126   if((variance_x < 0.0001) || (variance_y < 0.0001)) return 0;
00127 
00128   /* Fill in the variance entries in the landmark and do division. */
00129   landmark->variance.x = variance_x * AFS_COVARIANCE_FUDGE / 4;
00130   landmark->variance.y = variance_y * AFS_COVARIANCE_FUDGE / 4;
00131   landmark->variance.xy = variance_xy * AFS_COVARIANCE_FUDGE / 4;
00132 
00133   /* Fill in the mean entries in the landmark. */
00134   landmark->mean.x = mean_x;
00135   landmark->mean.y = mean_y;
00136 
00137   /* Success! */
00138   return 1;
00139 }
00140 
00141 #ifdef UNIT_TEST
00142 
00143 int main(int argc, char **argv)
00144 {
00145   double x1, y1, x2, y2, theta1, theta2;
00146   afsLandmarkLoc l;
00147 
00148   fputs("x1: ", stderr);
00149   scanf("%lf", &x1);
00150   fputs("y1: ", stderr);
00151   scanf("%lf", &y1);
00152   fputs("theta1: ", stderr);
00153   scanf("%lf", &theta1);
00154   fputs("x2: ", stderr);
00155   scanf("%lf", &x2);
00156   fputs("y2: ", stderr);
00157   scanf("%lf", &y2);
00158   fputs("theta2: ", stderr);
00159   scanf("%lf", &theta2);
00160 
00161   theta1 *= M_PI/180;
00162   theta2 *= M_PI/180;
00163 
00164   puts("hold on");
00165   if(afsTriangulator(x1, y1, theta1, x2, y2, theta2, &l)) {
00166     printf("C = [%f %f; %f %f];\n",
00167       l.variance.x, l.variance.xy, l.variance.xy, l.variance.y);
00168     puts("plot_ellipse(C)");
00169   }
00170   else puts("Failure.");
00171 
00172   return 0;
00173 }
00174 
00175 #endif
00176 
00177 #ifdef __cplusplus
00178 }
00179 #endif

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