00001
00002
00003
00004
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
00021
00022
00023
00024
00025
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
00033
00034
00035 ux1 = cos(theta1);
00036 uy1 = sin(theta1);
00037 ux2 = cos(theta2);
00038 uy2 = sin(theta2);
00039
00040
00041
00042
00043
00044 t = (ux2*(y1-y2) - uy2*(x1-x2)) / (uy2*ux1 - ux2*uy1);
00045
00046
00047 *pos_x = x1 + t*ux1;
00048 *pos_y = y1 + t*uy1;
00049 }
00050
00051
00052
00053 int afsTriangulator(double x1, double y1, double theta1,
00054 double x2, double y2, double theta2,
00055 afsLandmarkLoc *landmark)
00056 {
00057
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
00064
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
00070
00071 triangulate(x1,y1,theta1, x2,y2,theta2, &mean_x, &mean_y);
00072
00073
00074
00075
00076
00077
00078
00079
00080
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;
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
00126 if((variance_x < 0.0001) || (variance_y < 0.0001)) return 0;
00127
00128
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
00134 landmark->mean.x = mean_x;
00135 landmark->mean.y = mean_y;
00136
00137
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