10 #ifndef EIGEN_GSL_HELPER 11 #define EIGEN_GSL_HELPER 15 #include <gsl/gsl_blas.h> 16 #include <gsl/gsl_multifit.h> 17 #include <gsl/gsl_eigen.h> 18 #include <gsl/gsl_linalg.h> 19 #include <gsl/gsl_complex.h> 20 #include <gsl/gsl_complex_math.h> 24 template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
struct GslTraits 26 typedef gsl_matrix* Matrix;
27 typedef gsl_vector* Vector;
28 static Matrix createMatrix(
int rows,
int cols) {
return gsl_matrix_alloc(rows,cols); }
29 static Vector createVector(
int size) {
return gsl_vector_alloc(size); }
30 static void free(Matrix& m) { gsl_matrix_free(m); m=0; }
31 static void free(Vector& m) { gsl_vector_free(m); m=0; }
32 static void prod(
const Matrix& m,
const Vector& v, Vector& x) { gsl_blas_dgemv(CblasNoTrans,1,m,v,0,x); }
33 static void cholesky(Matrix& m) { gsl_linalg_cholesky_decomp(m); }
34 static void cholesky_solve(
const Matrix& m,
const Vector& b, Vector& x) { gsl_linalg_cholesky_solve(m,b,x); }
35 static void eigen_symm(
const Matrix& m, Vector& eval, Matrix& evec)
37 gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(m->size1);
38 Matrix a = createMatrix(m->size1, m->size2);
39 gsl_matrix_memcpy(a, m);
40 gsl_eigen_symmv(a,eval,evec,w);
41 gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
42 gsl_eigen_symmv_free(w);
45 static void eigen_symm_gen(
const Matrix& m,
const Matrix& _b, Vector& eval, Matrix& evec)
47 gsl_eigen_gensymmv_workspace * w = gsl_eigen_gensymmv_alloc(m->size1);
48 Matrix a = createMatrix(m->size1, m->size2);
49 Matrix b = createMatrix(_b->size1, _b->size2);
50 gsl_matrix_memcpy(a, m);
51 gsl_matrix_memcpy(b, _b);
52 gsl_eigen_gensymmv(a,b,eval,evec,w);
53 gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
54 gsl_eigen_gensymmv_free(w);
59 template<
typename Scalar>
struct GslTraits<Scalar,true>
61 typedef gsl_matrix_complex* Matrix;
62 typedef gsl_vector_complex* Vector;
63 static Matrix createMatrix(
int rows,
int cols) {
return gsl_matrix_complex_alloc(rows,cols); }
64 static Vector createVector(
int size) {
return gsl_vector_complex_alloc(size); }
65 static void free(Matrix& m) { gsl_matrix_complex_free(m); m=0; }
66 static void free(Vector& m) { gsl_vector_complex_free(m); m=0; }
67 static void cholesky(Matrix& m) { gsl_linalg_complex_cholesky_decomp(m); }
68 static void cholesky_solve(
const Matrix& m,
const Vector& b, Vector& x) { gsl_linalg_complex_cholesky_solve(m,b,x); }
69 static void prod(
const Matrix& m,
const Vector& v, Vector& x)
70 { gsl_blas_zgemv(CblasNoTrans,gsl_complex_rect(1,0),m,v,gsl_complex_rect(0,0),x); }
71 static void eigen_symm(
const Matrix& m, gsl_vector* &eval, Matrix& evec)
73 gsl_eigen_hermv_workspace * w = gsl_eigen_hermv_alloc(m->size1);
74 Matrix a = createMatrix(m->size1, m->size2);
75 gsl_matrix_complex_memcpy(a, m);
76 gsl_eigen_hermv(a,eval,evec,w);
77 gsl_eigen_hermv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
78 gsl_eigen_hermv_free(w);
81 static void eigen_symm_gen(
const Matrix& m,
const Matrix& _b, gsl_vector* &eval, Matrix& evec)
83 gsl_eigen_genhermv_workspace * w = gsl_eigen_genhermv_alloc(m->size1);
84 Matrix a = createMatrix(m->size1, m->size2);
85 Matrix b = createMatrix(_b->size1, _b->size2);
86 gsl_matrix_complex_memcpy(a, m);
87 gsl_matrix_complex_memcpy(b, _b);
88 gsl_eigen_genhermv(a,b,eval,evec,w);
89 gsl_eigen_hermv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
90 gsl_eigen_genhermv_free(w);
95 template<
typename MatrixType>
96 void convert(
const MatrixType& m, gsl_matrix* &res)
100 res = gsl_matrix_alloc(m.rows(), m.cols());
101 for (
int i=0 ; i<m.rows() ; ++i)
102 for (
int j=0 ; j<m.cols(); ++j)
103 gsl_matrix_set(res, i, j, m(i,j));
106 template<
typename MatrixType>
107 void convert(
const gsl_matrix* m,
MatrixType& res)
109 res.resize(
int(m->size1),
int(m->size2));
110 for (
int i=0 ; i<res.rows() ; ++i)
111 for (
int j=0 ; j<res.cols(); ++j)
112 res(i,j) = gsl_matrix_get(m,i,j);
115 template<
typename VectorType>
116 void convert(
const VectorType& m, gsl_vector* &res)
118 if (res) gsl_vector_free(res);
119 res = gsl_vector_alloc(m.size());
120 for (
int i=0 ; i<m.size() ; ++i)
121 gsl_vector_set(res, i, m[i]);
124 template<
typename VectorType>
125 void convert(
const gsl_vector* m,
VectorType& res)
127 res.resize (m->size);
128 for (
int i=0 ; i<res.rows() ; ++i)
129 res[i] = gsl_vector_get(m, i);
132 template<
typename MatrixType>
133 void convert(
const MatrixType& m, gsl_matrix_complex* &res)
135 res = gsl_matrix_complex_alloc(m.rows(), m.cols());
136 for (
int i=0 ; i<m.rows() ; ++i)
137 for (
int j=0 ; j<m.cols(); ++j)
139 gsl_matrix_complex_set(res, i, j,
140 gsl_complex_rect(m(i,j).real(), m(i,j).imag()));
144 template<
typename MatrixType>
145 void convert(
const gsl_matrix_complex* m,
MatrixType& res)
147 res.resize(
int(m->size1),
int(m->size2));
148 for (
int i=0 ; i<res.rows() ; ++i)
149 for (
int j=0 ; j<res.cols(); ++j)
151 GSL_REAL(gsl_matrix_complex_get(m,i,j)),
152 GSL_IMAG(gsl_matrix_complex_get(m,i,j)));
155 template<
typename VectorType>
156 void convert(
const VectorType& m, gsl_vector_complex* &res)
158 res = gsl_vector_complex_alloc(m.size());
159 for (
int i=0 ; i<m.size() ; ++i)
160 gsl_vector_complex_set(res, i, gsl_complex_rect(m[i].real(), m[i].imag()));
163 template<
typename VectorType>
164 void convert(
const gsl_vector_complex* m,
VectorType& res)
167 for (
int i=0 ; i<res.rows() ; ++i)
169 GSL_REAL(gsl_vector_complex_get(m, i)),
170 GSL_IMAG(gsl_vector_complex_get(m, i)));
175 #endif // EIGEN_GSL_HELPER
Definition: gsl_helper.h:24
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: TestIMU_Common.h:87
detail::size< coerce_list< Ts... >> size
Get the size of a list (number of elements.)
Definition: Size.h:56
double Scalar
Common scalar type.
Definition: FlexibleKalmanBase.h:48