00001
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 #include <stdio.h>
00052 #include <math.h>
00053 #include <grass/libtrans.h>
00054
00055
00056 static double A0, A1, A2, A3, A4, A5;
00057 static double B0, B1, B2, B3, B4, B5;
00058
00059
00060 static int resid(double *, double *, double *, double *, int *, int, double *,
00061 double *, int);
00062
00063
00090 int compute_transformation_coef(double ax[], double ay[], double bx[],
00091 double by[], int *use, int n)
00092 {
00093 int i;
00094 int j;
00095 int count;
00096 double aa[3];
00097 double aar[3];
00098 double bb[3];
00099 double bbr[3];
00100
00101 double cc[3][3];
00102 double x;
00103
00104 count = 0;
00105 for (i = 0; i < n; i++)
00106 if (use[i])
00107 count++;
00108 if (count < 4)
00109 return -2;
00110
00111 for (i = 0; i < 3; i++) {
00112 aa[i] = bb[i] = 0.0;
00113
00114 for (j = 0; j < 3; j++)
00115 cc[i][j] = 0.0;
00116 }
00117
00118 for (i = 0; i < n; i++) {
00119 if (!use[i])
00120 continue;
00121 cc[0][0] += 1;
00122 cc[0][1] += bx[i];
00123 cc[0][2] += by[i];
00124
00125 cc[1][1] += bx[i] * bx[i];
00126 cc[1][2] += bx[i] * by[i];
00127 cc[2][2] += by[i] * by[i];
00128
00129 aa[0] += ay[i];
00130 aa[1] += ay[i] * bx[i];
00131 aa[2] += ay[i] * by[i];
00132
00133 bb[0] += ax[i];
00134 bb[1] += ax[i] * bx[i];
00135 bb[2] += ax[i] * by[i];
00136 }
00137
00138 cc[1][0] = cc[0][1];
00139 cc[2][0] = cc[0][2];
00140 cc[2][1] = cc[1][2];
00141
00142
00143 if (inverse(cc) < 0)
00144 return (-1);
00145 if (m_mult(cc, aa, aar) < 0 || m_mult(cc, bb, bbr) < 0)
00146 return (-1);
00147
00148
00149 B0 = aar[0];
00150 B1 = aar[1];
00151 B2 = aar[2];
00152
00153 B3 = bbr[0];
00154 B4 = bbr[1];
00155 B5 = bbr[2];
00156
00157
00158 x = B2 * B4 - B1 * B5;
00159
00160 if (!x)
00161 return (-1);
00162
00163 A0 = (B1 * B3 - B0 * B4) / x;
00164 A1 = -B1 / x;
00165 A2 = B4 / x;
00166 A3 = (B0 * B5 - B2 * B3) / x;
00167 A4 = B2 / x;
00168 A5 = -B5 / x;
00169
00170 return 1;
00171 }
00172
00173
00174 int transform_a_into_b(double ax, double ay, double *bx, double *by)
00175 {
00176 *by = A0 + A1 * ax + A2 * ay;
00177 *bx = A3 + A4 * ax + A5 * ay;
00178
00179 return 0;
00180 }
00181
00182
00183 int transform_b_into_a(double bx, double by, double *ax, double *ay)
00184 {
00185 *ay = B0 + B1 * bx + B2 * by;
00186 *ax = B3 + B4 * bx + B5 * by;
00187
00188 return 0;
00189 }
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205 int residuals_a_predicts_b(double ax[], double ay[], double bx[], double by[],
00206 int use[], int n, double residuals[], double *rms)
00207 {
00208 resid(ax, ay, bx, by, use, n, residuals, rms, 1);
00209
00210 return 0;
00211 }
00212
00213
00214 int residuals_b_predicts_a(double ax[], double ay[], double bx[], double by[],
00215 int use[], int n, double residuals[], double *rms)
00216 {
00217 resid(ax, ay, bx, by, use, n, residuals, rms, 0);
00218
00219 return 0;
00220 }
00221
00222
00231 int print_transform_matrix(void)
00232 {
00233 fprintf(stdout, "\nTransformation Matrix\n");
00234 fprintf(stdout, "| xoff a b |\n");
00235 fprintf(stdout, "| yoff d e |\n");
00236 fprintf(stdout, "-------------------------------------------\n");
00237 fprintf(stdout, "%f %f %f \n", -B3, B2, -B5);
00238 fprintf(stdout, "%f %f %f \n", -B0, -B1, B4);
00239 fprintf(stdout, "-------------------------------------------\n");
00240
00241 return 1;
00242 }
00243
00244
00245 static int resid(double ax[], double ay[], double bx[], double by[],
00246 int use[], int n, double residuals[], double *rms, int atob)
00247 {
00248 double x, y;
00249 int i;
00250 int count;
00251 double sum;
00252 double delta;
00253 double dx, dy;
00254
00255 count = 0;
00256 sum = 0.0;
00257 for (i = 0; i < n; i++) {
00258 if (!use[i])
00259 continue;
00260
00261 count++;
00262 if (atob) {
00263 transform_a_into_b(ax[i], ay[i], &x, &y);
00264 dx = x - bx[i];
00265 dy = y - by[i];
00266 }
00267 else {
00268 transform_b_into_a(bx[i], by[i], &x, &y);
00269 dx = x - ax[i];
00270 dy = y - ay[i];
00271 }
00272
00273 delta = dx * dx + dy * dy;
00274 residuals[i] = sqrt(delta);
00275 sum += delta;
00276 }
00277 *rms = sqrt(sum / count);
00278
00279 return 0;
00280 }