24 #include <grass/lidar.h>
29 void node_x(
double x,
int *i_x,
double *csi_x,
double xMin,
double deltaX)
32 *i_x = (int)((
x - xMin) / deltaX);
33 *csi_x = (double)((
x - xMin) - (*i_x * deltaX));
41 void node_y(
double y,
int *i_y,
double *csi_y,
double yMin,
double deltaY)
44 *i_y = (int)((y - yMin) / deltaY);
45 *csi_y = (double)((y - yMin) - (*i_y * deltaY));
53 int order(
int i_x,
int i_y,
int yNum)
56 return (i_y + i_x * yNum);
65 return ((pow(2 - csi, 3.) - pow(1 - csi, 3.) * 4) / 6.);
71 return (pow(2 - csi, 3.) / 6.);
74 double phi_33(
double csi_x,
double csi_y)
80 double phi_34(
double csi_x,
double csi_y)
86 double phi_43(
double csi_x,
double csi_y)
92 double phi_44(
double csi_x,
double csi_y)
98 double phi(
double csi_x,
double csi_y)
101 return ((1 - csi_x) * (1 - csi_y));
108 double deltaX,
double deltaY,
int xNum,
int yNum,
109 double xMin,
double yMin,
int obsNum,
int parNum,
int BW)
112 int i, k, h, m, n, n0;
122 for (k = 0; k < parNum; k++) {
123 for (h = 0; h < BW; h++)
130 for (i = 0; i < obsNum; i++) {
132 node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
133 node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);
135 if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {
137 csi_x = csi_x / deltaX;
138 csi_y = csi_y / deltaY;
140 alpha[0][0] =
phi_44(1 + csi_x, 1 + csi_y);
141 alpha[0][1] =
phi_43(1 + csi_x, csi_y);
142 alpha[0][2] =
phi_43(1 + csi_x, 1 - csi_y);
143 alpha[0][3] =
phi_44(1 + csi_x, 2 - csi_y);
145 alpha[1][0] =
phi_34(csi_x, 1 + csi_y);
146 alpha[1][1] =
phi_33(csi_x, csi_y);
147 alpha[1][2] =
phi_33(csi_x, 1 - csi_y);
148 alpha[1][3] =
phi_34(csi_x, 2 - csi_y);
150 alpha[2][0] =
phi_34(1 - csi_x, 1 + csi_y);
151 alpha[2][1] =
phi_33(1 - csi_x, csi_y);
152 alpha[2][2] =
phi_33(1 - csi_x, 1 - csi_y);
153 alpha[2][3] =
phi_34(1 - csi_x, 2 - csi_y);
155 alpha[3][0] =
phi_44(2 - csi_x, 1 + csi_y);
156 alpha[3][1] =
phi_43(2 - csi_x, csi_y);
157 alpha[3][2] =
phi_43(2 - csi_x, 1 - csi_y);
158 alpha[3][3] =
phi_44(2 - csi_x, 2 - csi_y);
160 for (k = -1; k <= 2; k++) {
161 for (h = -1; h <= 2; h++) {
163 if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
164 ((i_y + h) >= 0) && ((i_y + h) < yNum)) {
165 for (m = k; m <= 2; m++) {
172 for (n = n0; n <= 2; n++) {
173 if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
174 ((i_y + n) >= 0) && ((i_y + n) < yNum)) {
175 N[
order(i_x + k, i_y + h, yNum)]
176 [
order(i_x + m, i_y + n, yNum) -
177 order(i_x + k, i_y + h, yNum)] +=
178 alpha[k + 1][h + 1] * (1 / Q[i]) *
184 TN[
order(i_x + k, i_y + h, yNum)] +=
185 obsVect[i][2] * (1 / Q[i]) * alpha[k + 1][h + 1];
199 void nCorrectLapl(
double **
N,
double lambda,
int xNum,
int yNum,
double deltaX,
208 double lambdaX, lambdaY;
211 lambdaX = lambda * (deltaY / deltaX);
212 lambdaY = lambda * (deltaX / deltaY);
218 alpha[0][2] = lambdaX * (1 / 9.);
219 alpha[0][3] = lambdaX * (1 / 36.);
222 alpha[1][0] = lambdaY * (1 / 36.);
223 alpha[1][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
224 alpha[1][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
225 alpha[1][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
226 alpha[1][4] = lambdaY * (1 / 36.);
228 alpha[2][0] = lambdaY * (1 / 9.);
229 alpha[2][1] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
230 alpha[2][2] = -lambdaX * (2 / 3.) - lambdaY * (2 / 3.);
231 alpha[2][3] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
232 alpha[2][4] = lambdaY * (1 / 9.);
234 alpha[3][0] = lambdaY * (1 / 36.);
235 alpha[3][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
236 alpha[3][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
237 alpha[3][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
238 alpha[3][4] = lambdaY * (1 / 36.);
241 alpha[4][1] = lambdaX * (1 / 36.);
242 alpha[4][2] = lambdaX * (1 / 9.);
243 alpha[4][3] = lambdaX * (1 / 36.);
246 for (i_x = 0; i_x < xNum; i_x++) {
247 for (i_y = 0; i_y < yNum; i_y++) {
249 for (k = -2; k <= 2; k++) {
250 for (h = -2; h <= 2; h++) {
252 if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
253 ((i_y + h) >= 0) && ((i_y + h) < yNum)) {
255 for (m = k; m <= 2; m++) {
262 for (n = n0; n <= 2; n++) {
263 if (((i_x + m) >= 0) &&
264 ((i_x + m) <= (xNum - 1)) &&
266 ((i_y + n) <= (yNum - 1))) {
268 if ((alpha[k + 2][h + 2] != 0) &&
269 (alpha[m + 2][n + 2] != 0)) {
270 N[
order(i_x + k, i_y + h, yNum)]
271 [
order(i_x + m, i_y + n, yNum) -
272 order(i_x + k, i_y + h, yNum)] +=
273 alpha[k + 2][h + 2] *
292 double deltaX,
double deltaY,
int xNum,
int yNum,
293 double xMin,
double yMin,
int obsNum,
int parNum,
int BW)
296 int i, k, h, m, n, n0;
306 for (k = 0; k < parNum; k++) {
307 for (h = 0; h < BW; h++)
314 for (i = 0; i < obsNum; i++) {
316 node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
317 node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);
319 if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {
321 csi_x = csi_x / deltaX;
322 csi_y = csi_y / deltaY;
324 alpha[0][0] =
phi(csi_x, csi_y);
325 alpha[0][1] =
phi(csi_x, 1 - csi_y);
326 alpha[1][0] =
phi(1 - csi_x, csi_y);
327 alpha[1][1] =
phi(1 - csi_x, 1 - csi_y);
329 for (k = 0; k <= 1; k++) {
330 for (h = 0; h <= 1; h++) {
332 if (((i_x + k) >= 0) && ((i_x + k) <= (xNum - 1)) &&
333 ((i_y + h) >= 0) && ((i_y + h) <= (yNum - 1))) {
335 for (m = k; m <= 1; m++) {
341 for (n = n0; n <= 1; n++) {
342 if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
343 ((i_y + n) >= 0) && ((i_y + n) < yNum)) {
344 N[
order(i_x + k, i_y + h, yNum)]
345 [
order(i_x + m, i_y + n, yNum) -
346 order(i_x + k, i_y + h, yNum)] +=
347 alpha[k][h] * (1 / Q[i]) * alpha[m][n];
352 TN[
order(i_x + k, i_y + h, yNum)] +=
353 obsVect[i][2] * (1 / Q[i]) * alpha[k][h];
367 void nCorrectGrad(
double **
N,
double lambda,
int xNum,
int yNum,
double deltaX,
375 double lambdaX, lambdaY;
377 lambdaX = lambda * (deltaY / deltaX);
378 lambdaY = lambda * (deltaX / deltaY);
380 parNum = xNum * yNum;
382 alpha[0] = lambdaX / 2. + lambdaY / 2.;
383 alpha[1] = -lambdaX / 4.;
384 alpha[2] = -lambdaY / 4.;
386 for (i = 0; i < parNum; i++) {
389 if ((i + 2) < parNum)
392 if ((i + 2 * yNum) < parNum)
393 N[i][2 * yNum] += alpha[1];
399 void nCorrectGrad(
double **
N,
double lambda,
int xNum,
int yNum,
double deltaX,
407 double lambdaX, lambdaY;
409 lambdaX = lambda * (deltaY / deltaX);
410 lambdaY = lambda * (deltaX / deltaY);
412 parNum = xNum * yNum;
414 alpha[0] = 2 * lambdaX + 2 * lambdaY;
418 for (i = 0; i < parNum; i++) {
421 if ((i + 1) < parNum)
424 if ((i + 1 * yNum) < parNum)
425 N[i][1 * yNum] += alpha[1];
435 double deltY,
int xNm,
int yNm,
double xMi,
double yMi,
448 for (i = 0; i < obsN; i++) {
452 node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
453 node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);
455 if ((i_x >= -2) && (i_x <= xNm) && (i_y >= -2) && (i_y <= yNm)) {
457 csi_x = csi_x / deltX;
458 csi_y = csi_y / deltY;
460 alpha[0][0] =
phi_44(1 + csi_x, 1 + csi_y);
461 alpha[0][1] =
phi_43(1 + csi_x, csi_y);
462 alpha[0][2] =
phi_43(1 + csi_x, 1 - csi_y);
463 alpha[0][3] =
phi_44(1 + csi_x, 2 - csi_y);
465 alpha[1][0] =
phi_34(csi_x, 1 + csi_y);
466 alpha[1][1] =
phi_33(csi_x, csi_y);
467 alpha[1][2] =
phi_33(csi_x, 1 - csi_y);
468 alpha[1][3] =
phi_34(csi_x, 2 - csi_y);
470 alpha[2][0] =
phi_34(1 - csi_x, 1 + csi_y);
471 alpha[2][1] =
phi_33(1 - csi_x, csi_y);
472 alpha[2][2] =
phi_33(1 - csi_x, 1 - csi_y);
473 alpha[2][3] =
phi_34(1 - csi_x, 2 - csi_y);
475 alpha[3][0] =
phi_44(2 - csi_x, 1 + csi_y);
476 alpha[3][1] =
phi_43(2 - csi_x, csi_y);
477 alpha[3][2] =
phi_43(2 - csi_x, 1 - csi_y);
478 alpha[3][3] =
phi_44(2 - csi_x, 2 - csi_y);
480 for (k = -1; k <= 2; k++) {
481 for (h = -1; h <= 2; h++) {
482 if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
483 ((i_y + h) >= 0) && ((i_y + h) < yNm))
484 obsE[i] += parV[
order(i_x + k, i_y + h, yNm)] *
498 int xNum,
int yNum,
double xMin,
double yMin,
512 node_x(
x, &i_x, &csi_x, xMin, deltaX);
513 node_y(y, &i_y, &csi_y, yMin, deltaY);
515 if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {
517 csi_x = csi_x / deltaX;
518 csi_y = csi_y / deltaY;
520 alpha[0][0] =
phi_44(1 + csi_x, 1 + csi_y);
521 alpha[0][1] =
phi_43(1 + csi_x, csi_y);
522 alpha[0][2] =
phi_43(1 + csi_x, 1 - csi_y);
523 alpha[0][3] =
phi_44(1 + csi_x, 2 - csi_y);
525 alpha[1][0] =
phi_34(csi_x, 1 + csi_y);
526 alpha[1][1] =
phi_33(csi_x, csi_y);
527 alpha[1][2] =
phi_33(csi_x, 1 - csi_y);
528 alpha[1][3] =
phi_34(csi_x, 2 - csi_y);
530 alpha[2][0] =
phi_34(1 - csi_x, 1 + csi_y);
531 alpha[2][1] =
phi_33(1 - csi_x, csi_y);
532 alpha[2][2] =
phi_33(1 - csi_x, 1 - csi_y);
533 alpha[2][3] =
phi_34(1 - csi_x, 2 - csi_y);
535 alpha[3][0] =
phi_44(2 - csi_x, 1 + csi_y);
536 alpha[3][1] =
phi_43(2 - csi_x, csi_y);
537 alpha[3][2] =
phi_43(2 - csi_x, 1 - csi_y);
538 alpha[3][3] =
phi_44(2 - csi_x, 2 - csi_y);
540 for (k = -1; k <= 2; k++) {
541 for (h = -1; h <= 2; h++) {
542 if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
543 ((i_y + h) >= 0) && ((i_y + h) < yNum))
544 z += parVect[
order(i_x + k, i_y + h, yNum)] *
557 double deltY,
int xNm,
int yNm,
double xMi,
double yMi,
570 for (i = 0; i < obsN; i++) {
574 node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
575 node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);
577 if ((i_x >= -1) && (i_x < xNm) && (i_y >= -1) && (i_y < yNm)) {
579 csi_x = csi_x / deltX;
580 csi_y = csi_y / deltY;
582 alpha[0][0] =
phi(csi_x, csi_y);
583 alpha[0][1] =
phi(csi_x, 1 - csi_y);
584 alpha[1][0] =
phi(1 - csi_x, csi_y);
585 alpha[1][1] =
phi(1 - csi_x, 1 - csi_y);
587 for (k = 0; k <= 1; k++) {
588 for (h = 0; h <= 1; h++) {
589 if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
590 ((i_y + h) >= 0) && ((i_y + h) < yNm))
592 parV[
order(i_x + k, i_y + h, yNm)] * alpha[k][h];
605 int xNum,
int yNum,
double xMin,
double yMin,
619 node_x(
x, &i_x, &csi_x, xMin, deltaX);
620 node_y(y, &i_y, &csi_y, yMin, deltaY);
622 if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {
624 csi_x = csi_x / deltaX;
625 csi_y = csi_y / deltaY;
627 alpha[0][0] =
phi(csi_x, csi_y);
628 alpha[0][1] =
phi(csi_x, 1 - csi_y);
630 alpha[1][0] =
phi(1 - csi_x, csi_y);
631 alpha[1][1] =
phi(1 - csi_x, 1 - csi_y);
633 for (k = 0; k <= 1; k++) {
634 for (h = 0; h <= 1; h++) {
635 if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
636 ((i_y + h) >= 0) && ((i_y + h) < yNum))
637 z += parVect[
order(i_x + k, i_y + h, yNum)] * alpha[k][h];
double phi_44(double csi_x, double csi_y)
double dataInterpolateBicubic(double x, double y, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, double *parVect)
int order(int i_x, int i_y, int yNum)
double phi_43(double csi_x, double csi_y)
double phi_33(double csi_x, double csi_y)
void normalDefBicubic(double **N, double *TN, double *Q, double **obsVect, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, int obsNum, int parNum, int BW)
void normalDefBilin(double **N, double *TN, double *Q, double **obsVect, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, int obsNum, int parNum, int BW)
void node_y(double y, int *i_y, double *csi_y, double yMin, double deltaY)
void obsEstimateBilin(double **obsV, double *obsE, double *parV, double deltX, double deltY, int xNm, int yNm, double xMi, double yMi, int obsN)
double phi_34(double csi_x, double csi_y)
double dataInterpolateBilin(double x, double y, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, double *parVect)
void nCorrectGrad(double **N, double lambda, int xNum, int yNum, double deltaX, double deltaY)
void node_x(double x, int *i_x, double *csi_x, double xMin, double deltaX)
double phi(double csi_x, double csi_y)
void obsEstimateBicubic(double **obsV, double *obsE, double *parV, double deltX, double deltY, int xNm, int yNm, double xMi, double yMi, int obsN)
void nCorrectLapl(double **N, double lambda, int xNum, int yNum, double deltaX, double deltaY)