1 /*
2 # This file is part of the Astrometry.net suite.
3 # Licensed under a 3-clause BSD style license - see LICENSE
4 */
5
6 #include <assert.h>
7 #include <gsl/gsl_matrix_double.h>
8 #include <gsl/gsl_vector_double.h>
9 #include <gsl/gsl_permutation.h>
10 #include <gsl/gsl_linalg.h>
11 #include <gsl/gsl_blas.h>
12 #include <gsl/gsl_errno.h>
13 #include <stdarg.h>
14
15 #include "os-features.h"
16 #include "gslutils.h"
17 #include "errors.h"
18
errhandler(const char * reason,const char * file,int line,int gsl_errno)19 static void errhandler(const char * reason,
20 const char * file,
21 int line,
22 int gsl_errno) {
23 ERROR("GSL error: \"%s\" in %s:%i (gsl errno %i = %s)",
24 reason, file, line,
25 gsl_errno,
26 gsl_strerror(gsl_errno));
27 }
28
gslutils_use_error_system()29 void gslutils_use_error_system() {
30 gsl_set_error_handler(errhandler);
31 }
32
gslutils_invert_3x3(const double * A,double * B)33 int gslutils_invert_3x3(const double* A, double* B) {
34 gsl_matrix* LU;
35 gsl_permutation *p;
36 gsl_matrix_view mB;
37 int rtn = 0;
38 int signum;
39
40 p = gsl_permutation_alloc(3);
41 gsl_matrix_const_view mA = gsl_matrix_const_view_array(A, 3, 3);
42 mB = gsl_matrix_view_array(B, 3, 3);
43 LU = gsl_matrix_alloc(3, 3);
44
45 gsl_matrix_memcpy(LU, &mA.matrix);
46 if (gsl_linalg_LU_decomp(LU, p, &signum) ||
47 gsl_linalg_LU_invert(LU, p, &mB.matrix)) {
48 ERROR("gsl_linalg_LU_decomp() or _invert() failed.");
49 rtn = -1;
50 }
51
52 gsl_permutation_free(p);
53 gsl_matrix_free(LU);
54 return rtn;
55 }
56
gslutils_matrix_multiply(gsl_matrix * C,const gsl_matrix * A,const gsl_matrix * B)57 void gslutils_matrix_multiply(gsl_matrix* C,
58 const gsl_matrix* A, const gsl_matrix* B) {
59 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A, B, 0.0, C);
60 }
61
gslutils_solve_leastsquares_v(gsl_matrix * A,int NB,...)62 int gslutils_solve_leastsquares_v(gsl_matrix* A, int NB, ...) {
63 int i, res;
64 gsl_vector** B = malloc(NB * sizeof(gsl_vector*));
65 // Whoa, three-star programming!
66 gsl_vector*** X = malloc(NB * sizeof(gsl_vector**));
67 gsl_vector*** R = malloc(NB * sizeof(gsl_vector**));
68
69 gsl_vector** Xtmp = malloc(NB * sizeof(gsl_vector*));
70 gsl_vector** Rtmp = malloc(NB * sizeof(gsl_vector*));
71
72 va_list va;
73 va_start(va, NB);
74 for (i=0; i<NB; i++) {
75 B[i] = va_arg(va, gsl_vector*);
76 X[i] = va_arg(va, gsl_vector**);
77 R[i] = va_arg(va, gsl_vector**);
78 }
79 va_end(va);
80
81 res = gslutils_solve_leastsquares(A, B, Xtmp, Rtmp, NB);
82 for (i=0; i<NB; i++) {
83 if (X[i])
84 *(X[i]) = Xtmp[i];
85 else
86 gsl_vector_free(Xtmp[i]);
87 if (R[i])
88 *(R[i]) = Rtmp[i];
89 else
90 gsl_vector_free(Rtmp[i]);
91 }
92 free(Xtmp);
93 free(Rtmp);
94 free(X);
95 free(R);
96 free(B);
97 return res;
98 }
99
gslutils_solve_leastsquares(gsl_matrix * A,gsl_vector ** B,gsl_vector ** X,gsl_vector ** resids,int NB)100 int gslutils_solve_leastsquares(gsl_matrix* A, gsl_vector** B,
101 gsl_vector** X, gsl_vector** resids,
102 int NB) {
103 int i;
104 gsl_vector *tau, *resid = NULL;
105 Unused int ret;
106 int M, N;
107
108 M = A->size1;
109 N = A->size2;
110
111 for (i=0; i<NB; i++) {
112 assert(B[i]);
113 assert(B[i]->size == M);
114 }
115
116 tau = gsl_vector_alloc(MIN(M, N));
117 assert(tau);
118
119 ret = gsl_linalg_QR_decomp(A, tau);
120 assert(ret == 0);
121 // A,tau now contains a packed version of Q,R.
122
123 for (i=0; i<NB; i++) {
124 if (!resid) {
125 resid = gsl_vector_alloc(M);
126 assert(resid);
127 }
128 X[i] = gsl_vector_alloc(N);
129 assert(X[i]);
130 ret = gsl_linalg_QR_lssolve(A, tau, B[i], X[i], resid);
131 assert(ret == 0);
132 if (resids) {
133 resids[i] = resid;
134 resid = NULL;
135 }
136 }
137
138 gsl_vector_free(tau);
139 if (resid)
140 gsl_vector_free(resid);
141
142 return 0;
143 }
144
145