int
gsl_linalg_QRPT_svx (const gsl_matrix * QR,
const gsl_vector * tau,
const gsl_permutation * p,
gsl_vector * x)
{
if (QR->size1 != QR->size2)
{
GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
}
else if (QR->size1 != p->size)
{
GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
}
else if (QR->size2 != x->size)
{
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
}
else
{
/* compute sol = Q^T b */
gsl_linalg_QR_QTvec (QR, tau, x);
/* Solve R x = sol, storing x inplace in sol */
gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);
gsl_permute_vector_inverse (p, x);
return GSL_SUCCESS;
}
}
int lsQRPT(gsl_matrix * A, gsl_vector * b, gsl_vector * x, double * sigma)
{
int i;
gsl_vector *tau, *res;
gsl_permutation *p;
gsl_vector_view norm;
if (A->size1 < A->size2) return -1;
if (A->size1 != b->size) return -1;
if (A->size2 != x->size) return -1;
tau = gsl_vector_alloc(x->size);
res = gsl_vector_alloc(b->size);
p = gsl_permutation_alloc(x->size);
norm = gsl_vector_subvector(res, 0, x->size);
gsl_linalg_QRPT_decomp(A, tau, p, &i, &norm.vector);
gsl_linalg_QR_lssolve(A, tau, b, x, res);
gsl_permute_vector_inverse(p, x);
*sigma = gsl_blas_dnrm2(res);
gsl_vector_free(tau);
gsl_vector_free(res);
gsl_permutation_free(p);
return 0;
}
int
gsl_linalg_QRPT_QRsolve (const gsl_matrix * Q, const gsl_matrix * R,
const gsl_permutation * p,
const gsl_vector * b,
gsl_vector * x)
{
if (Q->size1 != Q->size2 || R->size1 != R->size2)
{
return GSL_ENOTSQR;
}
else if (Q->size1 != p->size || Q->size1 != R->size1
|| Q->size1 != b->size)
{
return GSL_EBADLEN;
}
else
{
/* compute b' = Q^T b */
gsl_blas_dgemv (CblasTrans, 1.0, Q, b, 0.0, x);
/* Solve R x = b', storing x inplace */
gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, R, x);
/* Apply permutation to solution in place */
gsl_permute_vector_inverse (p, x);
return GSL_SUCCESS;
}
}
int
gsl_linalg_QRPT_Rsvx (const gsl_matrix * QR,
const gsl_permutation * p,
gsl_vector * x)
{
if (QR->size1 != QR->size2)
{
GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
}
else if (QR->size2 != x->size)
{
GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
}
else if (p->size != x->size)
{
GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
}
else
{
/* Solve R x = b, storing x inplace */
gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);
gsl_permute_vector_inverse (p, x);
return GSL_SUCCESS;
}
}
int
gsl_linalg_PTLQ_svx_T (const gsl_matrix * LQ,
const gsl_vector * tau,
const gsl_permutation * p,
gsl_vector * x)
{
if (LQ->size1 != LQ->size2)
{
GSL_ERROR ("LQ matrix must be square", GSL_ENOTSQR);
}
else if (LQ->size2 != p->size)
{
GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
}
else if (LQ->size1 != x->size)
{
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
}
else
{
/* compute sol = b^T Q^T */
gsl_linalg_LQ_vecQT (LQ, tau, x);
/* Solve L^T x = sol, storing x inplace in sol */
gsl_blas_dtrsv (CblasLower, CblasTrans, CblasNonUnit, LQ, x);
gsl_permute_vector_inverse (p, x);
return GSL_SUCCESS;
}
}
int
gsl_linalg_PTLQ_LQsolve_T (const gsl_matrix * Q, const gsl_matrix * L,
const gsl_permutation * p,
const gsl_vector * b,
gsl_vector * x)
{
if (Q->size1 != Q->size2 || L->size1 != L->size2)
{
return GSL_ENOTSQR;
}
else if (Q->size1 != p->size || Q->size1 != L->size1
|| Q->size1 != b->size)
{
return GSL_EBADLEN;
}
else
{
/* compute b' = Q b */
gsl_blas_dgemv (CblasNoTrans, 1.0, Q, b, 0.0, x);
/* Solve L^T x = b', storing x inplace */
gsl_blas_dtrsv (CblasLower, CblasTrans, CblasNonUnit, L, x);
/* Apply permutation to solution in place */
gsl_permute_vector_inverse (p, x);
return GSL_SUCCESS;
}
}
void Vector::permute ( const Permutation& p, const bool inverse ) {
if ( inverse ) {
gsl_permute_vector_inverse( &p, &this->vector );
} else {
gsl_permute_vector( &p, &this->vector );
}
}
int
gsl_linalg_PTLQ_Lsvx_T (const gsl_matrix * LQ,
const gsl_permutation * p,
gsl_vector * x)
{
if (LQ->size1 != LQ->size2)
{
GSL_ERROR ("LQ matrix must be square", GSL_ENOTSQR);
}
else if (LQ->size2 != x->size)
{
GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
}
else if (p->size != x->size)
{
GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
}
else
{
/* Solve L^T x = b, storing x inplace */
gsl_blas_dtrsv (CblasLower, CblasTrans, CblasNonUnit, LQ, x);
gsl_permute_vector_inverse (p, x);
return GSL_SUCCESS;
}
}
int
gsl_linalg_COD_lssolve (const gsl_matrix * QRZ, const gsl_vector * tau_Q, const gsl_vector * tau_Z,
const gsl_permutation * perm, const size_t rank, const gsl_vector * b,
gsl_vector * x, gsl_vector * residual)
{
const size_t M = QRZ->size1;
const size_t N = QRZ->size2;
if (M < N)
{
GSL_ERROR ("QRZ matrix must have M>=N", GSL_EBADLEN);
}
else if (M != b->size)
{
GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
}
else if (rank > GSL_MIN (M, N))
{
GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
}
else if (N != x->size)
{
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
}
else if (M != residual->size)
{
GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
}
else
{
gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QRZ, 0, 0, rank, rank);
gsl_vector_view QTb1 = gsl_vector_subvector(residual, 0, rank);
gsl_vector_view x1 = gsl_vector_subvector(x, 0, rank);
gsl_vector_set_zero(x);
/* compute residual = Q^T b */
gsl_vector_memcpy(residual, b);
gsl_linalg_QR_QTvec (QRZ, tau_Q, residual);
/* solve x1 := R11^{-1} (Q^T b)(1:r) */
gsl_vector_memcpy(&(x1.vector), &(QTb1.vector));
gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), &(x1.vector));
/* compute Z^T ( R11^{-1} x1; 0 ) */
cod_householder_ZTvec(QRZ, tau_Z, rank, x);
/* compute x = P Z^T ( R11^{-1} x1; 0 ) */
gsl_permute_vector_inverse(perm, x);
/* compute residual = b - A x = Q (Q^T b - R [ R11^{-1} x1; 0 ]) */
gsl_vector_set_zero(&(QTb1.vector));
gsl_linalg_QR_Qvec(QRZ, tau_Q, residual);
return GSL_SUCCESS;
}
}
int
gsl_linalg_QRPT_lssolve2 (const gsl_matrix * QR, const gsl_vector * tau, const gsl_permutation * p,
const gsl_vector * b, const size_t rank, gsl_vector * x, gsl_vector * residual)
{
const size_t M = QR->size1;
const size_t N = QR->size2;
if (M < N)
{
GSL_ERROR ("QR matrix must have M>=N", GSL_EBADLEN);
}
else if (M != b->size)
{
GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
}
else if (rank == 0 || rank > N)
{
GSL_ERROR ("rank must have 0 < rank <= N", GSL_EBADLEN);
}
else if (N != x->size)
{
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
}
else if (M != residual->size)
{
GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
}
else
{
gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QR, 0, 0, rank, rank);
gsl_vector_view QTb1 = gsl_vector_subvector(residual, 0, rank);
gsl_vector_view x1 = gsl_vector_subvector(x, 0, rank);
size_t i;
/* compute work = Q^T b */
gsl_vector_memcpy(residual, b);
gsl_linalg_QR_QTvec (QR, tau, residual);
/* solve R_{11} x(1:r) = [Q^T b](1:r) */
gsl_vector_memcpy(&(x1.vector), &(QTb1.vector));
gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), &(x1.vector));
/* x(r+1:N) = 0 */
for (i = rank; i < N; ++i)
gsl_vector_set(x, i, 0.0);
/* compute x = P y */
gsl_permute_vector_inverse (p, x);
/* compute residual = b - A x = Q (Q^T b - R x) */
gsl_vector_set_zero(&(QTb1.vector));
gsl_linalg_QR_Qvec(QR, tau, residual);
return GSL_SUCCESS;
}
}
static double
cholesky_LDLT_norm1(const gsl_matrix * LDLT, const gsl_permutation * p, gsl_vector * work)
{
const size_t N = LDLT->size1;
gsl_vector_const_view D = gsl_matrix_const_diagonal(LDLT);
gsl_vector_view diagA = gsl_vector_subvector(work, N, N);
double max = 0.0;
size_t i, j;
/* reconstruct diagonal entries of original matrix A */
for (j = 0; j < N; ++j)
{
double Ajj;
/* compute diagonal (j,j) entry of A */
Ajj = gsl_vector_get(&D.vector, j);
for (i = 0; i < j; ++i)
{
double Di = gsl_vector_get(&D.vector, i);
double Lji = gsl_matrix_get(LDLT, j, i);
Ajj += Di * Lji * Lji;
}
gsl_vector_set(&diagA.vector, j, Ajj);
}
gsl_permute_vector_inverse(p, &diagA.vector);
for (j = 0; j < N; ++j)
{
double sum = 0.0;
double Ajj = gsl_vector_get(&diagA.vector, j);
for (i = 0; i < j; ++i)
{
double *wi = gsl_vector_ptr(work, i);
double Aij = gsl_matrix_get(LDLT, i, j);
double absAij = fabs(Aij);
sum += absAij;
*wi += absAij;
}
gsl_vector_set(work, j, sum + fabs(Ajj));
}
for (i = 0; i < N; ++i)
{
double wi = gsl_vector_get(work, i);
max = GSL_MAX(max, wi);
}
return max;
}
static VALUE rb_gsl_vector_permute_inverse(VALUE obj, VALUE pp)
{
gsl_permutation *p = NULL;
gsl_vector *v = NULL;
int status;
CHECK_PERMUTATION(pp);
Data_Get_Struct(pp, gsl_permutation, p);
Data_Get_Struct(obj, gsl_vector, v);
status = gsl_permute_vector_inverse(p, v);
return INT2FIX(status);
}
/* singleton */
static VALUE rb_gsl_permute_vector_inverse(VALUE obj, VALUE pp, VALUE vv)
{
gsl_permutation *p = NULL;
gsl_vector *v;
int status;
CHECK_VECTOR(vv);
Data_Get_Struct(pp, gsl_permutation, p);
Data_Get_Struct(vv, gsl_vector, v);
status = gsl_permute_vector_inverse(p, v);
return INT2FIX(status);
}
int
gsl_linalg_pcholesky_svx(const gsl_matrix * LDLT,
const gsl_permutation * p,
gsl_vector * x)
{
if (LDLT->size1 != LDLT->size2)
{
GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
}
else if (LDLT->size1 != p->size)
{
GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
}
else if (LDLT->size2 != x->size)
{
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
}
else
{
gsl_vector_const_view D = gsl_matrix_const_diagonal(LDLT);
/* x := P b */
gsl_permute_vector(p, x);
/* solve: L w = P b */
gsl_blas_dtrsv(CblasLower, CblasNoTrans, CblasUnit, LDLT, x);
/* solve: D y = w */
gsl_vector_div(x, &D.vector);
/* solve: L^T z = y */
gsl_blas_dtrsv(CblasLower, CblasTrans, CblasUnit, LDLT, x);
/* compute: x = P^T z */
gsl_permute_vector_inverse(p, x);
return GSL_SUCCESS;
}
}
/**
* C++ version of gsl_permute_vector_inverse().
* @param p A permutation
* @param v A vector
* @return Error code on failure
*/
inline int vector_inverse( permutation const& p, gsl::vector& v ){
return gsl_permute_vector_inverse( p.get(), v.get() ); }
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;
}
int
gsl_linalg_pcholesky_invert(const gsl_matrix * LDLT, const gsl_permutation * p,
gsl_matrix * Ainv)
{
const size_t M = LDLT->size1;
const size_t N = LDLT->size2;
if (M != N)
{
GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
}
else if (LDLT->size1 != p->size)
{
GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
}
else if (Ainv->size1 != Ainv->size2)
{
GSL_ERROR ("Ainv matrix must be square", GSL_ENOTSQR);
}
else if (Ainv->size1 != M)
{
GSL_ERROR ("Ainv matrix has wrong dimensions", GSL_EBADLEN);
}
else
{
size_t i, j;
gsl_vector_view v1, v2;
/* invert the lower triangle of LDLT */
gsl_matrix_memcpy(Ainv, LDLT);
gsl_linalg_tri_lower_unit_invert(Ainv);
/* compute sqrt(D^{-1}) L^{-1} in the lower triangle of Ainv */
for (i = 0; i < N; ++i)
{
double di = gsl_matrix_get(LDLT, i, i);
double sqrt_di = sqrt(di);
for (j = 0; j < i; ++j)
{
double *Lij = gsl_matrix_ptr(Ainv, i, j);
*Lij /= sqrt_di;
}
gsl_matrix_set(Ainv, i, i, 1.0 / sqrt_di);
}
/*
* The lower triangle of Ainv now contains D^{-1/2} L^{-1}. Now compute
* A^{-1} = L^{-T} D^{-1} L^{-1}
*/
for (i = 0; i < N; ++i)
{
double aii = gsl_matrix_get(Ainv, i, i);
if (i < N - 1)
{
double tmp;
v1 = gsl_matrix_subcolumn(Ainv, i, i, N - i);
gsl_blas_ddot(&v1.vector, &v1.vector, &tmp);
gsl_matrix_set(Ainv, i, i, tmp);
if (i > 0)
{
gsl_matrix_view m = gsl_matrix_submatrix(Ainv, i + 1, 0, N - i - 1, i);
v1 = gsl_matrix_subcolumn(Ainv, i, i + 1, N - i - 1);
v2 = gsl_matrix_subrow(Ainv, i, 0, i);
gsl_blas_dgemv(CblasTrans, 1.0, &m.matrix, &v1.vector, aii, &v2.vector);
}
}
else
{
v1 = gsl_matrix_row(Ainv, N - 1);
gsl_blas_dscal(aii, &v1.vector);
}
}
/* copy lower triangle to upper */
gsl_matrix_transpose_tricpy('L', 0, Ainv, Ainv);
/* now apply permutation p to the matrix */
/* compute L^{-T} D^{-1} L^{-1} P^T */
for (i = 0; i < N; ++i)
{
v1 = gsl_matrix_row(Ainv, i);
gsl_permute_vector_inverse(p, &v1.vector);
}
/* compute P L^{-T} D^{-1} L^{-1} P^T */
for (i = 0; i < N; ++i)
{
v1 = gsl_matrix_column(Ainv, i);
gsl_permute_vector_inverse(p, &v1.vector);
}
return GSL_SUCCESS;
}
}
