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 <typename Type, int Rows, int MOpt>
32 YAML::Node YAML::convert<typename Eigen::Matrix<Type, Rows, 1, MOpt>>::encode(
33  const typename Eigen::Matrix<Type, Rows, 1, MOpt>& rhs)
34 {
35  Node node;
36  node.SetStyle(YAML::EmitterStyle::Flow);
37  for (int i = 0; i < rhs.size(); ++i)
38  {
39  node.push_back(rhs[i]);
40  }
41 
42  return node;
43 }
44 
46 template <class Type, int Rows, int MOpt>
47 bool YAML::convert<typename Eigen::Matrix<Type, Rows, 1, MOpt>>::decode(
48  const Node& node, typename Eigen::Matrix<Type, Rows, 1, MOpt>& rhs)
49 {
50  if (! node.IsSequence() || node.size() != Rows)
51  {
52  return false;
53  }
54 
55  for (unsigned i = 0; i < node.size(); ++i)
56  {
57  try
58  {
59  rhs[i] = node[i].as<Type>();
60  }
61  catch (YAML::RepresentationException)
62  {
63  rhs[i] = std::numeric_limits<Type>::quiet_NaN();
64 
66  SURGSIM_LOG(logger, WARNING) << "Bad conversion: #NaN value";
67  }
68  }
69  return true;
70 }
71 
73 template <class Type, int Rows, int Cols, int MOpt>
74 YAML::Node YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::encode(
75  const typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
76 {
77  YAML::Node node;
78  node.SetStyle(YAML::EmitterStyle::Flow);
79  for (int row = 0; row < Rows; ++row)
80  {
81  YAML::Node rowNode;
82  for (int col = 0; col < Cols; ++col)
83  {
84  rowNode.push_back(rhs.row(row)[col]);
85  }
86  node.push_back(rowNode);
87  }
88  return node;
89 }
90 
92 template <class Type, int Rows, int Cols, int MOpt>
93 bool YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::decode(
94  const Node& node,
95  typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
96 {
97  if (! node.IsSequence() || node.size() != Rows)
98  {
99  return false;
100  }
101 
102  for (size_t row = 0; row < node.size(); ++row)
103  {
104  YAML::Node rowNode = node[row];
105  if (!rowNode.IsSequence() || node.size() != Cols)
106  {
107  return false;
108  }
109  for (size_t col = 0; col < rowNode.size(); ++col)
110  {
111  try
112  {
113  rhs.row(row)[col] = rowNode[col].as<Type>();
114  }
115  catch (YAML::RepresentationException)
116  {
117  rhs.row(row)[col] = std::numeric_limits<Type>::quiet_NaN();
119  SURGSIM_LOG(logger, WARNING) << "Bad conversion: #NaN value";
120  }
121  }
122  }
123  return true;
124 }
125 
127 template <class Type, int QOpt>
128 YAML::Node YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::encode(
129  const typename Eigen::Quaternion<Type, QOpt>& rhs)
130 {
131  return Node(convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::encode(rhs.coeffs()));
132 }
133 
135 template <class Type, int QOpt>
136 bool YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::decode(
137  const Node& node,
138  typename Eigen::Quaternion<Type, QOpt>& rhs)
139 {
140  bool result = false;
141  if (node.IsSequence() && node.size() == 4)
142  {
143  result = convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::decode(node, rhs.coeffs());
144  }
145  return result;
146 }
147 
149 template <class Type, int Dim, int TMode, int TOptions>
150 YAML::Node YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::encode(
151  const typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
152 {
153  typedef typename Eigen::Transform<Type, Dim, TMode, TOptions>::LinearMatrixType LinearMatrixType;
154  LinearMatrixType linear(rhs.linear());
155  Eigen::Quaternion<Type, TOptions> quaternion(linear);
156  Eigen::Matrix<Type, Dim, 1, TOptions> translation(rhs.translation());
157 
158  Node node;
159  node[rotationPropertyName] = quaternion;
160  node[translationPropertyName] = translation;
161  return node;
162 }
163 
165 template <class Type, int Dim, int TMode, int TOptions>
166 bool YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::decode(
167  const Node& node,
168  typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
169 {
170  bool result = false;
171 
172 
173  if (node.IsMap())
174  {
175  Eigen::Quaternion<Type, TOptions> rotation(Eigen::Quaternion<Type, TOptions>::Identity());
176  Eigen::Matrix<Type, Dim, 1, TOptions> translation(Eigen::Matrix<Type, Dim, 1, TOptions>::Zero());
177  if (node[rotationPropertyName].IsDefined())
178  {
179  rotation = node[rotationPropertyName].as<Eigen::Quaternion<Type, TOptions>>();
180  result = true;
181  }
182  if (node[translationPropertyName].IsDefined())
183  {
184  translation = node[translationPropertyName].as<Eigen::Matrix<Type, Dim, 1, TOptions>>();
185  result = true;
186  }
187  rhs.makeAffine();
188  rhs.linear() = rotation.matrix();
189  rhs.translation() = translation;
190  }
191  return result;
192 }
193 
194 #endif // SURGSIM_MATH_MATHCONVERT_INL_H
The convenience header that provides the entirety of the logging API.
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:199
string(TOUPPER ${DEVICE}DEVICE_UPPER_CASE) option(BUILD_DEVICE_ $
Definition: CMakeLists.txt:35
static std::shared_ptr< Logger > getLogger(const std::string &name)
Get a logger by name from Logger Manager.
Definition: Logger.h:109
#define SURGSIM_LOG(logger, level)
Logs a message to the specified logger with the short level name.
Definition: LogMacros.h:60
#define SURGSIM_DOUBLE_SPECIALIZATION
Definition: Macros.h:44
const std::string serializeLogger
Definition: DataStructuresConvert-inl.h:29