16 #ifndef SURGSIM_MATH_MATHCONVERT_INL_H
17 #define SURGSIM_MATH_MATHCONVERT_INL_H
25 const std::string rotationPropertyName =
"Quaternion";
26 const std::string translationPropertyName =
"Translation";
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)
36 node.SetStyle(YAML::EmitterStyle::Flow);
37 for (
int i = 0; i < rhs.size(); ++i)
39 node.push_back(rhs[i]);
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)
50 if (! node.IsSequence() || node.size() != Rows)
55 for (
unsigned i = 0; i < node.size(); ++i)
59 rhs[i] = node[i].as<Type>();
61 catch (YAML::RepresentationException)
63 rhs[i] = std::numeric_limits<Type>::quiet_NaN();
66 SURGSIM_LOG(logger, WARNING) <<
"Bad conversion: #NaN value";
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)
78 node.SetStyle(YAML::EmitterStyle::Flow);
79 for (
int row = 0; row < Rows; ++row)
82 for (
int col = 0; col < Cols; ++col)
84 rowNode.push_back(rhs.row(row)[col]);
86 node.push_back(rowNode);
92 template <
class Type,
int Rows,
int Cols,
int MOpt>
93 bool YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::decode(
95 typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
97 if (! node.IsSequence() || node.size() != Rows)
102 for (
size_t row = 0; row < node.size(); ++row)
104 YAML::Node rowNode = node[row];
105 if (!rowNode.IsSequence() || node.size() != Cols)
109 for (
size_t col = 0; col < rowNode.size(); ++col)
113 rhs.row(row)[col] = rowNode[col].as<Type>();
115 catch (YAML::RepresentationException)
117 rhs.row(row)[col] = std::numeric_limits<Type>::quiet_NaN();
119 SURGSIM_LOG(logger, WARNING) <<
"Bad conversion: #NaN value";
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)
131 return Node(
convert<
typename Eigen::Matrix<Type, 4, 1, QOpt>>::encode(rhs.coeffs()));
135 template <
class Type,
int QOpt>
136 bool YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::decode(
138 typename Eigen::Quaternion<Type, QOpt>& rhs)
141 if (node.IsSequence() && node.size() == 4)
143 result = convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::decode(node, rhs.coeffs());
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)
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());
159 node[rotationPropertyName] = quaternion;
160 node[translationPropertyName] = translation;
165 template <
class Type,
int Dim,
int TMode,
int TOptions>
166 bool YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::decode(
168 typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
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())
179 rotation = node[rotationPropertyName].as<Eigen::Quaternion<Type, TOptions>>();
182 if (node[translationPropertyName].IsDefined())
184 translation = node[translationPropertyName].as<Eigen::Matrix<Type, Dim, 1, TOptions>>();
188 rhs.linear() = rotation.matrix();
189 rhs.translation() = translation;
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() to correctly cast Matrix44d to Matrix44f, will throw if the val is no...
Definition: Accessible.cpp:199
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