MathConvert-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_MATH_MATHCONVERT_INL_H
17 #define SURGSIM_MATH_MATHCONVERT_INL_H
18 
19 #include <string>
20 
21 #include "SurgSim/Framework/Log.h"
22 
23 namespace
24 {
25 const std::string rotationPropertyName = "Quaternion";
26 const std::string translationPropertyName = "Translation";
27 const std::string serializeLogger = "Serialization";
28 };
29 
31 template <class Type, int Rows, int Cols, int MOpt>
32 YAML::Node YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::encode(
33  const typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
34 {
35  typedef typename Eigen::Matrix<Type, Rows, Cols, MOpt>::Index Index;
36  YAML::Node node;
37  node.SetStyle(YAML::EmitterStyle::Flow);
38  if (Cols == 1)
39  {
40  for (Index i = 0; i < rhs.size(); ++i)
41  {
42  node.push_back(rhs(i, 0));
43  }
44  }
45  else
46  {
47  for (size_t row = 0; row < Rows; ++row)
48  {
49  YAML::Node rowNode;
50  for (size_t col = 0; col < Cols; ++col)
51  {
52  rowNode.push_back(rhs.row(row)[col]);
53  }
54  node.push_back(rowNode);
55  }
56  }
57  return node;
58 }
59 
60 
62 template <class Type, int Rows, int Cols, int MOpt>
63 bool YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::decode(
64  const Node& node,
65  typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs) //NOLINT
66 {
67  if (! node.IsSequence() || node.size() != Rows)
68  {
69  return false;
70  }
71 
72  if (Cols == 1)
73  {
74  for (size_t i = 0; i < node.size(); ++i)
75  {
76  try
77  {
78  rhs(i, 0) = node[i].as<Type>();
79  }
80  catch (YAML::RepresentationException)
81  {
82  rhs(i, 0) = std::numeric_limits<Type>::quiet_NaN();
83 
85  SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
86  }
87  }
88  }
89  else
90  {
91  for (size_t row = 0; row < node.size(); ++row)
92  {
93  YAML::Node rowNode = node[row];
94  if (!rowNode.IsSequence() || node.size() != Cols)
95  {
96  return false;
97  }
98  for (size_t col = 0; col < rowNode.size(); ++col)
99  {
100  try
101  {
102  rhs.row(row)[col] = rowNode[col].as<Type>();
103  }
104  catch (YAML::RepresentationException)
105  {
106  rhs.row(row)[col] = std::numeric_limits<Type>::quiet_NaN();
108  SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
109  }
110  }
111  }
112  }
113  return true;
114 }
115 
117 template <class Type, int QOpt>
118 YAML::Node YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::encode(
119  const typename Eigen::Quaternion<Type, QOpt>& rhs)
120 {
121  return Node(convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::encode(rhs.coeffs()));
122 }
123 
125 template <class Type, int QOpt>
126 bool YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::decode(
127  const Node& node,
128  typename Eigen::Quaternion<Type, QOpt>& rhs) //NOLINT
129 {
130  bool result = false;
131  if (node.IsSequence() && node.size() == 4)
132  {
133  result = convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::decode(node, rhs.coeffs());
134  }
135  else
136  {
137  Eigen::AngleAxis<Type> angleAxis;
138  result = convert<typename Eigen::AngleAxis<Type>>::decode(node, angleAxis);
139  if (result)
140  {
141  rhs = angleAxis;
142  }
143  }
144  return result;
145 }
146 
148 template <class Type, int Dim, int TMode, int TOptions>
149 YAML::Node YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::encode(
150  const typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
151 {
152  typedef typename Eigen::Transform<Type, Dim, TMode, TOptions>::LinearMatrixType LinearMatrixType;
153  LinearMatrixType linear(rhs.linear());
154  Eigen::Quaternion<Type, TOptions> quaternion(linear);
155  Eigen::Matrix<Type, Dim, 1, TOptions> translation(rhs.translation());
156 
157  Node node;
158  node[rotationPropertyName] = quaternion;
159  node[translationPropertyName] = translation;
160  return node;
161 }
162 
164 template <class Type, int Dim, int TMode, int TOptions>
165 bool YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::decode(
166  const Node& node,
167  typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs) //NOLINT
168 {
169  bool result = false;
170 
171 
172  if (node.IsMap())
173  {
174  Eigen::Quaternion<Type, TOptions> rotation(Eigen::Quaternion<Type, TOptions>::Identity());
175  Eigen::Matrix<Type, Dim, 1, TOptions> translation(Eigen::Matrix<Type, Dim, 1, TOptions>::Zero());
176  if (node[rotationPropertyName].IsDefined())
177  {
178  rotation = node[rotationPropertyName].as<Eigen::Quaternion<Type, TOptions>>();
179  result = true;
180  }
181  if (node[translationPropertyName].IsDefined())
182  {
183  translation = node[translationPropertyName].as<Eigen::Matrix<Type, Dim, 1, TOptions>>();
184  result = true;
185  }
186  rhs.makeAffine();
187  rhs.linear() = rotation.matrix();
188  rhs.translation() = translation;
189  }
190  return result;
191 }
192 
194 template <class Type>
195 YAML::Node YAML::convert<typename Eigen::AngleAxis<Type>>::encode(
196  const typename Eigen::AngleAxis<Type>& rhs)
197 {
198  Node node;
199  node.SetStyle(EmitterStyle::Flow);
200  node["Angle"] = rhs.angle();
201  node["Axis"] = convert<typename Eigen::Matrix<Type, 3, 1>>::encode(rhs.axis());
202  return node;
203 }
204 
206 template <class Type>
207 bool YAML::convert<typename Eigen::AngleAxis<Type>>::decode(
208  const Node& node,
209  typename Eigen::AngleAxis<Type>& rhs) //NOLINT
210 {
211  bool result = false;
212  if (node.IsMap() && node["Angle"].IsDefined() && node["Axis"].IsDefined())
213  {
214  try
215  {
216  rhs.angle() = node["Angle"].as<Type>();
217  }
218  catch (RepresentationException)
219  {
220  rhs.angle() = std::numeric_limits<Type>::quiet_NaN();
222  SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
223  }
224  result = convert<typename Eigen::Matrix<Type, 3, 1>>::decode(node["Axis"], rhs.axis());
225  }
226  return result;
227 }
228 
229 #endif // SURGSIM_MATH_MATHCONVERT_INL_H
SurgSim::Framework::Logger::getLogger
static std::shared_ptr< Logger > getLogger(const std::string &name)
Get a logger by name from Logger Manager.
Definition: Logger.h:109
SURGSIM_LOG
#define SURGSIM_LOG(logger, level)
Logs a message to the specified logger with the short level name.
Definition: LogMacros.h:60
SURGSIM_DOUBLE_SPECIALIZATION
#define SURGSIM_DOUBLE_SPECIALIZATION
Definition: Macros.h:44
Log.h
SurgSim::DataStructures::Convert::serializeLogger
const std::string serializeLogger
Definition: DataStructuresConvert-inl.h:29
SurgSim::Framework::convert
SurgSim::Math::Matrix44f convert(boost::any val)
Specialization for convert<T>() to correctly cast Matrix44d to Matrix44f, will throw if the val is no...
Definition: Accessible.cpp:210