Program Listing for File yaml-serialization-eigen.h

Return to documentation for file (aslam_cv2/aslam_cv_common/include/aslam/common/yaml-serialization-eigen.h)

#ifndef ASLAM_CV_COMMON_EIGEN_YAML_SERIALIZATION_H_
#define ASLAM_CV_COMMON_EIGEN_YAML_SERIALIZATION_H_

#include <Eigen/Core>
#include <glog/logging.h>
#include <yaml-cpp/yaml.h>

namespace YAML {  // This has to be in the same namespace as the Emitter.
// yaml serialization helper function for the Eigen3 Matrix object.
// The matrix is a base class for dense matrices.
// http://eigen.tuxfamily.org/dox-devel/TutorialMatrixClass.html
template <class Scalar_, int A_, int B_, int C_, int D_, int E_>
struct convert<Eigen::Matrix<Scalar_, A_, B_, C_, D_, E_> > {
  template <class Scalar, int A, int B, int C, int D, int E>
  static Node encode(const Eigen::Matrix<Scalar, A, B, C, D, E>& M) {
    Node node;
    typedef typename Eigen::Matrix<Scalar, A, B, C, D, E>::Index IndexType;
    IndexType rows = M.rows();
    IndexType cols = M.cols();
    node["rows"] = rows;
    node["cols"] = cols;
    CHECK_GT(rows, 0);
    CHECK_GT(cols, 0);
    for (IndexType i = 0; i < rows; ++i) {
      if (cols > 1) {
        YAML::Emitter ith_row;
        ith_row << YAML::Flow;
        ith_row << YAML::BeginSeq;
        for (IndexType j = 0; j < cols; ++j) {
          ith_row << M(i, j);
        }
        ith_row << YAML::EndSeq;
        CHECK(ith_row.good()) << "Emitter error: " << ith_row.GetLastError();
        node["data"].push_back(YAML::Load(ith_row.c_str()));
      } else {
        node["data"].push_back(M(i, 0));
      }
    }
    return node;
  }

  template <class Scalar, int A, int B, int C, int D, int E>
  static bool decodeEigenMatrixData(
      const Node& node, const int rows, const int cols,
      Eigen::Matrix<Scalar, A, B, C, D, E>& M) {
    typedef typename Eigen::Matrix<Scalar, A, B, C, D, E>::Index IndexType;
    if (!node.IsSequence()) {
      LOG(ERROR) << "The matrix data is not a sequence.";
      return false;
    }

    if (rows == 0 && cols == 0) {
      return true;
    }
    YAML::const_iterator it = node.begin();
    YAML::const_iterator it_end = node.end();

    // If the 2D matrix is stored as a linear array:
    if (static_cast<int>(node.size()) == rows * cols) {
      for (IndexType i = 0; i < rows; ++i) {
        for (IndexType j = 0; j < cols; ++j) {
          CHECK(it != it_end);
          if (it->IsSequence()) {
            CHECK(it->size() != 1u) << "Wrong dimension of Eigen-type matrix!";
            M(i, j) = it->begin()->as<Scalar>();
          } else {
            M(i, j) = it->as<Scalar>();
          }
          ++it;
        }
      }
    } else if (static_cast<int>(node.size()) == rows) {
      // If the 2D matrix is stored as a 2D, row-major matrix.
      for (IndexType i = 0; i < rows; ++i) {
        CHECK(it != it_end);

        // If there is only one column, the rows might not contain a sequence,
        // so we cannot initialize the iterators.

        if (it->IsSequence()) {
          YAML::const_iterator col_it = it->begin();
          YAML::const_iterator col_it_end = it->end();
          CHECK(static_cast<int>(it->size()) == cols)
              << "Wrong dimension of Eigen-type matrix! Expected " << cols
              << " columns, but provided " << it->size();

          for (IndexType j = 0; j < cols; ++j) {
            CHECK(col_it != col_it_end);
            M(i, j) = col_it->as<Scalar>();
            ++col_it;
          }
        } else {
          CHECK(cols == 1) << "Wrong dimension of Eigen-type matrix! Expected "
                           << cols << " columns, but provided " << 1;
          M(i, 0) = it->as<Scalar>();
        }
        ++it;
      }
    } else {
      LOG(ERROR) << "Wrong dimension (number of rows) of Eigen-type "
                    "matrix!";
      return false;
    }
    return true;
  }

  template <class Scalar, int A, int B, int C, int D, int E>
  static bool decode(
      const Node& node, Eigen::Matrix<Scalar, A, B, C, D, E>& M) {
    if (!node.IsMap()) {
      LOG(ERROR)
          << "Unable to get parse the matrix because the node is not a map.";
      return false;
    }

    typedef typename Eigen::Matrix<Scalar, A, B, C, D, E>::Index IndexType;
    IndexType rows = node["rows"].as<IndexType>();
    IndexType cols = node["cols"].as<IndexType>();

    if (rows != A || cols != B) {
      LOG(ERROR) << "The matrix is the wrong size (rows, cols). Wanted: (" << A
                 << "," << B << "), got (" << rows << ", " << cols << ")";
      return false;
    }

    return decodeEigenMatrixData(node["data"], rows, cols, M);
  }

  template <class Scalar, int B, int C, int D, int E>
  static bool decode(
      const Node& node, Eigen::Matrix<Scalar, Eigen::Dynamic, B, C, D, E>& M) {
    if (!node.IsMap()) {
      LOG(ERROR)
          << "Unable to get parse the matrix because the node is not a map.";
      return false;
    }

    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, B, C, D, E>::Index
        IndexType;
    IndexType rows = node["rows"].as<IndexType>();
    IndexType cols = node["cols"].as<IndexType>();

    if (cols != B) {
      LOG(ERROR) << "The matrix is the wrong size (rows, cols). Wanted: ("
                 << rows << "," << B << "), got (" << rows << ", " << cols
                 << ")";
      return false;
    }

    M.resize(rows, Eigen::NoChange);

    return decodeEigenMatrixData(node["data"], rows, cols, M);
  }

  template <class Scalar, int A, int C, int D, int E>
  static bool decode(
      const Node& node, Eigen::Matrix<Scalar, A, Eigen::Dynamic, C, D, E>& M) {
    if (!node.IsMap()) {
      LOG(ERROR)
          << "Unable to get parse the matrix because the node is not a map.";
      return false;
    }

    typedef typename Eigen::Matrix<Scalar, A, Eigen::Dynamic, C, D, E>::Index
        IndexType;
    IndexType rows = node["rows"].as<IndexType>();
    IndexType cols = node["cols"].as<IndexType>();

    if (rows != A) {
      LOG(ERROR) << "The matrix is the wrong size (rows, cols). Wanted: (" << A
                 << "," << cols << "), got (" << rows << ", " << cols << ")";
      return false;
    }

    M.resize(Eigen::NoChange, cols);

    return decodeEigenMatrixData(node["data"], rows, cols, M);
  }

  template <class Scalar, int C, int D, int E>
  static bool decode(
      const Node& node,
      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, C, D, E>& M) {
    if (!node.IsMap()) {
      LOG(ERROR)
          << "Unable to get parse the matrix because the node is not a map.";
      return false;
    }

    typedef typename Eigen::Matrix<
        Scalar, Eigen::Dynamic, Eigen::Dynamic, C, D, E>::Index IndexType;
    IndexType rows = node["rows"].as<IndexType>();
    IndexType cols = node["cols"].as<IndexType>();

    M.resize(rows, cols);

    return decodeEigenMatrixData(node["data"], rows, cols, M);
  }
};
}  // namespace YAML

#endif  // ASLAM_CV_COMMON_EIGEN_YAML_SERIALIZATION_H_