/*assumes we have a "coded" QR decomposition (a,tau) of the original matrix*/
inline void qr_decomp(double* a, double* tau, double* q, double* r,int m,int n){
gsl_matrix_view qv=gsl_matrix_view_array(q,m,m);
gsl_matrix_view rv=gsl_matrix_view_array(r,m,n);
gsl_matrix_view av=gsl_matrix_view_array(a,m,n);
int d;
if (m<n) d=m; else d=n;
gsl_vector_view tv=gsl_vector_view_array(tau,d);
gsl_linalg_QR_unpack(&av.matrix,&tv.vector,&qv.matrix,&rv.matrix);
}
CAMLprim value ml_gsl_linalg_QR_unpack(value QR, value TAU, value Q, value R)
{
_DECLARE_MATRIX3(QR, Q, R);
_DECLARE_VECTOR(TAU);
_CONVERT_MATRIX3(QR, Q, R);
_CONVERT_VECTOR(TAU);
gsl_linalg_QR_unpack(&m_QR, &v_TAU, &m_Q, &m_R);
return Val_unit;
}
/* compute QR factorization
M is mxn; Q is mxm and R is mxn
this is slow
*/
void compute_QR_factorization(gsl_matrix *M, gsl_matrix *Q, gsl_matrix *R){
//printf("QR setup..\n");
gsl_matrix *QR = gsl_matrix_calloc(M->size1, M->size2);
gsl_vector *tau = gsl_vector_alloc(min(M->size1,M->size2));
gsl_matrix_memcpy (QR, M);
//printf("QR decomp..\n");
gsl_linalg_QR_decomp (QR, tau);
//printf("QR unpack..\n");
gsl_linalg_QR_unpack (QR, tau, Q, R);
//printf("done QR..\n");
}
/* generate random square orthogonal matrix via QR decomposition */
static void
test_random_matrix_orth(gsl_matrix *m, const gsl_rng *r)
{
const size_t M = m->size1;
gsl_matrix *A = gsl_matrix_alloc(M, M);
gsl_vector *tau = gsl_vector_alloc(M);
gsl_matrix *R = gsl_matrix_alloc(M, M);
test_random_matrix(A, r, -1.0, 1.0);
gsl_linalg_QR_decomp(A, tau);
gsl_linalg_QR_unpack(A, tau, m, R);
gsl_matrix_free(A);
gsl_matrix_free(R);
gsl_vector_free(tau);
}
static int
set (void *vstate, gsl_multiroot_function * func, gsl_vector * x, gsl_vector * f, gsl_vector * dx, int scale)
{
hybrid_state_t *state = (hybrid_state_t *) vstate;
gsl_matrix *J = state->J;
gsl_matrix *q = state->q;
gsl_matrix *r = state->r;
gsl_vector *tau = state->tau;
gsl_vector *diag = state->diag;
GSL_MULTIROOT_FN_EVAL (func, x, f);
gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J) ;
state->iter = 1;
state->fnorm = enorm (f);
state->ncfail = 0;
state->ncsuc = 0;
state->nslow1 = 0;
state->nslow2 = 0;
gsl_vector_set_all (dx, 0.0);
/* Store column norms in diag */
if (scale)
compute_diag (J, diag);
else
gsl_vector_set_all (diag, 1.0);
/* Set delta to factor |D x| or to factor if |D x| is zero */
state->delta = compute_delta (diag, x);
/* Factorize J into QR decomposition */
gsl_linalg_QR_decomp (J, tau);
gsl_linalg_QR_unpack (J, tau, q, r);
return GSL_SUCCESS;
}
static int
md_qr(lua_State *L) /* (-1,+2,e) */
{
mMatReal *m = qlua_checkMatReal(L, 1);
mMatReal *qr = qlua_newMatReal(L, m->l_size, m->r_size);
mMatReal *q = qlua_newMatReal(L, m->l_size, m->l_size);
mMatReal *r = qlua_newMatReal(L, m->l_size, m->r_size);
int nm = m->l_size < m->r_size? m->l_size: m->r_size;
gsl_vector *tau;
gsl_matrix_memcpy(qr->m, m->m);
tau = new_gsl_vector(L, nm);
if (gsl_linalg_QR_decomp(qr->m, tau))
luaL_error(L, "matrix:qr() failed");
if (gsl_linalg_QR_unpack(qr->m, tau, q->m, r->m))
luaL_error(L, "matrix:qr() failed");
gsl_vector_free(tau);
return 2;
}
int
gsl_linalg_QRPT_decomp2 (const gsl_matrix * A, gsl_matrix * q, gsl_matrix * r, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
const size_t M = A->size1;
const size_t N = A->size2;
if (q->size1 != M || q->size2 !=M)
{
GSL_ERROR ("q must be M x M", GSL_EBADLEN);
}
else if (r->size1 != M || r->size2 !=N)
{
GSL_ERROR ("r must be M x N", GSL_EBADLEN);
}
else if (tau->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
}
else if (p->size != N)
{
GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
}
else if (norm->size != N)
{
GSL_ERROR ("norm size must be N", GSL_EBADLEN);
}
gsl_matrix_memcpy (r, A);
gsl_linalg_QRPT_decomp (r, tau, p, signum, norm);
/* FIXME: aliased arguments depends on behavior of unpack routine! */
gsl_linalg_QR_unpack (r, tau, q, r);
return GSL_SUCCESS;
}
/**
* C++ version of gsl_linalg_QR_unpack().
* @param QR A QR decomposition matrix
* @param tau A vector
* @param Q A matrix
* @param R A matrix
* @return Error code on failure
*/
inline int QR_unpack( matrix const& QR, vector const& tau, matrix& Q, matrix& R ){
return gsl_linalg_QR_unpack( QR.get(), tau.get(), Q.get(), R.get() ); }
static int
iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x, gsl_vector * f, gsl_vector * dx, int scale)
{
hybrid_state_t *state = (hybrid_state_t *) vstate;
const double fnorm = state->fnorm;
gsl_matrix *J = state->J;
gsl_matrix *q = state->q;
gsl_matrix *r = state->r;
gsl_vector *tau = state->tau;
gsl_vector *diag = state->diag;
gsl_vector *qtf = state->qtf;
gsl_vector *x_trial = state->x_trial;
gsl_vector *f_trial = state->f_trial;
gsl_vector *df = state->df;
gsl_vector *qtdf = state->qtdf;
gsl_vector *rdx = state->rdx;
gsl_vector *w = state->w;
gsl_vector *v = state->v;
double prered, actred;
double pnorm, fnorm1, fnorm1p;
double ratio;
double p1 = 0.1, p5 = 0.5, p001 = 0.001, p0001 = 0.0001;
/* Compute qtf = Q^T f */
compute_qtf (q, f, qtf);
/* Compute dogleg step */
dogleg (r, qtf, diag, state->delta, state->newton, state->gradient, dx);
/* Take a trial step */
compute_trial_step (x, dx, state->x_trial);
pnorm = scaled_enorm (diag, dx);
if (state->iter == 1)
{
if (pnorm < state->delta)
{
state->delta = pnorm;
}
}
/* Evaluate function at x + p */
{
int status = GSL_MULTIROOT_FN_EVAL (func, x_trial, f_trial);
if (status != GSL_SUCCESS)
{
return GSL_EBADFUNC;
}
}
/* Set df = f_trial - f */
compute_df (f_trial, f, df);
/* Compute the scaled actual reduction */
fnorm1 = enorm (f_trial);
actred = compute_actual_reduction (fnorm, fnorm1);
/* Compute rdx = R dx */
compute_rdx (r, dx, rdx);
/* Compute the scaled predicted reduction phi1p = |Q^T f + R dx| */
fnorm1p = enorm_sum (qtf, rdx);
prered = compute_predicted_reduction (fnorm, fnorm1p);
/* Compute the ratio of the actual to predicted reduction */
if (prered > 0)
{
ratio = actred / prered;
}
else
{
ratio = 0;
}
/* Update the step bound */
if (ratio < p1)
{
state->ncsuc = 0;
state->ncfail++;
state->delta *= p5;
}
else
{
state->ncfail = 0;
state->ncsuc++;
if (ratio >= p5 || state->ncsuc > 1)
state->delta = GSL_MAX (state->delta, pnorm / p5);
if (fabs (ratio - 1) <= p1)
state->delta = pnorm / p5;
}
/* Test for successful iteration */
if (ratio >= p0001)
{
gsl_vector_memcpy (x, x_trial);
gsl_vector_memcpy (f, f_trial);
state->fnorm = fnorm1;
state->iter++;
}
/* Determine the progress of the iteration */
state->nslow1++;
if (actred >= p001)
state->nslow1 = 0;
if (actred >= p1)
state->nslow2 = 0;
if (state->ncfail == 2)
{
gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J) ;
state->nslow2++;
if (state->iter == 1)
{
if (scale)
compute_diag (J, diag);
state->delta = compute_delta (diag, x);
}
else
{
if (scale)
update_diag (J, diag);
}
/* Factorize J into QR decomposition */
gsl_linalg_QR_decomp (J, tau);
gsl_linalg_QR_unpack (J, tau, q, r);
return GSL_SUCCESS;
}
/* Compute qtdf = Q^T df, w = (Q^T df - R dx)/|dx|, v = D^2 dx/|dx| */
compute_qtf (q, df, qtdf);
compute_wv (qtdf, rdx, dx, diag, pnorm, w, v);
/* Rank-1 update of the jacobian Q'R' = Q(R + w v^T) */
gsl_linalg_QR_update (q, r, w, v);
/* No progress as measured by jacobian evaluations */
if (state->nslow2 == 5)
{
return GSL_ENOPROGJ;
}
/* No progress as measured by function evaluations */
if (state->nslow1 == 10)
{
return GSL_ENOPROG;
}
return GSL_SUCCESS;
}
void Module_DLT::rq_decomp(double* solucion,
gsl_matrix* R_prima,
gsl_matrix* Q_prima,
gsl_vector* x
){
/*
int i, j, lotkin_signum, frank_signum;
int DIM = 3;
gsl_matrix *lotkin_a, *frank_a;
gsl_vector *x, *lotkin_b, *frank_b, *lotkin_x, *frank_x;
gsl_vector *lotkin_tau, *frank_tau;
/* allocate a, x, b
lotkin_a = gsl_matrix_alloc(DIM, DIM);
frank_a = gsl_matrix_alloc(DIM, DIM);
x = gsl_vector_alloc(DIM);
lotkin_b = gsl_vector_alloc(DIM);
frank_b = gsl_vector_alloc(DIM);
lotkin_x = gsl_vector_alloc(DIM);
frank_x = gsl_vector_alloc(DIM);
/* set x = [1 2 ... DIM]
for(i = 0; i < DIM; i++)
gsl_vector_set(x, i, (double)i);
/* set Lotkin matrix */
/* a_ij = 1 (i = 1) or 1/(i+j-1) (i != 1)
for(i = 0; i < DIM; i++)
gsl_matrix_set(lotkin_a, 0, i, 1.0);
for(i = 1; i < DIM; i++)
for(j = 0; j < DIM; j++)
gsl_matrix_set(lotkin_a, i, j, 1.0 / (double)(i + j + 1));
/* set Frank matrix
/* a_ij = DIM - min(i,j) + 1
for(i = 0; i < DIM; i++)
for(j = 0; j < DIM; j++)
gsl_matrix_set(frank_a, i, j, (double)DIM - (double)GSL_MAX(i, j) );
*/
/* set A matrix
gsl_matrix_set(lotkin_a, 0, 0, 12);
gsl_matrix_set(lotkin_a, 0, 1, 6);
gsl_matrix_set(lotkin_a, 0, 2, -4);
gsl_matrix_set(lotkin_a, 1, 0, -51);
gsl_matrix_set(lotkin_a, 1, 1, 167);
gsl_matrix_set(lotkin_a, 1, 2, 24);
gsl_matrix_set(lotkin_a, 2, 0, 4);
gsl_matrix_set(lotkin_a, 2, 1, -68);
gsl_matrix_set(lotkin_a, 2, 2, -41);
/* Print matrix
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
printf("%g ", gsl_matrix_get(lotkin_a, i, j));
printf("\n");
}
printf("\n");
/* b = A * x
gsl_blas_dgemv(CblasNoTrans, 1.0, lotkin_a, x, 0.0, lotkin_b);
/* QR decomposition and solve
lotkin_tau = gsl_vector_alloc(DIM);
gsl_linalg_QR_decomp(lotkin_a, lotkin_tau);
gsl_linalg_QR_solve(lotkin_a, lotkin_tau, lotkin_b, lotkin_x);
gsl_vector_free(lotkin_tau);
/* Print solution matrix
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
printf("%g ", gsl_matrix_get(lotkin_a, i, j));
printf("\n");
}
printf("\n");
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
//printf("%g ", gsl_vector_get(lotkin_x, i, j));
printf("\n");
}
/* free a, x, b
gsl_matrix_free(lotkin_a);
gsl_vector_free(x);
gsl_vector_free(lotkin_b);
gsl_vector_free(lotkin_x);
*/
/*
gsl_matrix* C = gsl_matrix_alloc(3,3);
/* Compute C = A B
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, R_prima, Q_prima,
0.0, C);
camera->rt11 = gsl_matrix_get(C, 0, 0);
camera->rt12 = gsl_matrix_get(C, 0, 1);
camera->rt13 = gsl_matrix_get(C, 0, 2);
camera->rt21 = gsl_matrix_get(C, 1, 0);
camera->rt22 = gsl_matrix_get(C, 1, 1);
camera->rt23 = gsl_matrix_get(C, 1, 2);
camera->rt31 = gsl_matrix_get(C, 2, 0);
camera->rt32 = gsl_matrix_get(C, 2, 1);
camera->rt33 = gsl_matrix_get(C, 2, 2);
camera->rt41 = 0;
camera->rt42 = 0;
camera->rt43 = 0;
camera->rt44 = 1;
**/
std::cout << "RQ_Decomp" << std::endl;
int n,mm,s,signum ;
gsl_matrix *M,*Q,*R;
gsl_vector* tau;
double tmp,det;
/* para invertir las matriz M,Q,R */
gsl_permutation* p = gsl_permutation_alloc (3);
gsl_permutation* p2 = gsl_permutation_alloc (3);
gsl_permutation* p3 = gsl_permutation_alloc (3);
gsl_matrix* M_prima = gsl_matrix_alloc(3,3);
gsl_matrix* Q_prima_tmp = gsl_matrix_alloc(3,3);
/* para resolver el centro de la camara usando Mx=C
donde C es el verctor p4 de la matriz P */
gsl_vector* p4 = gsl_vector_alloc(3);
gsl_matrix* temp = gsl_matrix_alloc(3,3);
gsl_matrix* I_C = gsl_matrix_alloc(3,4);
gsl_matrix* test = gsl_matrix_alloc(3,4);
M = gsl_matrix_alloc(3,3);
Q = gsl_matrix_alloc(3,3);
R = gsl_matrix_alloc(3,3);
tau = gsl_vector_alloc(3);
/* Copiamos la submatriz 3x3 Izq de la solucion P a la matriz M */
gsl_matrix_set(M,0,0,solucion[0]);
gsl_matrix_set(M,0,1,solucion[1]);
gsl_matrix_set(M,0,2,solucion[2]);
gsl_matrix_set(M,1,0,solucion[4]);
gsl_matrix_set(M,1,1,solucion[5]);
gsl_matrix_set(M,1,2,solucion[6]);
gsl_matrix_set(M,2,0,solucion[8]);
gsl_matrix_set(M,2,1,solucion[9]);
gsl_matrix_set(M,2,2,solucion[10]);
/* Copiamos el vector p4 */
gsl_vector_set(p4,0,solucion[3]);
gsl_vector_set(p4,1,solucion[7]);
gsl_vector_set(p4,2,solucion[11]);
/* invertimos la matriz M */
gsl_linalg_LU_decomp (M, p, &s);
gsl_linalg_LU_solve(M,p,p4,x);
gsl_linalg_LU_invert (M, p, M_prima);
/* Hacemos una descomposicion a la matriz M invertida */
gsl_linalg_QR_decomp (M_prima,tau);
gsl_linalg_QR_unpack (M_prima,tau,Q,R);
/* Invertimos R */
gsl_linalg_LU_decomp (R, p2, &s);
gsl_linalg_LU_invert (R, p2, R_prima);
/* Invertimos Q */
gsl_linalg_LU_decomp (Q, p3, &s);
gsl_linalg_LU_invert (Q, p3, Q_prima);
gsl_matrix_memcpy(Q_prima_tmp, Q_prima);
std::cout << "Calculamos" << std::endl;
if (DEBUG) {
/** checking results:
If the rq decompsition is correct we should obtain
the decomposed matrix:
orig_matrix = K*R*T
where T = (I|C)
*/
gsl_matrix_set(I_C,0,3,gsl_vector_get(x,0));
gsl_matrix_set(I_C,1,3,gsl_vector_get(x,1));
gsl_matrix_set(I_C,2,3,gsl_vector_get(x,2));
gsl_matrix_set(I_C,0,0,1);
gsl_matrix_set(I_C,0,1,0);
gsl_matrix_set(I_C,0,2,0);
gsl_matrix_set(I_C,1,0,0);
gsl_matrix_set(I_C,1,1,1);
gsl_matrix_set(I_C,1,2,0);
gsl_matrix_set(I_C,2,0,0);
gsl_matrix_set(I_C,2,1,0);
gsl_matrix_set(I_C,2,2,1);
gsl_linalg_matmult(R_prima,Q_prima,temp);
gsl_linalg_matmult(temp,I_C,test);
printf(" Result -> \n");
for (n=0; n<3; n++){
// for (mm=0; mm<4; mm++){
for (mm=0; mm<3; mm++){
printf(" %g \t",gsl_matrix_get(temp,n,mm));
// se debe sacar test
}
printf("\n");
}
}
/* El elemento (3,3) de la matriz R tiene que ser 1
para ello tenemos que normalizar la matriz dividiendo
entre este elemento
*/
tmp = gsl_matrix_get(R_prima,2,2);
for (n=0; n<3; n++)
for (mm=0; mm<3; mm++){
gsl_matrix_set(R_prima,n,mm, gsl_matrix_get(R_prima,n,mm)/tmp);
}
/* Si obtenemos valores negativos en la
diagonal de K tenemos que cambiar de signo la columna de K y la fila de Q
correspondiente
*/
if (DEBUG)
print_matrix(R_prima);
if (DEBUG)
print_matrix(Q_prima);
if (gsl_matrix_get(R_prima,0,0)<0){
if (DEBUG) printf(" distancia focat 0,0 negativa\n");
gsl_matrix_set(R_prima,0,0,
abs(gsl_matrix_get(R_prima,0,0))
);
for (n=0;n<3;n++)
gsl_matrix_set(Q_prima,0,n,
gsl_matrix_get(Q_prima,0,n)*-1
);
}
if (DEBUG) printf("R_prima\n");
print_matrix(R_prima);
if (DEBUG) printf("Q_prima\n");
print_matrix(Q_prima);
if (gsl_matrix_get(R_prima,1,1)<0){
if (DEBUG) printf(" distancia focal 1,1 negativa\n");
for (n=0;n<3;n++){
gsl_matrix_set(Q_prima,1,n,
gsl_matrix_get(Q_prima,1,n)*-1
);
gsl_matrix_set(R_prima,n,1,
gsl_matrix_get(R_prima,n,1)*-1
);
}
}
if (DEBUG) printf("R_prima\n");
print_matrix(R_prima);
if (DEBUG) printf("Q_prima\n");
print_matrix(Q_prima);
/*Finalmente, si Q queda con determinante -1 cambiamos de signo
todos sus elementos para obtener una rotación sin "reflexion".
NOTA: Este trozo de codigo lo he desactivado debido a que si lo
hacemos obtenemos una orientacion equivocada a la hora de dibujarla
con OGL
*/
gsl_linalg_LU_decomp (Q_prima_tmp, p3, &s);
signum=1;
det = gsl_linalg_LU_det(Q_prima_tmp,signum);
if (-1 == det && 0){
if (DEBUG) printf("Q has a negatif det");
for (n=0;n<3;n++)
for (mm=0;mm<3;mm++)
gsl_matrix_set(Q_prima,n,mm,gsl_matrix_get(Q_prima,n,mm)*-1);
}
}
int main() {
int ret;
int i, j;
gsl_vector* tau;
gsl_matrix *A;
gsl_matrix *Q, *R, *RTR;
gsl_matrix_view Rtop;
int M = 4;
int N = 3;
/*
gsl_matrix A;
double data[9];
memset(&A, 0, sizeof(gsl_matrix));
A.size1 = 3;
A.size2 = 3;
A.tda = 3;
A.data = data;
gsl_matrix_set(&A, 0, 0, 34.0);
gsl_matrix_set(&A, 0, 1, 4.0);
gsl_matrix_set(&A, 0, 2, 14.0);
gsl_matrix_set(&A, 1, 0, 1.0);
gsl_matrix_set(&A, 1, 1, 8.0);
gsl_matrix_set(&A, 1, 2, 3.0);
gsl_matrix_set(&A, 2, 0, 7.0);
gsl_matrix_set(&A, 2, 1, 1.0);
gsl_matrix_set(&A, 2, 2, 8.0);
*/
A = gsl_matrix_alloc(M, N);
for (i=0; i<M; i++)
for (j=0; j<N; j++)
gsl_matrix_set(A, i, j, (double)rand()/(double)RAND_MAX);
for (i=0; i<A->size1; i++) {
printf((i==0) ? "A = (" : " (");
for (j=0; j<A->size2; j++) {
printf(" %12.5g ", gsl_matrix_get(A, i, j));
}
printf(")\n");
}
printf("\n");
tau = gsl_vector_alloc(N);
ret = gsl_linalg_QR_decomp(A, tau);
Q = gsl_matrix_alloc(M, M);
R = gsl_matrix_alloc(M, N);
ret = gsl_linalg_QR_unpack(A, tau, Q, R);
for (i=0; i<Q->size1; i++) {
printf((i==0) ? "Q = (" : " (");
for (j=0; j<Q->size2; j++) {
printf(" %12.5g ", gsl_matrix_get(Q, i, j));
}
printf(")\n");
}
printf("\n");
for (i=0; i<R->size1; i++) {
printf((i==0) ? "R = (" : " (");
for (j=0; j<R->size2; j++) {
printf(" %12.5g ", gsl_matrix_get(R, i, j));
}
printf(")\n");
}
printf("\n");
Rtop = gsl_matrix_submatrix(R, 0, 0, N, N);
RTR = gsl_matrix_alloc(N, N);
gsl_matrix_memcpy(RTR, &(Rtop.matrix));
ret = gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit,
1.0, RTR, RTR);
//(Rtop.matrix), &(Rtop.matrix));
for (i=0; i<RTR->size1; i++) {
printf((i==0) ? "RTR = (" : " (");
for (j=0; j<RTR->size2; j++) {
printf(" %12.5g ", gsl_matrix_get(RTR, i, j));
}
printf(")\n");
}
printf("\n");
gsl_matrix_free(RTR);
gsl_matrix_free(Q);
gsl_matrix_free(R);
gsl_vector_free(tau);
gsl_matrix_free(A);
return 0;
}
int
gsl_linalg_hesstri_decomp(gsl_matrix * A, gsl_matrix * B, gsl_matrix * U,
gsl_matrix * V, gsl_vector * work)
{
const size_t N = A->size1;
if ((N != A->size2) || (N != B->size1) || (N != B->size2))
{
GSL_ERROR ("Hessenberg-triangular reduction requires square matrices",
GSL_ENOTSQR);
}
else if (N != work->size)
{
GSL_ERROR ("length of workspace must match matrix dimension",
GSL_EBADLEN);
}
else
{
double cs, sn; /* rotation parameters */
size_t i, j; /* looping */
gsl_vector_view xv, yv; /* temporary views */
/* B -> Q^T B = R (upper triangular) */
gsl_linalg_QR_decomp(B, work);
/* A -> Q^T A */
gsl_linalg_QR_QTmat(B, work, A);
/* initialize U and V if desired */
if (U)
{
gsl_linalg_QR_unpack(B, work, U, B);
}
else
{
/* zero out lower triangle of B */
for (j = 0; j < N - 1; ++j)
{
for (i = j + 1; i < N; ++i)
gsl_matrix_set(B, i, j, 0.0);
}
}
if (V)
gsl_matrix_set_identity(V);
if (N < 3)
return GSL_SUCCESS; /* nothing more to do */
/* reduce A and B */
for (j = 0; j < N - 2; ++j)
{
for (i = N - 1; i >= (j + 2); --i)
{
/* step 1: rotate rows i - 1, i to kill A(i,j) */
/*
* compute G = [ CS SN ] so that G^t [ A(i-1,j) ] = [ * ]
* [-SN CS ] [ A(i, j) ] [ 0 ]
*/
gsl_linalg_givens(gsl_matrix_get(A, i - 1, j),
gsl_matrix_get(A, i, j),
&cs,
&sn);
/* invert so drot() works correctly (G -> G^t) */
sn = -sn;
/* compute G^t A(i-1:i, j:n) */
xv = gsl_matrix_subrow(A, i - 1, j, N - j);
yv = gsl_matrix_subrow(A, i, j, N - j);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
/* compute G^t B(i-1:i, i-1:n) */
xv = gsl_matrix_subrow(B, i - 1, i - 1, N - i + 1);
yv = gsl_matrix_subrow(B, i, i - 1, N - i + 1);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
if (U)
{
/* accumulate U: U -> U G */
xv = gsl_matrix_column(U, i - 1);
yv = gsl_matrix_column(U, i);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
}
/* step 2: rotate columns i, i - 1 to kill B(i, i - 1) */
gsl_linalg_givens(-gsl_matrix_get(B, i, i),
gsl_matrix_get(B, i, i - 1),
&cs,
&sn);
/* invert so drot() works correctly (G -> G^t) */
sn = -sn;
/* compute B(1:i, i-1:i) G */
xv = gsl_matrix_subcolumn(B, i - 1, 0, i + 1);
yv = gsl_matrix_subcolumn(B, i, 0, i + 1);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
/* apply to A(1:n, i-1:i) */
xv = gsl_matrix_column(A, i - 1);
yv = gsl_matrix_column(A, i);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
if (V)
{
/* accumulate V: V -> V G */
xv = gsl_matrix_column(V, i - 1);
yv = gsl_matrix_column(V, i);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
}
}
}
return GSL_SUCCESS;
}
} /* gsl_linalg_hesstri_decomp() */
static void
linreg_fit_qr (const gsl_matrix *cov, linreg *l)
{
double intcpt_coef = 0.0;
double intercept_variance = 0.0;
gsl_matrix *xtx;
gsl_matrix *q;
gsl_matrix *r;
gsl_vector *xty;
gsl_vector *tau;
gsl_vector *params;
double tmp = 0.0;
size_t i;
size_t j;
xtx = gsl_matrix_alloc (cov->size1 - 1, cov->size2 - 1);
xty = gsl_vector_alloc (cov->size1 - 1);
tau = gsl_vector_alloc (cov->size1 - 1);
params = gsl_vector_alloc (cov->size1 - 1);
for (i = 0; i < xtx->size1; i++)
{
gsl_vector_set (xty, i, gsl_matrix_get (cov, cov->size2 - 1, i));
for (j = 0; j < xtx->size2; j++)
{
gsl_matrix_set (xtx, i, j, gsl_matrix_get (cov, i, j));
}
}
gsl_linalg_QR_decomp (xtx, tau);
q = gsl_matrix_alloc (xtx->size1, xtx->size2);
r = gsl_matrix_alloc (xtx->size1, xtx->size2);
gsl_linalg_QR_unpack (xtx, tau, q, r);
gsl_linalg_QR_solve (xtx, tau, xty, params);
for (i = 0; i < params->size; i++)
{
l->coeff[i] = gsl_vector_get (params, i);
}
l->sst = gsl_matrix_get (cov, cov->size1 - 1, cov->size2 - 1);
l->ssm = 0.0;
for (i = 0; i < l->n_indeps; i++)
{
l->ssm += gsl_vector_get (xty, i) * l->coeff[i];
}
l->sse = l->sst - l->ssm;
gsl_blas_dtrsm (CblasLeft, CblasLower, CblasNoTrans, CblasNonUnit, linreg_mse (l),
r, q);
/* Copy the lower triangle into the upper triangle. */
for (i = 0; i < q->size1; i++)
{
gsl_matrix_set (l->cov, i + 1, i + 1, gsl_matrix_get (q, i, i));
for (j = i + 1; j < q->size2; j++)
{
intercept_variance -= 2.0 * gsl_matrix_get (q, i, j) *
linreg_get_indep_variable_mean (l, i) *
linreg_get_indep_variable_mean (l, j);
gsl_matrix_set (q, i, j, gsl_matrix_get (q, j, i));
}
}
l->intercept = linreg_get_depvar_mean (l);
tmp = 0.0;
for (i = 0; i < l->n_indeps; i++)
{
tmp = linreg_get_indep_variable_mean (l, i);
l->intercept -= l->coeff[i] * tmp;
intercept_variance += tmp * tmp * gsl_matrix_get (q, i, i);
}
/* Covariances related to the intercept. */
intercept_variance += linreg_mse (l) / linreg_n_obs (l);
gsl_matrix_set (l->cov, 0, 0, intercept_variance);
for (i = 0; i < q->size1; i++)
{
for (j = 0; j < q->size2; j++)
{
intcpt_coef -= gsl_matrix_get (q, i, j)
* linreg_get_indep_variable_mean (l, j);
}
gsl_matrix_set (l->cov, 0, i + 1, intcpt_coef);
gsl_matrix_set (l->cov, i + 1, 0, intcpt_coef);
intcpt_coef = 0.0;
}
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (xty);
gsl_vector_free (tau);
gsl_matrix_free (xtx);
gsl_vector_free (params);
}