25 #include <grass/lidar.h> 30 void node_x(
double x,
int *i_x,
double *csi_x,
double xMin,
double deltaX)
33 *i_x = (int)((x - xMin) / deltaX);
34 *csi_x = (double)((x - xMin) - (*i_x * deltaX));
42 void node_y(
double y,
int *i_y,
double *csi_y,
double yMin,
double deltaY)
45 *i_y = (int)((y - yMin) / deltaY);
46 *csi_y = (double)((y - yMin) - (*i_y * deltaY));
54 int order(
int i_x,
int i_y,
int yNum)
57 return (i_y + i_x * yNum);
66 return ((pow(2 - csi, 3.) - pow(1 - csi, 3.) * 4) / 6.);
72 return (pow(2 - csi, 3.) / 6.);
75 double phi_33(
double csi_x,
double csi_y)
81 double phi_34(
double csi_x,
double csi_y)
87 double phi_43(
double csi_x,
double csi_y)
93 double phi_44(
double csi_x,
double csi_y)
99 double phi(
double csi_x,
double csi_y)
102 return ((1 - csi_x) * (1 - csi_y));
109 double deltaX,
double deltaY,
int xNum,
int yNum,
110 double xMin,
double yMin,
int obsNum,
int parNum,
114 int i, k, h, m, n, n0;
124 for (k = 0; k < parNum; k++) {
125 for (h = 0; h < BW; h++)
132 for (i = 0; i < obsNum; i++) {
134 node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
135 node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);
137 if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {
139 csi_x = csi_x / deltaX;
140 csi_y = csi_y / deltaY;
142 alpha[0][0] =
phi_44(1 + csi_x, 1 + csi_y);
143 alpha[0][1] =
phi_43(1 + csi_x, csi_y);
144 alpha[0][2] =
phi_43(1 + csi_x, 1 - csi_y);
145 alpha[0][3] =
phi_44(1 + csi_x, 2 - csi_y);
147 alpha[1][0] =
phi_34(csi_x, 1 + csi_y);
148 alpha[1][1] =
phi_33(csi_x, csi_y);
149 alpha[1][2] =
phi_33(csi_x, 1 - csi_y);
150 alpha[1][3] =
phi_34(csi_x, 2 - csi_y);
152 alpha[2][0] =
phi_34(1 - csi_x, 1 + csi_y);
153 alpha[2][1] =
phi_33(1 - csi_x, csi_y);
154 alpha[2][2] =
phi_33(1 - csi_x, 1 - csi_y);
155 alpha[2][3] =
phi_34(1 - csi_x, 2 - csi_y);
157 alpha[3][0] =
phi_44(2 - csi_x, 1 + csi_y);
158 alpha[3][1] =
phi_43(2 - csi_x, csi_y);
159 alpha[3][2] =
phi_43(2 - csi_x, 1 - csi_y);
160 alpha[3][3] =
phi_44(2 - csi_x, 2 - csi_y);
162 for (k = -1; k <= 2; k++) {
163 for (h = -1; h <= 2; h++) {
165 if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
166 ((i_y + h) >= 0) && ((i_y + h) < yNum)) {
167 for (m = k; m <= 2; m++) {
174 for (n = n0; n <= 2; n++) {
175 if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
176 ((i_y + n) >= 0) && ((i_y + n) < yNum)) {
196 TN[
order(i_x + k, i_y + h, yNum)] +=
197 obsVect[i][2] * (1 / Q[i]) * alpha[k + 1][h + 1];
212 double deltaX,
double deltaY)
220 double lambdaX, lambdaY;
223 lambdaX = lambda * (deltaY / deltaX);
224 lambdaY = lambda * (deltaX / deltaY);
227 alpha[0][1] = lambdaX * (1 / 36.);
228 alpha[0][2] = lambdaX * (1 / 9.);
229 alpha[0][3] = lambdaX * (1 / 36.);
232 alpha[1][0] = lambdaY * (1 / 36.);
233 alpha[1][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
234 alpha[1][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
235 alpha[1][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
236 alpha[1][4] = lambdaY * (1 / 36.);
238 alpha[2][0] = lambdaY * (1 / 9.);
239 alpha[2][1] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
240 alpha[2][2] = -lambdaX * (2 / 3.) - lambdaY * (2 / 3.);
241 alpha[2][3] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
242 alpha[2][4] = lambdaY * (1 / 9.);
244 alpha[3][0] = lambdaY * (1 / 36.);
245 alpha[3][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
246 alpha[3][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
247 alpha[3][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
248 alpha[3][4] = lambdaY * (1 / 36.);
251 alpha[4][1] = lambdaX * (1 / 36.);
252 alpha[4][2] = lambdaX * (1 / 9.);
253 alpha[4][3] = lambdaX * (1 / 36.);
256 for (i_x = 0; i_x < xNum; i_x++) {
257 for (i_y = 0; i_y < yNum; i_y++) {
259 for (k = -2; k <= 2; k++) {
260 for (h = -2; h <= 2; h++) {
262 if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
263 ((i_y + h) >= 0) && ((i_y + h) < yNum)) {
265 for (m = k; m <= 2; m++) {
272 for (n = n0; n <= 2; n++) {
273 if (((i_x + m) >= 0) &&
274 ((i_x + m) <= (xNum - 1)) &&
276 ((i_y + n) <= (yNum - 1))) {
278 if ((alpha[k + 2][h + 2] != 0) &&
279 (alpha[m + 2][n + 2] != 0)) {
294 alpha[k + 2][h + 2] * alpha[m +
314 double deltaX,
double deltaY,
int xNum,
int yNum,
315 double xMin,
double yMin,
int obsNum,
int parNum,
int BW)
318 int i, k, h, m, n, n0;
328 for (k = 0; k < parNum; k++) {
329 for (h = 0; h < BW; h++)
336 for (i = 0; i < obsNum; i++) {
338 node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
339 node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);
341 if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {
343 csi_x = csi_x / deltaX;
344 csi_y = csi_y / deltaY;
346 alpha[0][0] =
phi(csi_x, csi_y);
347 alpha[0][1] =
phi(csi_x, 1 - csi_y);
348 alpha[1][0] =
phi(1 - csi_x, csi_y);
349 alpha[1][1] =
phi(1 - csi_x, 1 - csi_y);
351 for (k = 0; k <= 1; k++) {
352 for (h = 0; h <= 1; h++) {
354 if (((i_x + k) >= 0) && ((i_x + k) <= (xNum - 1)) &&
355 ((i_y + h) >= 0) && ((i_y + h) <= (yNum - 1))) {
357 for (m = k; m <= 1; m++) {
363 for (n = n0; n <= 1; n++) {
364 if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
365 ((i_y + n) >= 0) && ((i_y + n) < yNum)) {
378 alpha[k][h] * (1 / Q[i]) *
384 TN[
order(i_x + k, i_y + h, yNum)] +=
385 obsVect[i][2] * (1 / Q[i]) * alpha[k][h];
400 void nCorrectGrad(
double **
N,
double lambda,
int xNum,
int yNum,
401 double deltaX,
double deltaY)
408 double lambdaX, lambdaY;
410 lambdaX = lambda * (deltaY / deltaX);
411 lambdaY = lambda * (deltaX / deltaY);
413 parNum = xNum * yNum;
415 alpha[0] = lambdaX / 2. + lambdaY / 2.;
416 alpha[1] = -lambdaX / 4.;
417 alpha[2] = -lambdaY / 4.;
419 for (i = 0; i < parNum; i++) {
422 if ((i + 2) < parNum)
425 if ((i + 2 * yNum) < parNum)
426 N[i][2 * yNum] += alpha[1];
433 double deltaX,
double deltaY)
440 double lambdaX, lambdaY;
442 lambdaX = lambda * (deltaY / deltaX);
443 lambdaY = lambda * (deltaX / deltaY);
445 parNum = xNum * yNum;
447 alpha[0] = 2 * lambdaX + 2 * lambdaY;
451 for (i = 0; i < parNum; i++) {
454 if ((i + 1) < parNum)
457 if ((i + 1 * yNum) < parNum)
458 N[i][1 * yNum] += alpha[1];
468 double deltX,
double deltY,
int xNm,
int yNm,
469 double xMi,
double yMi,
int obsN)
481 for (i = 0; i < obsN; i++) {
485 node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
486 node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);
488 if ((i_x >= -2) && (i_x <= xNm) && (i_y >= -2) && (i_y <= yNm)) {
490 csi_x = csi_x / deltX;
491 csi_y = csi_y / deltY;
493 alpha[0][0] =
phi_44(1 + csi_x, 1 + csi_y);
494 alpha[0][1] =
phi_43(1 + csi_x, csi_y);
495 alpha[0][2] =
phi_43(1 + csi_x, 1 - csi_y);
496 alpha[0][3] =
phi_44(1 + csi_x, 2 - csi_y);
498 alpha[1][0] =
phi_34(csi_x, 1 + csi_y);
499 alpha[1][1] =
phi_33(csi_x, csi_y);
500 alpha[1][2] =
phi_33(csi_x, 1 - csi_y);
501 alpha[1][3] =
phi_34(csi_x, 2 - csi_y);
503 alpha[2][0] =
phi_34(1 - csi_x, 1 + csi_y);
504 alpha[2][1] =
phi_33(1 - csi_x, csi_y);
505 alpha[2][2] =
phi_33(1 - csi_x, 1 - csi_y);
506 alpha[2][3] =
phi_34(1 - csi_x, 2 - csi_y);
508 alpha[3][0] =
phi_44(2 - csi_x, 1 + csi_y);
509 alpha[3][1] =
phi_43(2 - csi_x, csi_y);
510 alpha[3][2] =
phi_43(2 - csi_x, 1 - csi_y);
511 alpha[3][3] =
phi_44(2 - csi_x, 2 - csi_y);
513 for (k = -1; k <= 2; k++) {
514 for (h = -1; h <= 2; h++) {
515 if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
516 ((i_y + h) >= 0) && ((i_y + h) < yNm))
518 parV[
order(i_x + k, i_y + h, yNm)] * alpha[k +
534 double deltaY,
int xNum,
int yNum,
double xMin,
535 double yMin,
double *parVect)
549 node_x(x, &i_x, &csi_x, xMin, deltaX);
550 node_y(y, &i_y, &csi_y, yMin, deltaY);
552 if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {
554 csi_x = csi_x / deltaX;
555 csi_y = csi_y / deltaY;
557 alpha[0][0] =
phi_44(1 + csi_x, 1 + csi_y);
558 alpha[0][1] =
phi_43(1 + csi_x, csi_y);
559 alpha[0][2] =
phi_43(1 + csi_x, 1 - csi_y);
560 alpha[0][3] =
phi_44(1 + csi_x, 2 - csi_y);
562 alpha[1][0] =
phi_34(csi_x, 1 + csi_y);
563 alpha[1][1] =
phi_33(csi_x, csi_y);
564 alpha[1][2] =
phi_33(csi_x, 1 - csi_y);
565 alpha[1][3] =
phi_34(csi_x, 2 - csi_y);
567 alpha[2][0] =
phi_34(1 - csi_x, 1 + csi_y);
568 alpha[2][1] =
phi_33(1 - csi_x, csi_y);
569 alpha[2][2] =
phi_33(1 - csi_x, 1 - csi_y);
570 alpha[2][3] =
phi_34(1 - csi_x, 2 - csi_y);
572 alpha[3][0] =
phi_44(2 - csi_x, 1 + csi_y);
573 alpha[3][1] =
phi_43(2 - csi_x, csi_y);
574 alpha[3][2] =
phi_43(2 - csi_x, 1 - csi_y);
575 alpha[3][3] =
phi_44(2 - csi_x, 2 - csi_y);
577 for (k = -1; k <= 2; k++) {
578 for (h = -1; h <= 2; h++) {
579 if (((i_x + k) >= 0) && ((i_x + k) < xNum) && ((i_y + h) >= 0)
580 && ((i_y + h) < yNum))
581 z += parVect[
order(i_x + k, i_y + h, yNum)] * alpha[k +
596 double deltY,
int xNm,
int yNm,
double xMi,
double yMi,
609 for (i = 0; i < obsN; i++) {
613 node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
614 node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);
616 if ((i_x >= -1) && (i_x < xNm) && (i_y >= -1) && (i_y < yNm)) {
618 csi_x = csi_x / deltX;
619 csi_y = csi_y / deltY;
621 alpha[0][0] =
phi(csi_x, csi_y);
622 alpha[0][1] =
phi(csi_x, 1 - csi_y);
623 alpha[1][0] =
phi(1 - csi_x, csi_y);
624 alpha[1][1] =
phi(1 - csi_x, 1 - csi_y);
626 for (k = 0; k <= 1; k++) {
627 for (h = 0; h <= 1; h++) {
628 if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
629 ((i_y + h) >= 0) && ((i_y + h) < yNm))
631 parV[
order(i_x + k, i_y + h, yNm)] * alpha[k][h];
645 int xNum,
int yNum,
double xMin,
double yMin,
659 node_x(x, &i_x, &csi_x, xMin, deltaX);
660 node_y(y, &i_y, &csi_y, yMin, deltaY);
662 if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {
664 csi_x = csi_x / deltaX;
665 csi_y = csi_y / deltaY;
667 alpha[0][0] =
phi(csi_x, csi_y);
668 alpha[0][1] =
phi(csi_x, 1 - csi_y);
670 alpha[1][0] =
phi(1 - csi_x, csi_y);
671 alpha[1][1] =
phi(1 - csi_x, 1 - csi_y);
673 for (k = 0; k <= 1; k++) {
674 for (h = 0; h <= 1; h++) {
675 if (((i_x + k) >= 0) && ((i_x + k) < xNum) && ((i_y + h) >= 0)
676 && ((i_y + h) < yNum))
677 z += parVect[
order(i_x + k, i_y + h, yNum)] * alpha[k][h];
double phi_33(double csi_x, double csi_y)
void obsEstimateBilin(double **obsV, double *obsE, double *parV, double deltX, double deltY, int xNm, int yNm, double xMi, double yMi, int obsN)
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)
void nCorrectLapl(double **N, double lambda, int xNum, int yNum, double deltaX, double deltaY)
double dataInterpolateBilin(double x, double y, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, double *parVect)
void obsEstimateBicubic(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)
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)
double phi_44(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)
double dataInterpolateBicubic(double x, double y, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, double *parVect)
double phi_43(double csi_x, double csi_y)
int order(int i_x, int i_y, int yNum)
double phi(double csi_x, double csi_y)