yaml.hpp 1.88 KB
Newer Older
wangdawei's avatar
wangdawei committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
#include <Eigen/Core>
#include "yaml-cpp/yaml.h"

namespace YAML
{
    template <>
    struct convert<Eigen::Vector3d>
    {
        static Node encode(const Eigen::Vector3d &rhs)
        {
            Node node;
            node.push_back(rhs.x());
            node.push_back(rhs.y());
            node.push_back(rhs.z());
            return node;
        }
        static bool decode(const Node &node, Eigen::Vector3d &rhs)
        {
            if (!node.IsSequence() || node.size() != 3)
            {
                return false;
            }
            rhs.x() = node[0].as<double>();
            rhs.y() = node[1].as<double>();
            rhs.z() = node[2].as<double>();
            return true;
        }
    };
    template <>
    struct convert<Eigen::Matrix3d>
    {
        static Node encode(const Eigen::Matrix3d &rhs)
        {
            Node node;
            node.push_back(rhs(0, 0));
            node.push_back(rhs(0, 1));
            node.push_back(rhs(0, 2));
            node.push_back(rhs(1, 0));
            node.push_back(rhs(1, 1));
            node.push_back(rhs(1, 2));
            node.push_back(rhs(2, 0));
            node.push_back(rhs(2, 1));
            node.push_back(rhs(2, 2));
            return node;
        }
        static bool decode(const Node &node, Eigen::Matrix3d &rhs)
        {
            if (!node.IsSequence() || node.size() != 9)
            {
                return false;
            }
            rhs(0, 0) = node[0].as<double>();
            rhs(0, 1) = node[1].as<double>();
            rhs(0, 2) = node[2].as<double>();
            rhs(1, 0) = node[3].as<double>();
            rhs(1, 1) = node[4].as<double>();
            rhs(1, 2) = node[5].as<double>();
            rhs(2, 0) = node[6].as<double>();
            rhs(2, 1) = node[7].as<double>();
            rhs(2, 2) = node[8].as<double>();
            return true;
        }
    };
} // namespace YAML