CAMLprim value ml_gsl_linalg_LU_svx(value LU, value P, value X)
{
GSL_PERMUT_OF_BIGARRAY(P);
_DECLARE_MATRIX(LU);
_DECLARE_VECTOR(X);
_CONVERT_MATRIX(LU);
_CONVERT_VECTOR(X);
gsl_linalg_LU_svx(&m_LU, &perm_P, &v_X);
return Val_unit;
}
/**
* C++ version of gsl_linalg_LU_svx().
* @param LU An LU decomposition matrix
* @param p A permutation
* @param x A vector
* @return Error code on failure
*/
inline int LU_svx( matrix const& LU, permutation const& p, vector& x ){
return gsl_linalg_LU_svx( LU.get(), p.get(), x.get() ); }
/* Function: TPS
*
* Description: Calculate Thin Plate Spline (TPS) weights from control points
* and build a new height grid by interpolating with them. Code taken and
* modified from http://elonen.iki.fi/code/tpsdemo/index.html
*
* Parameters:
* features3DPrime: feature points to serve as control for TPS calculation
* numPoints: number of feature points
* grid: 2D double array to store output of the TPS calculations. The rows
* represent x coordinate values, columns represent y coordinate values,
* and values of the array represent the z coordinate values
* gridHeight: height of the grid
* gridWidth: width of the grid
* gridHeightStartIndex: The grid row and column indices start at 0 because
* it is an array. However, the actual x and y start values do not
* necessarily start 0, so gridHeightStartIndex represents the value at
* which the y coordinates start at in the grid.
* gridWidthStartIndex: the value at which the x coordinates start at in the
* grid
* regularization: smoothing parameter that is the ratio of the error variance
* to the scale parameter of the covariance function
*
* Returns: 0 on success, error code on error.
*/
int TPS(
_In_ CvPoint3D32f *features3DPrime,
_In_ int numPoints,
_Out_ double **grid,
_In_ int gridHeight,
_In_ int gridWidth,
_In_ int gridHeightStartIndex,
_In_ int gridWidthStartIndex,
_In_ double regularization)
{
// We need at least 3 points to define a plane
if (numPoints < 3)
{
return NOT_ENOUGH_POINTS_ERROR;
}
// Allocate the matrix and vector
gsl_matrix *L = gsl_matrix_alloc(numPoints+3, numPoints+3);
gsl_vector *V = gsl_vector_alloc(numPoints+3);
if (L == NULL || V == NULL)
{
return OUT_OF_MEMORY_ERROR;
}
// Fill K (p x p, upper left of L) and calculate mean edge length from control
// points. K is symmetrical so we really have to calculate only about half
// of the coefficients.
double a = 0.0;
for (int i = 0; i < numPoints; i++)
{
CvPoint3D32f iPoint = features3DPrime[i];
for (int j = i+1; j < numPoints; j++)
{
CvPoint3D32f jPoint = features3DPrime[j];
iPoint.z = 0.0;
jPoint.z = 0.0;
double edgeLength = sqrt(pow(iPoint.x-jPoint.x, 2) + pow(iPoint.y-jPoint.y, 2));
gsl_matrix_set(L, i, j, TPSBase(edgeLength));
gsl_matrix_set(L, j, i, TPSBase(edgeLength));
a += edgeLength * 2;
}
}
a /= (double)(numPoints * numPoints);
// Fill the rest of L
for (int i = 0; i < numPoints; i++)
{
// diagonal: reqularization parameters (lambda * a^2)
gsl_matrix_set(L, i, i, regularization * a * a);
// P (p x 3, upper right)
gsl_matrix_set(L, i, numPoints, 1.0);
gsl_matrix_set(L, i, numPoints+1, features3DPrime[i].x);
gsl_matrix_set(L, i, numPoints+2, features3DPrime[i].y);
// P transposed (3 x p, bottom left)
gsl_matrix_set(L, numPoints, i, 1.0);
gsl_matrix_set(L, numPoints+1, i, features3DPrime[i].x);
gsl_matrix_set(L, numPoints+2, i, features3DPrime[i].y);
}
// O (3 x 3, lower right)
for (int i = numPoints; i < numPoints+3; i++)
{
for (int j = numPoints; j < numPoints+3; j++)
{
gsl_matrix_set(L, i, j, 0.0);
}
}
// Fill the right hand vector V
for (int i = 0; i < numPoints; i++)
{
gsl_vector_set(V, i, features3DPrime[i].z);
}
gsl_vector_set(V, numPoints, 0.0);
gsl_vector_set(V, numPoints+1, 0.0);
gsl_vector_set(V, numPoints+2, 0.0);
int signum;
gsl_permutation *perm = gsl_permutation_alloc(numPoints+3);
if (perm == NULL)
{
return OUT_OF_MEMORY_ERROR;
}
// Solve the linear system inplace
gsl_linalg_LU_decomp(L, perm, &signum);
gsl_linalg_LU_svx(L, perm, V);
gsl_permutation_free(perm);
// Interpolate grid heights
for (int x = gridWidthStartIndex; x < gridWidthStartIndex + gridWidth; x++)
{
for (int y = gridHeightStartIndex; y < gridHeightStartIndex + gridHeight; y++ )
{
double h = gsl_vector_get(V, numPoints) + gsl_vector_get(V, numPoints+1)*x + gsl_vector_get(V, numPoints+2)*y;
for (int i = 0; i < numPoints; i++)
{
CvPoint3D32f iPoint = features3DPrime[i];
CvPoint3D32f curPoint = cvPoint3D32f((double)x, (double)y, 0.0);
iPoint.z = 0;
h += gsl_vector_get(V, i) * TPSBase(sqrt(pow(iPoint.x-curPoint.x, 2) + pow(iPoint.y-curPoint.y, 2) + pow(iPoint.z-curPoint.z, 2)));
}
grid[x - gridWidthStartIndex][y - gridHeightStartIndex] = h;
}
}
gsl_matrix_free(L);
gsl_vector_free(V);
return 0;
}
