Program Listing for File lie_group_base.h

↰ Return to documentation for file (manif/impl/lie_group_base.h)

#ifndef _MANIF_MANIF_LIE_GROUP_BASE_H_
#define _MANIF_MANIF_LIE_GROUP_BASE_H_

#include "manif/impl/macro.h"
#include "manif/impl/traits.h"
#include "manif/impl/eigen.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/assignment_assert.h"
#include "manif/impl/cast.h"

#include "manif/constants.h"

#include <tl/optional.hpp>

namespace manif {

template <class _Derived>
struct LieGroupBase
{
  static constexpr int Dim     = internal::traits<_Derived>::Dim;
  static constexpr int DoF     = internal::traits<_Derived>::DoF;
  static constexpr int RepSize = internal::traits<_Derived>::RepSize;

  using Scalar         = typename internal::traits<_Derived>::Scalar;
  using LieGroup       = typename internal::traits<_Derived>::LieGroup;
  using DataType       = typename internal::traits<_Derived>::DataType;
  using Tangent        = typename internal::traits<_Derived>::Tangent;
  using Jacobian       = typename internal::traits<_Derived>::Jacobian;
  using Vector         = typename internal::traits<_Derived>::Vector;

  using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;

  template <typename _Scalar>
  using LieGroupTemplate = typename internal::traitscast<LieGroup, _Scalar>::cast;

public:

  static const OptJacobianRef _;

protected:

  MANIF_DEFAULT_CONSTRUCTOR(LieGroupBase)

public:

  _Derived& operator =(const LieGroupBase& m);

  template <typename _DerivedOther>
  _Derived& operator =(const LieGroupBase<_DerivedOther>& m);

  template <typename _EigenDerived>
  _Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& data);

  DataType& coeffs();

  const DataType& coeffs() const;

  Scalar* data();
  const Scalar* data() const;

  template <class _NewScalar>
  LieGroupTemplate<_NewScalar> cast() const;

//  template <class _DerivedOther>
//  LieGroupTemplate<_DerivedOther> as() const;

  _Derived& setIdentity();

  _Derived& setRandom();

  // Minimum API
  // Those functions must be implemented in the Derived class !

  LieGroup inverse(OptJacobianRef J_m_t = {}) const;

  Tangent log(OptJacobianRef J_t_m = {}) const;

  MANIF_DEPRECATED
  Tangent lift(OptJacobianRef J_t_m = {}) const;

  template <typename _DerivedOther>
  LieGroup compose(const LieGroupBase<_DerivedOther>& m,
                   OptJacobianRef J_mc_ma = {},
                   OptJacobianRef J_mc_mb = {}) const;

  template <typename _EigenDerived>
  Vector act(const Eigen::MatrixBase<_EigenDerived>& v,
             tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m = {},
             tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v = {}) const;

  Jacobian adj() const;

  // Deduced API

  template <typename _DerivedOther>
  LieGroup rplus(const TangentBase<_DerivedOther>& t,
                 OptJacobianRef J_mout_m = {},
                 OptJacobianRef J_mout_t = {}) const;

  template <typename _DerivedOther>
  LieGroup lplus(const TangentBase<_DerivedOther>& t,
                 OptJacobianRef J_mout_m = {},
                 OptJacobianRef J_mout_t = {}) const;

  template <typename _DerivedOther>
  LieGroup plus(const TangentBase<_DerivedOther>& t,
                OptJacobianRef J_mout_m = {},
                OptJacobianRef J_mout_t = {}) const;

  template <typename _DerivedOther>
  Tangent rminus(const LieGroupBase<_DerivedOther>& m,
                 OptJacobianRef J_t_ma = {},
                 OptJacobianRef J_t_mb = {}) const;

  template <typename _DerivedOther>
  Tangent lminus(const LieGroupBase<_DerivedOther>& m,
                 OptJacobianRef J_t_ma = {},
                 OptJacobianRef J_t_mb = {}) const;

  template <typename _DerivedOther>
  Tangent minus(const LieGroupBase<_DerivedOther>& m,
                OptJacobianRef J_t_ma = {},
                OptJacobianRef J_t_mb = {}) const;

  template <typename _DerivedOther>
  LieGroup between(const LieGroupBase<_DerivedOther>& m,
                   OptJacobianRef J_mc_ma = {},
                   OptJacobianRef J_mc_mb = {}) const;

  template <typename _DerivedOther>
  bool isApprox(const LieGroupBase<_DerivedOther>& m,
                const Scalar eps = Constants<Scalar>::eps) const;

  // Some operators

  template <typename _DerivedOther>
  bool operator ==(const LieGroupBase<_DerivedOther>& m) const;


  template <typename _DerivedOther>
  bool operator!=(
      const LieGroupBase<_DerivedOther> &m) const {
    return !(*this == m);
  }

  template <typename _DerivedOther>
  LieGroup operator +(const TangentBase<_DerivedOther>& t) const;

  template <typename _DerivedOther>
  _Derived& operator +=(const TangentBase<_DerivedOther>& t);

  template <typename _DerivedOther>
  Tangent operator -(const LieGroupBase<_DerivedOther>& m) const;

  template <typename _DerivedOther>
  LieGroup operator *(const LieGroupBase<_DerivedOther>& m) const;

  template <typename _DerivedOther>
  _Derived& operator *=(const LieGroupBase<_DerivedOther>& m);

  auto operator [](const unsigned int i) const -> decltype(coeffs()[i]){
    return coeffs()[i];
  }

  auto operator [](const unsigned int i) -> decltype(coeffs()[i]){
    return coeffs()[i];
  }

  constexpr unsigned int size() const {
    return RepSize;
  }

  // Some static helpers

  static LieGroup Identity();
  static LieGroup Random();

protected:

  inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); }
  inline const _Derived& derived() const & noexcept { return *static_cast< const _Derived* >(this); }
};

template <typename _Derived>
constexpr int LieGroupBase<_Derived>::Dim;
template <typename _Derived>
constexpr int LieGroupBase<_Derived>::DoF;
template <typename _Derived>
constexpr int LieGroupBase<_Derived>::RepSize;

template <typename _Derived>
const typename LieGroupBase<_Derived>::OptJacobianRef
LieGroupBase<_Derived>::_ = {};

// Copy

template <typename _Derived>
_Derived&
LieGroupBase<_Derived>::operator =(const LieGroupBase& m)
{
  derived().coeffs() = m.coeffs();
  return derived();
}

template <typename _Derived>
template <typename _DerivedOther>
_Derived&
LieGroupBase<_Derived>::operator =(const LieGroupBase<_DerivedOther>& m)
{
  derived().coeffs() = m.coeffs();
  return derived();
}

template <typename _Derived>
template <typename _EigenDerived>
_Derived&
LieGroupBase<_Derived>::operator =(const Eigen::MatrixBase<_EigenDerived>& data)
{
  internal::AssignmentEvaluator<
      typename internal::traits<_Derived>::Base>().run(data);

  derived().coeffs() = data;
  return derived();
}

template <typename _Derived>
typename LieGroupBase<_Derived>::DataType&
LieGroupBase<_Derived>::coeffs()
{
  return derived().coeffs();
}

template <typename _Derived>
const typename LieGroupBase<_Derived>::DataType&
LieGroupBase<_Derived>::coeffs() const
{
  return derived().coeffs();
}

template <typename _Derived>
typename LieGroupBase<_Derived>::Scalar*
LieGroupBase<_Derived>::data()
{
  return derived().coeffs().data();
}

template <typename _Derived>
const typename LieGroupBase<_Derived>::Scalar*
LieGroupBase<_Derived>::data() const
{
  return derived().coeffs().data();
}

template <typename _Derived>
template <class _NewScalar>
typename LieGroupBase<_Derived>::template LieGroupTemplate<_NewScalar>
LieGroupBase<_Derived>::cast() const
{
  return internal::CastEvaluator<
    typename internal::traits<_Derived>::Base, _NewScalar
  >(derived()).run();
}

template <typename _Derived>
_Derived&
LieGroupBase<_Derived>::setIdentity()
{
  const static Tangent zero = Tangent::Zero();
  derived() = zero.exp();
  return derived();
}

template <typename _Derived>
_Derived&
LieGroupBase<_Derived>::setRandom()
{
  internal::RandomEvaluator<
      typename internal::traits<_Derived>::Base>(
        derived()).run();

  return derived();
}

template <typename _Derived>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::inverse(OptJacobianRef J_m_t) const
{
  return derived().inverse(J_m_t);
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::rplus(
    const TangentBase<_DerivedOther>& t,
    OptJacobianRef J_mout_m,
    OptJacobianRef J_mout_t) const
{
  if (J_mout_t)
  {
    (*J_mout_t) = t.rjac();
  }

  return compose(t.exp(), J_mout_m, _);
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::lplus(
    const TangentBase<_DerivedOther>& t,
    OptJacobianRef J_mout_m,
    OptJacobianRef J_mout_t) const
{
  if (J_mout_t)
  {
    J_mout_t->noalias() = inverse().adj() * t.rjac();
  }

  if (J_mout_m)
  {
    J_mout_m->setIdentity();
  }

  return t.exp().compose(derived());
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::plus(
    const TangentBase<_DerivedOther>& t,
    OptJacobianRef J_mout_m,
    OptJacobianRef J_mout_t) const
{
  return derived().rplus(t, J_mout_m, J_mout_t);
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::rminus(
    const LieGroupBase<_DerivedOther>& m,
    OptJacobianRef J_t_ma,
    OptJacobianRef J_t_mb) const
{
  const Tangent t = m.inverse().compose(derived()).log();

  if (J_t_ma)
  {
    (*J_t_ma) = t.rjacinv();
  }
  if (J_t_mb)
  {
    (*J_t_mb) = -(-t).rjacinv();
  }

  return t;
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::lminus(
    const LieGroupBase<_DerivedOther>& m,
    OptJacobianRef J_t_ma,
    OptJacobianRef J_t_mb) const
{
  const Tangent t = compose(m.inverse()).log();

  if (J_t_ma)
  {
    J_t_ma->noalias() = t.rjacinv() * m.adj();

    if (J_t_mb)
    {
      *J_t_mb = -(*J_t_ma);
    }
  }
  else if (J_t_mb)
  {
    J_t_mb->noalias() = -(t.rjacinv() * m.adj());
  }

  return t;
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::minus(
    const LieGroupBase<_DerivedOther>& m,
    OptJacobianRef J_t_ma,
    OptJacobianRef J_t_mb) const
{
  return derived().rminus(m, J_t_ma, J_t_mb);
}

template <typename _Derived>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::log(OptJacobianRef J_t_m) const
{
  return derived().log(J_t_m);
}

template <typename _Derived>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::lift(OptJacobianRef J_t_m) const
{
  return derived().log(J_t_m);
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::compose(
    const LieGroupBase<_DerivedOther>& m,
    OptJacobianRef J_mc_ma,
    OptJacobianRef J_mc_mb) const
{
  return derived().compose(m, J_mc_ma, J_mc_mb);
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::between(
    const LieGroupBase<_DerivedOther>& m,
    OptJacobianRef J_mc_ma,
    OptJacobianRef J_mc_mb) const
{
  const LieGroup mc = inverse().compose(m);

  if (J_mc_ma)
  {
    *J_mc_ma = -(mc.inverse().adj());
  }

  if (J_mc_mb)
  {
    J_mc_mb->setIdentity();
  }

  return mc;
}

template <typename _Derived>
template <typename _DerivedOther>
bool LieGroupBase<_Derived>::isApprox(const LieGroupBase<_DerivedOther>& m,
                                      const Scalar eps) const
{
  return rminus(m).isApprox(Tangent::Zero(), eps);
}

template <typename _Derived>
typename LieGroupBase<_Derived>::Jacobian
LieGroupBase<_Derived>::adj() const
{
  return derived().adj();
}

template <typename _Derived>
template <typename _EigenDerived>
typename LieGroupBase<_Derived>::Vector
LieGroupBase<_Derived>::act(
  const Eigen::MatrixBase<_EigenDerived>& v,
  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m,
  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v
) const
{
  return derived().act(v, J_vout_m, J_vout_v);
}

// Operators

template <typename _Derived>
template <typename _DerivedOther>
bool LieGroupBase<_Derived>::operator ==(
    const LieGroupBase<_DerivedOther>& m) const
{
  return isApprox(m);
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::operator +(
    const TangentBase<_DerivedOther>& t) const
{
  return derived().rplus(t);
}

template <typename _Derived>
template <typename _DerivedOther>
_Derived&
LieGroupBase<_Derived>::operator +=(
    const TangentBase<_DerivedOther>& t)
{
  derived() = derived().rplus(t);
  return derived();
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::operator -(
    const LieGroupBase<_DerivedOther>& m) const
{
  return derived().rminus(m);
}

template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::operator *(
    const LieGroupBase<_DerivedOther>& m) const
{
  return derived().compose(m);
}

template <typename _Derived>
template <typename _DerivedOther>
_Derived&
LieGroupBase<_Derived>::operator *=(
    const LieGroupBase<_DerivedOther>& m)
{
  derived() = derived().compose(m);
  return derived();
}

// Static helpers

template <typename _Derived>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::Identity()
{
  const static LieGroup I(LieGroup().setIdentity());
  return I;
}

template <typename _Derived>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::Random()
{
  return LieGroup().setRandom();
}

// Utils

template <typename _Stream, typename _Derived>
_Stream& operator << (
    _Stream& s,
    const manif::LieGroupBase<_Derived>& m)
{
  s << m.coeffs().transpose();
  return s;
}

} /* namespace manif */

#endif /* _MANIF_MANIF_LIE_GROUP_BASE_H_ */