int GMRFLib_gsl_spd_inverse(gsl_matrix * A) { /* * replace SPD matrix A with its inverse */ gsl_matrix *L; gsl_vector *x; size_t i, n; assert(A->size1 == A->size2); n = A->size1; x = gsl_vector_calloc(n); L = GMRFLib_gsl_duplicate_matrix(A); gsl_linalg_cholesky_decomp(L); for (i = 0; i < n; i++) { gsl_vector_set_basis(x, i); gsl_linalg_cholesky_svx(L, x); gsl_matrix_set_col(A, i, x); } gsl_vector_free(x); gsl_matrix_free(L); return GMRFLib_SUCCESS; }
int lseShurComplement(gsl_matrix * A, gsl_matrix * C, gsl_vector * b, gsl_vector * d, gsl_vector * x, gsl_vector * lambda, double * sigma) { int i; double xi; gsl_vector *c0, *S, *tau; gsl_matrix *CT, *U; gsl_permutation *perm; gsl_vector_view row, cp; gsl_matrix_view R; if (A->size2 != C->size2) return -1; if (A->size2 != x->size) return -1; if (A->size1 < A->size2) return -1; if (b != NULL && A->size1 != b->size) return -1; if (C->size1 != d->size) return -1; if (C->size1 != lambda->size) return -1; c0 = gsl_vector_alloc(x->size); gsl_matrix_get_row(c0, C, 0); /* Cholesky factorization of A^T A = R^T R via QRPT decomposition */ perm = gsl_permutation_alloc(x->size); tau = gsl_vector_alloc(x->size); gsl_linalg_QRPT_decomp(A, tau, perm, &i, x); /* cp = R^{-T} P A^T b = Q^T b */ if (b != NULL) { gsl_linalg_QR_QTvec(A, tau, b); cp = gsl_vector_subvector(b, 0, x->size); } gsl_vector_free(tau); /* C P -> C */ R = gsl_matrix_submatrix(A, 0, 0, A->size2, A->size2); for (i = 0; i < C->size1; ++i) { row = gsl_matrix_row(C, i); gsl_permute_vector(perm, &row.vector); } /* Compute C inv(R) -> C */ gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, &R.matrix, C); /* The Schur complement D = C C^T, Compute SVD of D = U S^2 U^T by SVD of C^T = V S U^T */ CT = gsl_matrix_alloc(C->size2, C->size1); gsl_matrix_transpose_memcpy(CT, C); U = gsl_matrix_alloc(CT->size2, CT->size2); S = gsl_vector_alloc(CT->size2); gsl_linalg_SV_decomp(CT, U, S, lambda); /* Right hand side of the Shur complement system d - C (A^T A)^-1 A^T b = d - C cp -> d (with C P R^-1 -> C and R^-T P^T A^T b -> cp) */ if (b != NULL) { gsl_blas_dgemv(CblasNoTrans, -1.0, C, &cp.vector, 1.0, d); } /* Calculate S U^T lambda, where -lambda is the Lagrange multiplier */ gsl_blas_dgemv(CblasTrans, 1.0, U, d, 0.0, lambda); gsl_vector_div(lambda, S); /* Calculate sigma = || A x ||_2 = || x ||_2 (before inv(R) x -> x) */ *sigma = gsl_blas_dnrm2(lambda); /* Compute inv(R)^T C^T lambda = C^T lambda (with C inv(R) ->C) */ gsl_blas_dgemv(CblasNoTrans, 1.0, CT, lambda, 0.0, x); /* x = inv(A^T A) C^T lambda = inv(R) [inv(R)^T C^T lambda] */ if (R.matrix.data[R.matrix.size1 * R.matrix.size2 - 1] != 0.0) { gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &R.matrix, x); } else { /* Special case when A is singular */ gsl_vector_set_basis(x, x->size - 1); *sigma = 0.0; } /* Permute back, 1-step iterative refinement on first constraint */ gsl_permute_vector_inverse(perm, x); gsl_blas_ddot(x, c0, &xi); gsl_vector_scale(x, d->data[0] / xi); /* get the real lambda from S U^T lambda previously stored in lambda */ gsl_vector_div(lambda, S); gsl_vector_memcpy(S, lambda); gsl_blas_dgemv(CblasNoTrans, 1.0, U, S, 0.0, lambda); gsl_vector_free(c0); gsl_vector_free(S); gsl_matrix_free(U); gsl_matrix_free(CT); gsl_permutation_free(perm); return 0; }