Lie.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 <gtsam/base/Manifold.h>
#include <gtsam/base/Group.h>

namespace gtsam {

template<class T>
inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);}

template<class T>
inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));}

template<class T>
inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}

template <class T>
class LieConcept {
private:
  static T concept_check(const T& t) {

    T t2 = t;

    size_t dim_ret = t.dim();

    T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));

    Vector logmap_identity_ret = T::Logmap(t);

    T between_ret = expmap_identity_ret.between(t2);

    return between_ret;
  }

};

template<class T>
T BCH(const T& X, const T& Y) {
  static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
  T X_Y = bracket(X, Y);
  return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
      bracket(X, X_Y));
}

template <class T> Matrix wedge(const Vector& x);

template <class T>
T expm(const Vector& x, int K=7) {
  Matrix xhat = wedge<T>(x);
  return T(expm(xhat,K));
}

} // namespace gtsam

#define GTSAM_CONCEPT_LIE_INST(T) \
    template class gtsam::ManifoldConcept<T>; \
    template class gtsam::GroupConcept<T>; \
    template class gtsam::LieConcept<T>;

#define GTSAM_CONCEPT_LIE_TYPE(T) \
    typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
    typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
    typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;