Loading...
Searching...
No Matches
InterpSpline.c
Go to the documentation of this file.
/***********************************************************************
*
* MODULE: lidarlib
*
* AUTHOR(S): Roberto Antolin
*
* PURPOSE: LIDAR Spline Interpolation
*
* COPYRIGHT: (C) 2006 by Politecnico di Milano -
* Polo Regionale di Como
*
* This program is free software under the
* GNU General Public License (>=v2).
* Read the file COPYING that comes with GRASS
* for details.
*
**************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include <string.h>
#include <grass/lidar.h>
/*----------------------------------------------------------------------------*/
/* Abscissa node index computation */
void node_x(double x, int *i_x, double *csi_x, double xMin, double deltaX)
{
*i_x = (int)((x - xMin) / deltaX);
*csi_x = (double)((x - xMin) - (*i_x * deltaX));
return;
}
/*----------------------------------------------------------------------------*/
/* Ordinate node index computation */
void node_y(double y, int *i_y, double *csi_y, double yMin, double deltaY)
{
*i_y = (int)((y - yMin) / deltaY);
*csi_y = (double)((y - yMin) - (*i_y * deltaY));
return;
}
/*----------------------------------------------------------------------------*/
/* Node order computation */
int order(int i_x, int i_y, int yNum)
{
return (i_y + i_x * yNum);
}
/*----------------------------------------------------------------------------*/
/* Design matrix coefficients computation */
double phi_3(double csi)
{
return ((pow(2 - csi, 3.) - pow(1 - csi, 3.) * 4) / 6.);
}
double phi_4(double csi)
{
return (pow(2 - csi, 3.) / 6.);
}
double phi_33(double csi_x, double csi_y)
{
return (phi_3(csi_x) * phi_3(csi_y));
}
double phi_34(double csi_x, double csi_y)
{
return (phi_3(csi_x) * phi_4(csi_y));
}
double phi_43(double csi_x, double csi_y)
{
return (phi_4(csi_x) * phi_3(csi_y));
}
double phi_44(double csi_x, double csi_y)
{
return (phi_4(csi_x) * phi_4(csi_y));
}
double phi(double csi_x, double csi_y)
{
return ((1 - csi_x) * (1 - csi_y));
}
/*----------------------------------------------------------------------------*/
/* Normal system computation for bicubic spline interpolation */
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)
{
int i, k, h, m, n, n0; /* counters */
double alpha[4][4]; /* coefficients */
int i_x; /* x = (xMin + (i_x * deltaX) + csi_x) */
double csi_x;
int i_y; /* y = (yMin + (i_y * deltaY) + csi_y) */
double csi_y;
/*--------------------------------------*/
for (k = 0; k < parNum; k++) {
for (h = 0; h < BW; h++)
N[k][h] = 0.; /* Normal matrix inizialization */
TN[k] = 0.; /* Normal vector inizialization */
}
/*--------------------------------------*/
for (i = 0; i < obsNum; i++) {
node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);
if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {
csi_x = csi_x / deltaX;
csi_y = csi_y / deltaY;
alpha[0][0] = phi_44(1 + csi_x, 1 + csi_y);
alpha[0][1] = phi_43(1 + csi_x, csi_y);
alpha[0][2] = phi_43(1 + csi_x, 1 - csi_y);
alpha[0][3] = phi_44(1 + csi_x, 2 - csi_y);
alpha[1][0] = phi_34(csi_x, 1 + csi_y);
alpha[1][1] = phi_33(csi_x, csi_y);
alpha[1][2] = phi_33(csi_x, 1 - csi_y);
alpha[1][3] = phi_34(csi_x, 2 - csi_y);
alpha[2][0] = phi_34(1 - csi_x, 1 + csi_y);
alpha[2][1] = phi_33(1 - csi_x, csi_y);
alpha[2][2] = phi_33(1 - csi_x, 1 - csi_y);
alpha[2][3] = phi_34(1 - csi_x, 2 - csi_y);
alpha[3][0] = phi_44(2 - csi_x, 1 + csi_y);
alpha[3][1] = phi_43(2 - csi_x, csi_y);
alpha[3][2] = phi_43(2 - csi_x, 1 - csi_y);
alpha[3][3] = phi_44(2 - csi_x, 2 - csi_y);
for (k = -1; k <= 2; k++) {
for (h = -1; h <= 2; h++) {
if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
((i_y + h) >= 0) && ((i_y + h) < yNum)) {
for (m = k; m <= 2; m++) {
if (m == k)
n0 = h;
else
n0 = -1;
for (n = n0; n <= 2; n++) {
if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
((i_y + n) >= 0) && ((i_y + n) < yNum)) {
N[order(i_x + k, i_y + h, yNum)]
[order(i_x + m, i_y + n, yNum) -
order(i_x + k, i_y + h, yNum)] +=
alpha[k + 1][h + 1] * (1 / Q[i]) *
alpha[m + 1][n + 1];
/* 1/Q[i] only refers to the variances */
}
}
}
TN[order(i_x + k, i_y + h, yNum)] +=
obsVect[i][2] * (1 / Q[i]) * alpha[k + 1][h + 1];
}
}
}
}
}
return;
}
/*----------------------------------------------------------------------------*/
/* Normal system correction - Introduzione della correzione dovuta alle
pseudosservazioni (Tykonov) - LAPALCIANO - */
void nCorrectLapl(double **N, double lambda, int xNum, int yNum, double deltaX,
double deltaY)
{
int i_x, i_y; /* counters */
int k, h, m, n, n0; /* counters */
double alpha[5][5]; /* coefficients */
double lambdaX, lambdaY;
/*--------------------------------------*/
lambdaX = lambda * (deltaY / deltaX);
lambdaY = lambda * (deltaX / deltaY);
alpha[0][0] = 0;
alpha[0][1] =
lambdaX *
(1 / 36.); /* There is lambda because Q^(-1) contains 1/(1/lambda) */
alpha[0][2] = lambdaX * (1 / 9.);
alpha[0][3] = lambdaX * (1 / 36.);
alpha[0][4] = 0;
alpha[1][0] = lambdaY * (1 / 36.);
alpha[1][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
alpha[1][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
alpha[1][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
alpha[1][4] = lambdaY * (1 / 36.);
alpha[2][0] = lambdaY * (1 / 9.);
alpha[2][1] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
alpha[2][2] = -lambdaX * (2 / 3.) - lambdaY * (2 / 3.);
alpha[2][3] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
alpha[2][4] = lambdaY * (1 / 9.);
alpha[3][0] = lambdaY * (1 / 36.);
alpha[3][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
alpha[3][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
alpha[3][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
alpha[3][4] = lambdaY * (1 / 36.);
alpha[4][0] = 0;
alpha[4][1] = lambdaX * (1 / 36.);
alpha[4][2] = lambdaX * (1 / 9.);
alpha[4][3] = lambdaX * (1 / 36.);
alpha[4][4] = 0;
for (i_x = 0; i_x < xNum; i_x++) {
for (i_y = 0; i_y < yNum; i_y++) {
for (k = -2; k <= 2; k++) {
for (h = -2; h <= 2; h++) {
if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
((i_y + h) >= 0) && ((i_y + h) < yNum)) {
for (m = k; m <= 2; m++) {
if (m == k)
n0 = h;
else
n0 = -2;
for (n = n0; n <= 2; n++) {
if (((i_x + m) >= 0) &&
((i_x + m) <= (xNum - 1)) &&
((i_y + n) >= 0) &&
((i_y + n) <= (yNum - 1))) {
if ((alpha[k + 2][h + 2] != 0) &&
(alpha[m + 2][n + 2] != 0)) {
N[order(i_x + k, i_y + h, yNum)]
[order(i_x + m, i_y + n, yNum) -
order(i_x + k, i_y + h, yNum)] +=
alpha[k + 2][h + 2] *
alpha[m + 2][n + 2];
}
}
}
}
}
}
}
}
}
return;
}
/*----------------------------------------------------------------------------*/
/* Normal system computation for bilinear spline interpolation */
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)
{
int i, k, h, m, n, n0; /* counters */
double alpha[2][2]; /* coefficients */
int i_x; /* x = (xMin + (i_x * deltaX) + csi_x) */
double csi_x;
int i_y; /* y = (yMin + (i_y * deltaY) + csi_y) */
double csi_y;
/*--------------------------------------*/
for (k = 0; k < parNum; k++) {
for (h = 0; h < BW; h++)
N[k][h] = 0.; /* Normal matrix inizialization */
TN[k] = 0.; /* Normal vector inizialization */
}
/*--------------------------------------*/
for (i = 0; i < obsNum; i++) {
node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);
if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {
csi_x = csi_x / deltaX;
csi_y = csi_y / deltaY;
alpha[0][0] = phi(csi_x, csi_y);
alpha[0][1] = phi(csi_x, 1 - csi_y);
alpha[1][0] = phi(1 - csi_x, csi_y);
alpha[1][1] = phi(1 - csi_x, 1 - csi_y);
for (k = 0; k <= 1; k++) {
for (h = 0; h <= 1; h++) {
if (((i_x + k) >= 0) && ((i_x + k) <= (xNum - 1)) &&
((i_y + h) >= 0) && ((i_y + h) <= (yNum - 1))) {
for (m = k; m <= 1; m++) {
if (m == k)
n0 = h;
else
n0 = 0;
for (n = n0; n <= 1; n++) {
if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
((i_y + n) >= 0) && ((i_y + n) < yNum)) {
N[order(i_x + k, i_y + h, yNum)]
[order(i_x + m, i_y + n, yNum) -
order(i_x + k, i_y + h, yNum)] +=
alpha[k][h] * (1 / Q[i]) * alpha[m][n];
/* 1/Q[i] only refers to the variances */
}
}
}
TN[order(i_x + k, i_y + h, yNum)] +=
obsVect[i][2] * (1 / Q[i]) * alpha[k][h];
}
}
}
}
}
return;
}
/*----------------------------------------------------------------------------*/
/* Normal system correction - Introduzione della correzione dovuta alle
pseudosservazioni (Tykonov) - GRADIENTE - */
#ifdef notdef
void nCorrectGrad(double **N, double lambda, int xNum, int yNum, double deltaX,
double deltaY)
{
int i;
int parNum;
double alpha[3];
double lambdaX, lambdaY;
lambdaX = lambda * (deltaY / deltaX);
lambdaY = lambda * (deltaX / deltaY);
parNum = xNum * yNum;
alpha[0] = lambdaX / 2. + lambdaY / 2.;
alpha[1] = -lambdaX / 4.;
alpha[2] = -lambdaY / 4.;
for (i = 0; i < parNum; i++) {
N[i][0] += alpha[0];
if ((i + 2) < parNum)
N[i][2] += alpha[2];
if ((i + 2 * yNum) < parNum)
N[i][2 * yNum] += alpha[1];
}
}
#endif
/*1-DELTA discretization */
void nCorrectGrad(double **N, double lambda, int xNum, int yNum, double deltaX,
double deltaY)
{
int i;
int parNum;
double alpha[3];
double lambdaX, lambdaY;
lambdaX = lambda * (deltaY / deltaX);
lambdaY = lambda * (deltaX / deltaY);
parNum = xNum * yNum;
alpha[0] = 2 * lambdaX + 2 * lambdaY;
alpha[1] = -lambdaX;
alpha[2] = -lambdaY;
for (i = 0; i < parNum; i++) {
N[i][0] += alpha[0];
if ((i + 1) < parNum)
N[i][1] += alpha[2];
if ((i + 1 * yNum) < parNum)
N[i][1 * yNum] += alpha[1];
}
return;
}
/*----------------------------------------------------------------------------*/
/* Observations estimation */
void obsEstimateBicubic(double **obsV, double *obsE, double *parV, double deltX,
double deltY, int xNm, int yNm, double xMi, double yMi,
int obsN)
{
int i, k, h; /* counters */
double alpha[4][4]; /* coefficients */
int i_x; /* x = (xMin + (i_x * deltaX) + csi_x) */
double csi_x;
int i_y; /* y = (yMin + (i_y * deltaY) + csi_y) */
double csi_y;
for (i = 0; i < obsN; i++) {
obsE[i] = 0;
node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);
if ((i_x >= -2) && (i_x <= xNm) && (i_y >= -2) && (i_y <= yNm)) {
csi_x = csi_x / deltX;
csi_y = csi_y / deltY;
alpha[0][0] = phi_44(1 + csi_x, 1 + csi_y);
alpha[0][1] = phi_43(1 + csi_x, csi_y);
alpha[0][2] = phi_43(1 + csi_x, 1 - csi_y);
alpha[0][3] = phi_44(1 + csi_x, 2 - csi_y);
alpha[1][0] = phi_34(csi_x, 1 + csi_y);
alpha[1][1] = phi_33(csi_x, csi_y);
alpha[1][2] = phi_33(csi_x, 1 - csi_y);
alpha[1][3] = phi_34(csi_x, 2 - csi_y);
alpha[2][0] = phi_34(1 - csi_x, 1 + csi_y);
alpha[2][1] = phi_33(1 - csi_x, csi_y);
alpha[2][2] = phi_33(1 - csi_x, 1 - csi_y);
alpha[2][3] = phi_34(1 - csi_x, 2 - csi_y);
alpha[3][0] = phi_44(2 - csi_x, 1 + csi_y);
alpha[3][1] = phi_43(2 - csi_x, csi_y);
alpha[3][2] = phi_43(2 - csi_x, 1 - csi_y);
alpha[3][3] = phi_44(2 - csi_x, 2 - csi_y);
for (k = -1; k <= 2; k++) {
for (h = -1; h <= 2; h++) {
if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
((i_y + h) >= 0) && ((i_y + h) < yNm))
obsE[i] += parV[order(i_x + k, i_y + h, yNm)] *
alpha[k + 1][h + 1];
}
}
}
}
return;
}
/*--------------------------------------------------------------------------------------*/
/* Data interpolation in a generic point */
double dataInterpolateBicubic(double x, double y, double deltaX, double deltaY,
int xNum, int yNum, double xMin, double yMin,
double *parVect)
{
double z; /* abscissa, ordinate and associated value */
int k, h; /* counters */
double alpha[4][4]; /* coefficients */
int i_x, i_y; /* x = (xMin + (i_x * deltaX) + csi_x) */
double csi_x, csi_y; /* y = (yMin + (i_y * deltaY) + csi_y) */
z = 0;
node_x(x, &i_x, &csi_x, xMin, deltaX);
node_y(y, &i_y, &csi_y, yMin, deltaY);
if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {
csi_x = csi_x / deltaX;
csi_y = csi_y / deltaY;
alpha[0][0] = phi_44(1 + csi_x, 1 + csi_y);
alpha[0][1] = phi_43(1 + csi_x, csi_y);
alpha[0][2] = phi_43(1 + csi_x, 1 - csi_y);
alpha[0][3] = phi_44(1 + csi_x, 2 - csi_y);
alpha[1][0] = phi_34(csi_x, 1 + csi_y);
alpha[1][1] = phi_33(csi_x, csi_y);
alpha[1][2] = phi_33(csi_x, 1 - csi_y);
alpha[1][3] = phi_34(csi_x, 2 - csi_y);
alpha[2][0] = phi_34(1 - csi_x, 1 + csi_y);
alpha[2][1] = phi_33(1 - csi_x, csi_y);
alpha[2][2] = phi_33(1 - csi_x, 1 - csi_y);
alpha[2][3] = phi_34(1 - csi_x, 2 - csi_y);
alpha[3][0] = phi_44(2 - csi_x, 1 + csi_y);
alpha[3][1] = phi_43(2 - csi_x, csi_y);
alpha[3][2] = phi_43(2 - csi_x, 1 - csi_y);
alpha[3][3] = phi_44(2 - csi_x, 2 - csi_y);
for (k = -1; k <= 2; k++) {
for (h = -1; h <= 2; h++) {
if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
((i_y + h) >= 0) && ((i_y + h) < yNum))
z += parVect[order(i_x + k, i_y + h, yNum)] *
alpha[k + 1][h + 1];
}
}
}
return z;
}
/*----------------------------------------------------------------------------*/
/* Observations estimation */
void obsEstimateBilin(double **obsV, double *obsE, double *parV, double deltX,
double deltY, int xNm, int yNm, double xMi, double yMi,
int obsN)
{
int i, k, h; /* counters */
double alpha[2][2]; /* coefficients */
int i_x; /* x = (xMin + (i_x * deltaX) + csi_x) */
double csi_x;
int i_y; /* y = (yMin + (i_y * deltaY) + csi_y) */
double csi_y;
for (i = 0; i < obsN; i++) {
obsE[i] = 0;
node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);
if ((i_x >= -1) && (i_x < xNm) && (i_y >= -1) && (i_y < yNm)) {
csi_x = csi_x / deltX;
csi_y = csi_y / deltY;
alpha[0][0] = phi(csi_x, csi_y);
alpha[0][1] = phi(csi_x, 1 - csi_y);
alpha[1][0] = phi(1 - csi_x, csi_y);
alpha[1][1] = phi(1 - csi_x, 1 - csi_y);
for (k = 0; k <= 1; k++) {
for (h = 0; h <= 1; h++) {
if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
((i_y + h) >= 0) && ((i_y + h) < yNm))
obsE[i] +=
parV[order(i_x + k, i_y + h, yNm)] * alpha[k][h];
}
}
}
}
return;
}
/*--------------------------------------------------------------------------------------*/
/* Data interpolation in a generic point */
double dataInterpolateBilin(double x, double y, double deltaX, double deltaY,
int xNum, int yNum, double xMin, double yMin,
double *parVect)
{
double z; /* abscissa, ordinate and associated value */
int k, h; /* counters */
double alpha[2][2]; /* coefficients */
int i_x, i_y; /* x = (xMin + (i_x * deltaX) + csi_x) */
double csi_x, csi_y; /* y = (yMin + (i_y * deltaY) + csi_y) */
z = 0;
node_x(x, &i_x, &csi_x, xMin, deltaX);
node_y(y, &i_y, &csi_y, yMin, deltaY);
if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {
csi_x = csi_x / deltaX;
csi_y = csi_y / deltaY;
alpha[0][0] = phi(csi_x, csi_y);
alpha[0][1] = phi(csi_x, 1 - csi_y);
alpha[1][0] = phi(1 - csi_x, csi_y);
alpha[1][1] = phi(1 - csi_x, 1 - csi_y);
for (k = 0; k <= 1; k++) {
for (h = 0; h <= 1; h++) {
if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
((i_y + h) >= 0) && ((i_y + h) < yNum))
z += parVect[order(i_x + k, i_y + h, yNum)] * alpha[k][h];
}
}
}
return z;
}
phi_44
double phi_44(double csi_x, double csi_y)
Definition InterpSpline.c:92
phi_4
double phi_4(double csi)
Definition InterpSpline.c:68
dataInterpolateBicubic
double dataInterpolateBicubic(double x, double y, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, double *parVect)
Definition InterpSpline.c:497
order
int order(int i_x, int i_y, int yNum)
Definition InterpSpline.c:53
phi_43
double phi_43(double csi_x, double csi_y)
Definition InterpSpline.c:86
phi_33
double phi_33(double csi_x, double csi_y)
Definition InterpSpline.c:74
normalDefBicubic
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)
Definition InterpSpline.c:107
normalDefBilin
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)
Definition InterpSpline.c:291
node_y
void node_y(double y, int *i_y, double *csi_y, double yMin, double deltaY)
Definition InterpSpline.c:41
obsEstimateBilin
void obsEstimateBilin(double **obsV, double *obsE, double *parV, double deltX, double deltY, int xNm, int yNm, double xMi, double yMi, int obsN)
Definition InterpSpline.c:556
phi_34
double phi_34(double csi_x, double csi_y)
Definition InterpSpline.c:80
phi_3
double phi_3(double csi)
Definition InterpSpline.c:62
dataInterpolateBilin
double dataInterpolateBilin(double x, double y, double deltaX, double deltaY, int xNum, int yNum, double xMin, double yMin, double *parVect)
Definition InterpSpline.c:604
nCorrectGrad
void nCorrectGrad(double **N, double lambda, int xNum, int yNum, double deltaX, double deltaY)
Definition InterpSpline.c:399
node_x
void node_x(double x, int *i_x, double *csi_x, double xMin, double deltaX)
Definition InterpSpline.c:29
phi
double phi(double csi_x, double csi_y)
Definition InterpSpline.c:98
obsEstimateBicubic
void obsEstimateBicubic(double **obsV, double *obsE, double *parV, double deltX, double deltY, int xNm, int yNm, double xMi, double yMi, int obsN)
Definition InterpSpline.c:434
nCorrectLapl
void nCorrectLapl(double **N, double lambda, int xNum, int yNum, double deltaX, double deltaY)
Definition InterpSpline.c:199
x
#define x