eigen-io.h
Go to the documentation of this file.
1 //
2 // Copyright 2016 CNRS
3 //
4 // Author: Rohan Budhiraja
5 //
6 
7 #ifndef DYNAMIC_GRAPH_EIGEN_IO_H
8 #define DYNAMIC_GRAPH_EIGEN_IO_H
9 
12 
13 #include <Eigen/Geometry>
14 #include <boost/format.hpp>
15 #include <boost/numeric/conversion/cast.hpp>
16 
18 
19 // TODO: Eigen 3.3 onwards has a global Eigen::Index definition.
20 // If Eigen version is updated, use Eigen::Index instead of this macro.
21 
22 /* \brief Eigen Vector input from istream
23  *
24  * Input Vector format: [N](val1,val2,val3,...,valN)
25  * e.g. [5](1,23,32.2,12.12,32)
26  */
27 namespace Eigen {
28 typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE eigen_index;
29 
30 inline std::istringstream &operator>>(std::istringstream &iss,
31  dynamicgraph::Vector &inst) {
32  unsigned int _size;
33  double _dbl_val;
34  char _ch;
35  boost::format fmt(
36  "Failed to enter %s as vector."
37  " Reenter as [N](val1,val2,val3,...,valN)");
38  fmt % iss.str();
39  if (iss >> _ch && _ch != '[') {
40  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
41  } else {
42  if (iss >> _size && !iss.fail()) {
43  inst.resize(_size);
44  } else
45  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
46  if (iss >> _ch && _ch != ']')
47  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
48  else {
49  if (iss >> _ch && _ch != '(')
50  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
51  else {
52  for (unsigned int i = 0; i < _size; i++) {
53  iss >> _dbl_val;
54  if (iss.peek() == ',' || iss.peek() == ' ') iss.ignore();
55  inst(i) = _dbl_val;
56  }
57  if (iss >> _ch && _ch != ')')
58  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
59  }
60  }
61  }
62  return iss;
63 }
64 
65 /* \brief Eigen Matrix input from istream
66  *
67  * Matrix format: [M,N]((val11,val12,val13,...,val1N),...,
68  * (valM1,valM2,...,valMN))
69  * e.g. [2,5]((1 23 32.2 12.12 32),(2 32 23 92.01 19.2))
70  */
71 
72 template <typename Derived>
73 inline std::istringstream &operator>>(std::istringstream &iss,
74  DenseBase<Derived> &inst) {
75  unsigned int _colsize;
76  unsigned int _rowsize;
77  double _dbl_val;
78  char _ch;
79  boost::format fmt(
80  "Failed to enter %s as matrix. Reenter as "
81  "((val11,val12,val13,...,val1N),"
82  "...,(valM1,valM2,...,valMN))");
83  MatrixXd _tmp_matrix;
84  fmt % iss.str();
85  if (iss >> _ch && _ch != '[') {
86  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
87  } else {
88  iss >> _rowsize;
89  if (iss.peek() == ',' || iss.peek() == ' ') iss.ignore();
90  iss >> _colsize;
91  if (iss.fail())
92  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
93  else {
94  _tmp_matrix.resize(_rowsize, _colsize);
95  if (iss >> _ch && _ch != ']')
96  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
97  else {
98  if (iss >> _ch && _ch != '(')
99  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
100  else {
101  for (unsigned int j = 0; j < _rowsize; j++) {
102  if (iss >> _ch && _ch != '(')
103  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
104  for (unsigned int i = 0; i < _colsize; i++) {
105  iss >> _dbl_val;
106  if (iss.peek() == ',' || iss.peek() == ' ') iss.ignore();
107  _tmp_matrix(j, i) = _dbl_val;
108  }
109  if (iss >> _ch && _ch != ')')
110  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
111  if (iss.peek() == ',' || iss.peek() == ' ') iss.ignore();
112  }
113  if (iss >> _ch && _ch != ')')
114  throw ExceptionSignal(ExceptionSignal::GENERIC, fmt.str());
115  }
116  }
117  }
118  }
119  inst = _tmp_matrix;
120  return iss;
121 }
122 
123 inline std::istringstream &operator>>(std::istringstream &iss,
124  Transform<double, 3, Affine> &inst) {
125  MatrixXd M;
126  iss >> M;
127  inst.matrix() = M;
128  return iss;
129 }
130 
131 /* \brief Eigen Homogeneous Matrix output
132  *
133  * Matrix format: [M,N]((val11,val12,val13,...,val1N),...,
134  * (valM1,valM2,...,valMN))
135  * e.g. [2,5]((1 23 32.2 12.12 32),(2 32 23 92.01 19.2))
136  */
137 
138 inline std::ostream &operator<<(std::ostream &os,
139  Transform<double, 3, Affine> MH) {
140  IOFormat boostFmt(StreamPrecision, DontAlignCols, ",", ",", "(", ")", "(",
141  ")");
142 
143  os << "[4,4]" << MH.matrix().format(boostFmt);
144  return os;
145 }
146 
147 inline std::ostream &operator<<(std::ostream &os, AngleAxisd quat) {
148  VectorXd v(4);
149  v(0) = quat.angle();
150  v.tail<3>() = quat.axis();
151  os << v;
152  return os;
153 }
154 
155 inline std::istringstream &operator>>(std::istringstream &iss,
156  AngleAxisd &inst) {
157  VectorXd v(4);
158  iss >> v;
159  inst.angle() = v(0);
160  inst.axis() = v.tail<3>();
161  return iss;
162 }
163 
164 } // namespace Eigen
165 
166 #endif // DYNAMIC_GRAPH_EIGEN_IO_H
Eigen::VectorXd Vector
Definition: eigen-io.h:27
Exceptions raised when an error related to signals happen.
std::ostream & operator<<(std::ostream &os, Transform< double, 3, Affine > MH)
Definition: eigen-io.h:138
EIGEN_DEFAULT_DENSE_INDEX_TYPE eigen_index
Definition: eigen-io.h:28
std::istringstream & operator>>(std::istringstream &iss, dynamicgraph::Vector &inst)
Definition: eigen-io.h:30


dynamic-graph
Author(s): Nicolas Mansard, Olivier Stasse
autogenerated on Sun Jun 25 2023 02:06:03