OSVR-Core
gsl_helper.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra. Eigen itself is part of the KDE project.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_GSL_HELPER
11 #define EIGEN_GSL_HELPER
12 
13 #include <Eigen/Core>
14 
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>
21 
22 namespace Eigen {
23 
24 template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex> struct GslTraits
25 {
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)
36  {
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);
43  free(a);
44  }
45  static void eigen_symm_gen(const Matrix& m, const Matrix& _b, Vector& eval, Matrix& evec)
46  {
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);
55  free(a);
56  }
57 };
58 
59 template<typename Scalar> struct GslTraits<Scalar,true>
60 {
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)
72  {
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);
79  free(a);
80  }
81  static void eigen_symm_gen(const Matrix& m, const Matrix& _b, gsl_vector* &eval, Matrix& evec)
82  {
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);
91  free(a);
92  }
93 };
94 
95 template<typename MatrixType>
96 void convert(const MatrixType& m, gsl_matrix* &res)
97 {
98 // if (res)
99 // gsl_matrix_free(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));
104 }
105 
106 template<typename MatrixType>
107 void convert(const gsl_matrix* m, MatrixType& res)
108 {
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);
113 }
114 
115 template<typename VectorType>
116 void convert(const VectorType& m, gsl_vector* &res)
117 {
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]);
122 }
123 
124 template<typename VectorType>
125 void convert(const gsl_vector* m, VectorType& res)
126 {
127  res.resize (m->size);
128  for (int i=0 ; i<res.rows() ; ++i)
129  res[i] = gsl_vector_get(m, i);
130 }
131 
132 template<typename MatrixType>
133 void convert(const MatrixType& m, gsl_matrix_complex* &res)
134 {
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)
138  {
139  gsl_matrix_complex_set(res, i, j,
140  gsl_complex_rect(m(i,j).real(), m(i,j).imag()));
141  }
142 }
143 
144 template<typename MatrixType>
145 void convert(const gsl_matrix_complex* m, MatrixType& res)
146 {
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)
150  res(i,j) = typename MatrixType::Scalar(
151  GSL_REAL(gsl_matrix_complex_get(m,i,j)),
152  GSL_IMAG(gsl_matrix_complex_get(m,i,j)));
153 }
154 
155 template<typename VectorType>
156 void convert(const VectorType& m, gsl_vector_complex* &res)
157 {
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()));
161 }
162 
163 template<typename VectorType>
164 void convert(const gsl_vector_complex* m, VectorType& res)
165 {
166  res.resize(m->size);
167  for (int i=0 ; i<res.rows() ; ++i)
168  res[i] = typename VectorType::Scalar(
169  GSL_REAL(gsl_vector_complex_get(m, i)),
170  GSL_IMAG(gsl_vector_complex_get(m, i)));
171 }
172 
173 }
174 
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
Definition: FFTW.cpp:65
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