LieMatrix.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <cstdarg>

#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <boost/serialization/nvp.hpp>

namespace gtsam {

struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {


  LieMatrix() {}

  LieMatrix(const Matrix& v) : Matrix(v) {}

// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)

  template<int M, int N>
  LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
#endif

  LieMatrix(size_t m, size_t n, const double* const data) :
      Matrix(Eigen::Map<const Matrix>(data, m, n)) {}


  GTSAM_EXPORT void print(const std::string& name="") const;

  inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
    return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
  }
  

  inline Matrix matrix() const {
    return static_cast<Matrix>(*this);
  }
  

  inline size_t dim() const { return this->size(); }

  inline LieMatrix retract(const Vector& v) const {
    if(v.size() != this->size())
      throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");

    return LieMatrix(*this +
      Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
      &v(0), this->rows(), this->cols()));
  }

  inline Vector localCoordinates(const LieMatrix& t2) const {
    Vector result(this->size());
    Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
      &result(0), this->rows(), this->cols()) = t2 - *this;
    return result;
  }


  inline static LieMatrix identity() {
    throw std::runtime_error("LieMatrix::identity(): Don't use this function");
    return LieMatrix();
  }

  // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments
  // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class
  // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector
  // as the other geometry objects (Point3, Rot3, etc.) have this problem
  inline LieMatrix compose(const LieMatrix& p,
      boost::optional<gtsam::Matrix&> H1=boost::none,
      boost::optional<gtsam::Matrix&> H2=boost::none) const {
    if(H1) *H1 = eye(dim());
    if(H2) *H2 = eye(p.dim());

    return LieMatrix(*this + p);
  }

  inline LieMatrix between(const LieMatrix& l2,
      boost::optional<gtsam::Matrix&> H1=boost::none,
      boost::optional<gtsam::Matrix&> H2=boost::none) const {
    if(H1) *H1 = -eye(dim());
    if(H2) *H2 = eye(l2.dim());
    return LieMatrix(l2 - *this);
  }

  inline LieMatrix inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
    if(H) *H = -eye(dim());

    return LieMatrix(-(*this));
  }


  static inline LieMatrix Expmap(const Vector& v) {
    throw std::runtime_error("LieMatrix::Expmap(): Don't use this function");
    return LieMatrix(v); }

  static inline Vector Logmap(const LieMatrix& p) {
    Vector result(p.size());
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(
        result.data(), p.rows(), p.cols()) = p;
    return result;
  }
  

private:

  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & boost::serialization::make_nvp("LieMatrix",
       boost::serialization::base_object<Value>(*this));
    ar & boost::serialization::make_nvp("Matrix",
       boost::serialization::base_object<Matrix>(*this));

  }

};
} // \namespace gtsam