MathConvert-inl.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_MATHCONVERT_INL_H
#define SURGSIM_MATH_MATHCONVERT_INL_H
#include <string>
#include "SurgSim/Framework/Log.h"
namespace
{
const std::string rotationPropertyName = "Quaternion";
const std::string translationPropertyName = "Translation";
const std::string serializeLogger = "Serialization";
};
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type, int Rows, int Cols, int MOpt>
YAML::Node YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::encode(
const typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
{
typedef typename Eigen::Matrix<Type, Rows, Cols, MOpt>::Index Index;
YAML::Node node;
node.SetStyle(YAML::EmitterStyle::Flow);
if (Cols == 1)
{
for (Index i = 0; i < rhs.size(); ++i)
{
node.push_back(rhs(i, 0));
}
}
else
{
for (size_t row = 0; row < Rows; ++row)
{
YAML::Node rowNode;
for (size_t col = 0; col < Cols; ++col)
{
rowNode.push_back(rhs.row(row)[col]);
}
node.push_back(rowNode);
}
}
return node;
}
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type, int Rows, int Cols, int MOpt>
bool YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::decode(
const Node& node,
typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs) //NOLINT
{
if (! node.IsSequence() || node.size() != Rows)
{
return false;
}
if (Cols == 1)
{
for (size_t i = 0; i < node.size(); ++i)
{
try
{
rhs(i, 0) = node[i].as<Type>();
}
catch (YAML::RepresentationException)
{
rhs(i, 0) = std::numeric_limits<Type>::quiet_NaN();
auto logger = SurgSim::Framework::Logger::getLogger(serializeLogger);
SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
}
}
}
else
{
for (size_t row = 0; row < node.size(); ++row)
{
YAML::Node rowNode = node[row];
if (!rowNode.IsSequence() || node.size() != Cols)
{
return false;
}
for (size_t col = 0; col < rowNode.size(); ++col)
{
try
{
rhs.row(row)[col] = rowNode[col].as<Type>();
}
catch (YAML::RepresentationException)
{
rhs.row(row)[col] = std::numeric_limits<Type>::quiet_NaN();
auto logger = SurgSim::Framework::Logger::getLogger(serializeLogger);
SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
}
}
}
}
return true;
}
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type, int QOpt>
YAML::Node YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::encode(
const typename Eigen::Quaternion<Type, QOpt>& rhs)
{
return Node(convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::encode(rhs.coeffs()));
}
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type, int QOpt>
bool YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::decode(
const Node& node,
typename Eigen::Quaternion<Type, QOpt>& rhs) //NOLINT
{
bool result = false;
if (node.IsSequence() && node.size() == 4)
{
result = convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::decode(node, rhs.coeffs());
}
else
{
Eigen::AngleAxis<Type> angleAxis;
result = convert<typename Eigen::AngleAxis<Type>>::decode(node, angleAxis);
if (result)
{
rhs = angleAxis;
}
}
return result;
}
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type, int Dim, int TMode, int TOptions>
YAML::Node YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::encode(
const typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
{
typedef typename Eigen::Transform<Type, Dim, TMode, TOptions>::LinearMatrixType LinearMatrixType;
LinearMatrixType linear(rhs.linear());
Eigen::Quaternion<Type, TOptions> quaternion(linear);
Eigen::Matrix<Type, Dim, 1, TOptions> translation(rhs.translation());
Node node;
node[rotationPropertyName] = quaternion;
node[translationPropertyName] = translation;
return node;
}
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type, int Dim, int TMode, int TOptions>
bool YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::decode(
const Node& node,
typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs) //NOLINT
{
bool result = false;
if (node.IsMap())
{
Eigen::Quaternion<Type, TOptions> rotation(Eigen::Quaternion<Type, TOptions>::Identity());
Eigen::Matrix<Type, Dim, 1, TOptions> translation(Eigen::Matrix<Type, Dim, 1, TOptions>::Zero());
if (node[rotationPropertyName].IsDefined())
{
rotation = node[rotationPropertyName].as<Eigen::Quaternion<Type, TOptions>>();
result = true;
}
if (node[translationPropertyName].IsDefined())
{
translation = node[translationPropertyName].as<Eigen::Matrix<Type, Dim, 1, TOptions>>();
result = true;
}
rhs.makeAffine();
rhs.linear() = rotation.matrix();
rhs.translation() = translation;
}
return result;
}
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type>
YAML::Node YAML::convert<typename Eigen::AngleAxis<Type>>::encode(
const typename Eigen::AngleAxis<Type>& rhs)
{
Node node;
node.SetStyle(EmitterStyle::Flow);
node["Angle"] = rhs.angle();
node["Axis"] = convert<typename Eigen::Matrix<Type, 3, 1>>::encode(rhs.axis());
return node;
}
SURGSIM_DOUBLE_SPECIALIZATION
template <class Type>
bool YAML::convert<typename Eigen::AngleAxis<Type>>::decode(
const Node& node,
typename Eigen::AngleAxis<Type>& rhs) //NOLINT
{
bool result = false;
if (node.IsMap() && node["Angle"].IsDefined() && node["Axis"].IsDefined())
{
try
{
rhs.angle() = node["Angle"].as<Type>();
}
catch (RepresentationException)
{
rhs.angle() = std::numeric_limits<Type>::quiet_NaN();
auto logger = SurgSim::Framework::Logger::getLogger(serializeLogger);
SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
}
result = convert<typename Eigen::Matrix<Type, 3, 1>>::decode(node["Axis"], rhs.axis());
}
return result;
}
#endif // SURGSIM_MATH_MATHCONVERT_INL_H
Log.h
The convenience header that provides the entirety of the logging API.
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
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
SurgSim::DataStructures::Convert::serializeLogger
const std::string serializeLogger
Definition: DataStructuresConvert-inl.h:29