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