Choreonoid  1.5
EigenArchive.h
Go to the documentation of this file.
1 
5 #ifndef CNOID_UTIL_EIGEN_ARCHIVE_H
6 #define CNOID_UTIL_EIGEN_ARCHIVE_H
7 
8 #include "ValueTree.h"
9 #include "EigenUtil.h"
10 //#include "YAMLWriter.h"
11 #include <boost/function.hpp>
12 #include <boost/format.hpp>
13 
14 namespace cnoid {
15 
16 template<typename Derived>
17 void read(const Listing& listing, Eigen::MatrixBase<Derived>& x)
18 {
19  const int nr = x.rows();
20  const int nc = x.cols();
21  if(listing.size() != nr * nc){
22  listing.throwException(
23  str(boost::format("A %1% x %2% matrix / vector value is expected") % nr % nc));
24  }
25  int index = 0;
26  for(int i=0; i < nr; ++i){
27  for(int j=0; j < nc; ++j){
28  x(i, j) = listing[index++].toDouble();
29  }
30  }
31 }
32 
33 
34 template<typename Derived>
35 bool read(const Mapping& mapping, const std::string& key, Eigen::MatrixBase<Derived>& x)
36 {
37  const Listing& s = *mapping.findListing(key);
38  if(!s.isValid()){
39  return false;
40  }
41  read(s, x);
42  return true;
43 }
44 
45 
46 template<typename Scalar, int Dim, int Mode>
47 bool read(const Mapping& mapping, const std::string& key, Eigen::Transform<Scalar, Dim, Mode>& T)
48 {
49  return read(mapping, key, T.matrix());
50 }
51 
52 
53 template<typename Derived>
54 void readEx(const Mapping& mapping, const std::string& key, Eigen::MatrixBase<Derived>& x)
55 {
56  if(!read(mapping, key, x)){
57  mapping.throwKeyNotFoundException(key);
58  }
59 }
60 
61 template<typename Derived>
62 Listing& write(Mapping& mapping, const std::string& key, const Eigen::MatrixBase<Derived>& x)
63 {
64  Listing& s = *mapping.createFlowStyleListing(key);
65  s.setDoubleFormat("%.9g");
66  const int nr = x.rows();
67  const int nc = x.cols();
68  if(nc == 1){
69  for(int i=0; i < nr; ++i){
70  s.append(x(i));
71  }
72  } else {
73  for(int i=0; i < nr; ++i){
74  s.appendLF();
75  for(int j=0; j < nc; ++j){
76  s.append(x(i, j));
77  }
78  }
79  }
80  return s;
81 }
82 
83 
84 template<typename Scalar, int Dim, int Mode>
85 Listing& write(Mapping& mapping, const std::string& key, const Eigen::Transform<Scalar, Dim, Mode>& T)
86 {
87  return write(mapping, key, T.matrix());
88 }
89 
90 
94 /*
95 template<typename Derived>
96 void write(YAMLWriter& writer, const std::string& key, const Eigen::MatrixBase<Derived>& x)
97 {
98  writer.putKey(key);
99  writer.startFlowStyleMapping();
100 
101  const int nr = x.rows();
102  const int nc = x.cols();
103  if(nc == 1){
104  for(int i=0; i < nr; ++i){
105  writer.putScalar(x(i));
106  }
107  } else {
108  for(int i=0; i < nr; ++i){
109  //writer.putLF();
110  for(int j=0; j < nc; ++j){
111  writer.putScalar(x(i, j));
112  }
113  }
114  }
115  writer.endListing();
116 }
117 */
118 
119 
120 template<typename Scalar>
121 bool read(const Mapping& mapping, const std::string& key, Eigen::AngleAxis<Scalar>& r)
122 {
123  const Listing& s = *mapping.findListing(key);
124  if(s.isValid() && s.size() == 4){
125  r.axis() << s[0].toDouble(), s[1].toDouble(), s[2].toDouble();
126  r.angle() = s[3].toDouble();
127  return true;
128  }
129  return false;
130 }
131 
132 template<typename Scalar>
133 Listing& write(Mapping& mapping, const std::string& key, const Eigen::AngleAxis<Scalar>& r)
134 {
135  Listing& s = *mapping.createFlowStyleListing(key);
136  s.setDoubleFormat("%.9g");
137  s.append(r.axis()[0]);
138  s.append(r.axis()[1]);
139  s.append(r.axis()[2]);
140  s.append(r.angle());
141  return s;
142 }
143 
144 inline bool read(const Mapping& mapping, const std::string& key, boost::function<void(Vector3&)> setterFunc)
145 {
146  Vector3 x;
147  if(read(mapping, key, x)){
148  setterFunc(x);
149  return true;
150  }
151  return false;
152 }
153 
154 }
155 
156 #endif
void throwException(const std::string &message) const
Definition: ValueTree.cpp:412
Definition: ValueTree.h:224
void append(ValueNode *node)
Definition: ValueTree.h:484
int size() const
Definition: ValueTree.h:440
double toDouble() const
Definition: ValueTree.cpp:200
Listing & write(Mapping &mapping, const std::string &key, const Eigen::MatrixBase< Derived > &x)
Definition: EigenArchive.h:62
std::string str(const Vector3 &v)
Definition: EigenUtil.cpp:90
Definition: ValueTree.h:424
Listing * createFlowStyleListing(const std::string &key)
Definition: ValueTree.h:295
void read(const Listing &listing, Eigen::MatrixBase< Derived > &x)
Definition: EigenArchive.h:17
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
bool isValid() const
Definition: ValueTree.h:51
Listing * findListing(const std::string &key) const
Definition: ValueTree.cpp:545
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
void throwKeyNotFoundException(const std::string &key) const
Definition: ValueTree.cpp:590
void setDoubleFormat(const char *format)
Definition: ValueTree.cpp:930
void appendLF()
Definition: ValueTree.cpp:948
void readEx(const Mapping &mapping, const std::string &key, Eigen::MatrixBase< Derived > &x)
Definition: EigenArchive.h:54