eigen_extensions.h
Go to the documentation of this file.
1 #ifndef EIGEN_EXTENSIONS_H
2 #define EIGEN_EXTENSIONS_H
3 
4 #include <Eigen/Eigen>
5 //#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
6 #include <Eigen/Sparse>
7 //#include <boost/filesystem.hpp>
8 #include <stdint.h>
9 #include <fstream>
10 #include <iostream>
11 //#include <gzstream/gzstream.h>
12 
13 namespace eigen_extensions {
14 
15  inline void stdToEig(const std::vector<double>& std, Eigen::VectorXd* eig)
16  {
17  eig->resize(std.size());
18  for(size_t i = 0; i < std.size(); ++i)
19  eig->coeffRef(i) = std[i];
20  }
21 
22  inline double stdev(const Eigen::VectorXd& vec)
23  {
24  double mean = vec.sum() / (double)vec.rows();
25  double total = 0;
26  for(int i = 0; i < vec.rows(); ++i)
27  total += (vec.coeffRef(i) - mean) * (vec.coeffRef(i) - mean);
28  double var = total / (double)vec.rows();
29  return sqrt(var);
30  }
31 
32  template<class S, int T, int U>
33  void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
34 
35  template<class S, int T, int U>
36  void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
37 
38  template<class S, int T, int U>
39  void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
40 
41  template<class S, int T, int U>
42  void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
43 
44 
45  // -- Scalar serialization
46  // TODO: Can you name these {de,}serialize() and still have the right
47  // functions get called when serializing matrices?
48  template<class T>
49  void serializeScalar(T val, std::ostream& strm);
50 
51  template<class T>
52  void deserializeScalar(std::istream& strm, T* val);
53 
54  template<class T>
55  void serializeScalarASCII(T val, std::ostream& strm);
56 
57  template<class T>
58  void deserializeScalarASCII(std::istream& strm, T* val);
59 
60 
61  /************************************************************
62  * Template implementations
63  ************************************************************/
64 
65  template<class S, int T, int U>
66  void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
67  {
68  int bytes = sizeof(S);
69  int rows = mat.rows();
70  int cols = mat.cols();
71  strm.write((char*)&bytes, sizeof(int));
72  strm.write((char*)&rows, sizeof(int));
73  strm.write((char*)&cols, sizeof(int));
74  strm.write((const char*)mat.data(), sizeof(S) * rows * cols);
75  }
76 
77  template<class S, int T, int U>
78  void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
79  {
80  int bytes;
81  int rows;
82  int cols;
83  strm.read((char*)&bytes, sizeof(int));
84  strm.read((char*)&rows, sizeof(int));
85  strm.read((char*)&cols, sizeof(int));
86  assert(bytes == sizeof(S));
87 
88  S *buf = (S*) malloc(sizeof(S) * rows * cols);
89  strm.read((char*)buf, sizeof(S) * rows * cols);
90  *mat = Eigen::Map< Eigen::Matrix<S, T, U> >(buf, rows, cols);
91  free(buf);
92  }
93 
94  template<class S, int T, int U>
95  void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
96  {
97  int old_precision = strm.precision();
98  strm.precision(16);
99  strm << "% " << mat.rows() << " " << mat.cols() << std::endl;
100  strm << mat << std::endl;
101  strm.precision(old_precision);
102  }
103 
104  template<class S, int T, int U>
105  void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
106  {
107  // -- Read the header.
108  std::string line;
109  while(line.length() == 0) getline(strm, line);
110  assert(line[0] == '%');
111  std::istringstream iss(line.substr(1));
112  int rows;
113  int cols;
114  iss >> rows;
115  iss >> cols;
116  // -- Read in the data.
117  *mat = Eigen::Matrix<S, T, U>(rows, cols);
118  for(int y = 0; y < rows; ++y) {
119  getline(strm, line);
120  std::istringstream iss(line);
121  for(int x = 0; x < cols; ++x) {
122  iss >> mat->coeffRef(y, x);
123  }
124  }
125  }
126 
127  template<class T>
128  void serializeScalar(T val, std::ostream& strm)
129  {
130  strm.write((char*)&val, sizeof(T));
131  }
132 
133  template<class T>
134  void deserializeScalar(std::istream& strm, T* val)
135  {
136  strm.read((char*)val, sizeof(T));
137  }
138 
139  template<class T>
140  void serializeScalarASCII(T val, std::ostream& strm)
141  {
142  int old_precision = strm.precision();
143  strm.precision(16);
144  strm << "% " << val << std::endl;
145  strm.precision(old_precision);
146  }
147 
148  template<class T>
149  void deserializeScalarASCII(std::istream& strm, T* val)
150  {
151  std::string line;
152  while(line.length() == 0) getline(strm, line);
153  assert(line[0] == '%');
154  std::istringstream iss(line.substr(1));
155  iss >> *val;
156  }
157 
158 }
159 
160 #endif // EIGEN_EXTENSIONS_H
void stdToEig(const std::vector< double > &std, Eigen::VectorXd *eig)
void serializeScalarASCII(T val, std::ostream &strm)
void deserializeASCII(std::istream &strm, Eigen::Matrix< S, T, U > *mat)
void serialize(const Eigen::Matrix< S, T, U > &mat, std::ostream &strm)
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
double stdev(const Eigen::VectorXd &vec)
void serializeScalar(T val, std::ostream &strm)
void deserializeScalarASCII(std::istream &strm, T *val)
void serializeASCII(const Eigen::Matrix< S, T, U > &mat, std::ostream &strm)
void deserializeScalar(std::istream &strm, T *val)
void deserialize(std::istream &strm, Eigen::Matrix< S, T, U > *mat)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58