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];