Program Listing for File tangent_base.h

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

#ifndef _MANIF_MANIF_TANGENT_BASE_H_
#define _MANIF_MANIF_TANGENT_BASE_H_

#include "manif/impl/macro.h"
#include "manif/impl/traits.h"
#include "manif/impl/generator.h"
#include "manif/impl/random.h"
#include "manif/impl/bracket.h"
#include "manif/impl/vee.h"
#include "manif/impl/eigen.h"

#include "manif/constants.h"

#include <tl/optional.hpp>

namespace manif {

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

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

  using InnerWeightsMatrix = Jacobian;

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

  template <typename _Scalar>
  using TangentTemplate = typename internal::traitscast<Tangent, _Scalar>::cast;

protected:

  MANIF_DEFAULT_CONSTRUCTOR(TangentBase)

public:

  _Derived& operator =(const TangentBase& t);

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

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

  DataType& coeffs();
  const DataType& coeffs() const;

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

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

  // Common Tangent API

  _Derived& setZero();

  _Derived& setRandom();

  template <typename _EigenDerived>
  _Derived& setVee(const Eigen::MatrixBase<_EigenDerived>& a);

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

  LieAlg generator(const int i) const;

  InnerWeightsMatrix innerWeights() const;

  template <typename _DerivedOther>
  Scalar inner(const TangentBase<_DerivedOther>& t) const;

  Scalar weightedNorm() const;

  Scalar squaredWeightedNorm() const;

  LieAlg hat() const;

  LieGroup exp(OptJacobianRef J_m_t =
                OptJacobianRef{}) const;

  MANIF_DEPRECATED
  LieGroup retract(OptJacobianRef J_m_t =
                    OptJacobianRef{}) const;

  LieGroup rplus(const LieGroup& m,
                 OptJacobianRef J_mout_t = {},
                 OptJacobianRef J_mout_m = {}) const;

  LieGroup lplus(const LieGroup& m,
                 OptJacobianRef J_mout_t = {},
                 OptJacobianRef J_mout_m = {}) const;

  LieGroup plus(const LieGroup& m,
                OptJacobianRef J_mout_t = {},
                OptJacobianRef J_mout_m = {}) const;

  template <typename _DerivedOther>
  Tangent plus(const TangentBase<_DerivedOther>& t,
               OptJacobianRef J_mout_ta = {},
               OptJacobianRef J_mout_tb = {}) const;

  template <typename _DerivedOther>
  Tangent minus(const TangentBase<_DerivedOther>& t,
                OptJacobianRef J_mout_ta = {},
                OptJacobianRef J_mout_tb = {}) const;

  Jacobian rjac() const;

  Jacobian ljac() const;

  template <typename U = _Derived>
  typename std::enable_if<
    internal::has_rjacinv<U>::value,
    typename TangentBase<U>::Jacobian>::type rjacinv() const;

  template <typename U = _Derived>
  typename std::enable_if<
    ! internal::has_rjacinv<U>::value,
    typename TangentBase<U>::Jacobian>::type rjacinv() const;

  template <typename U = _Derived>
  typename std::enable_if<
    internal::has_ljacinv<U>::value,
    typename TangentBase<U>::Jacobian>::type ljacinv() const;

  template <typename U = _Derived>
  typename std::enable_if<
    ! internal::has_ljacinv<U>::value,
    typename TangentBase<U>::Jacobian>::type ljacinv() const;

  Jacobian smallAdj() const;

  template <typename _DerivedOther>
  Tangent bracket(const TangentBase<_DerivedOther>& b) const;

  template <typename _EigenDerived>
  bool isApprox(const Eigen::MatrixBase<_EigenDerived>& v,
                const Scalar eps = Constants<Scalar>::eps) const;

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

  // Some operators

  // Copy assignment

  template <typename T>
  auto operator <<(T&& v)
  ->decltype( std::declval<DataType>().operator<<(std::forward<T>(v)) );

  // Math

  Tangent operator -() const;

  LieGroup operator +(const LieGroup& m) const;

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

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

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

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

  Tangent operator *=(const Scalar scalar);

  Tangent operator /=(const Scalar scalar);

  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;
  }

  // static helpers

  static Tangent Zero();
  static Tangent Random();
  static LieAlg Generator(const int i);
  static InnerWeightsMatrix InnerWeights();

  template <typename _DerivedOther>
  static Tangent Bracket(
    const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b
  );

  template <typename _EigenDerived>
  static Tangent Vee(const Eigen::MatrixBase<_EigenDerived>& alg);

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 TangentBase<_Derived>::Dim;
template <typename _Derived>
constexpr int TangentBase<_Derived>::DoF;
template <typename _Derived>
constexpr int TangentBase<_Derived>::RepSize;

// Copy

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

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

template <typename _Derived>
template <typename _EigenDerived>
_Derived&
TangentBase<_Derived>::operator =(const Eigen::MatrixBase<_EigenDerived>& v)
{
  coeffs() = v;
  return derived();
}

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

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

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

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

template <typename _Derived>
template <class _NewScalar>
typename TangentBase<_Derived>::template TangentTemplate<_NewScalar>
TangentBase<_Derived>::cast() const
{
  return TangentTemplate<_NewScalar>(coeffs().template cast<_NewScalar>());
}

template <class _Derived>
_Derived& TangentBase<_Derived>::setZero()
{
  coeffs().setZero();
  return derived();
}

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

  return derived();
}

template <class _Derived>
template <typename _EigenDerived>
_Derived& TangentBase<_Derived>::setVee(const Eigen::MatrixBase<_EigenDerived>& a) {
  internal::VeeEvaluator<typename internal::traits<_Derived>::Base>(derived()).run(a);
  return derived();
}

template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
  return derived().exp(J_m_t);
}

template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
  return derived().exp(J_m_t);
}

template <typename _Derived>
typename TangentBase<_Derived>::LieAlg
TangentBase<_Derived>::generator(const int i) const
{
  return Generator(i);
}

template <typename _Derived>
typename TangentBase<_Derived>::InnerWeightsMatrix
TangentBase<_Derived>::innerWeights() const
{
  return InnerWeights();
}

template <typename _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Scalar
TangentBase<_Derived>::inner(const TangentBase<_DerivedOther>& t) const
{
  return (coeffs().transpose() * InnerWeights() * t.coeffs())(0);
}

template <class _Derived>
typename TangentBase<_Derived>::Scalar
TangentBase<_Derived>::weightedNorm() const
{
  using std::sqrt;
  return sqrt( squaredWeightedNorm() );
}

template <class _Derived>
typename TangentBase<_Derived>::Scalar
TangentBase<_Derived>::squaredWeightedNorm() const
{
  return (coeffs().transpose() * InnerWeights() * coeffs())(0);
}

template <class _Derived>
typename TangentBase<_Derived>::LieAlg
TangentBase<_Derived>::hat() const
{
  return derived().hat();
}

template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::rplus(const LieGroup& m,
                             OptJacobianRef J_mout_t,
                             OptJacobianRef J_mout_m) const
{
  return m.rplus(derived(), J_mout_m, J_mout_t);
}

template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::lplus(const LieGroup& m,
                             OptJacobianRef J_mout_t,
                             OptJacobianRef J_mout_m) const
{
  return m.lplus(derived(), J_mout_m, J_mout_t);
}

template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::plus(const LieGroup& m,
                            OptJacobianRef J_mout_t,
                            OptJacobianRef J_mout_m) const
{
  return m.lplus(derived(), J_mout_m, J_mout_t);
}

template <class _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::plus(const TangentBase<_DerivedOther>& t,
                            OptJacobianRef J_mout_ta,
                            OptJacobianRef J_mout_tb) const
{
  if (J_mout_ta)
    J_mout_ta->setIdentity();

  if (J_mout_tb)
    J_mout_tb->setIdentity();

  return *this + t;
}

template <class _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::minus(const TangentBase<_DerivedOther>& t,
                             OptJacobianRef J_mout_ta,
                             OptJacobianRef J_mout_tb) const
{
  if (J_mout_ta)
    J_mout_ta->setIdentity();

  if (J_mout_tb)
  {
    J_mout_tb->setIdentity();
    (*J_mout_tb) *= Scalar(-1);
  }

  return *this - t;
}

template <class _Derived>
typename TangentBase<_Derived>::Jacobian
TangentBase<_Derived>::rjac() const
{
  return derived().rjac();
}

template <class _Derived>
typename TangentBase<_Derived>::Jacobian
TangentBase<_Derived>::ljac() const
{
  return derived().ljac();
}


template <class _Derived>
template <typename U>
typename std::enable_if<
  internal::has_rjacinv<U>::value,
  typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::rjacinv() const
{
  return derived().rjacinv();
}

template <class _Derived>
template <typename U>
typename std::enable_if<
  ! internal::has_rjacinv<U>::value,
  typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::rjacinv() const
{
  return derived().rjac().inverse();
}

template <class _Derived>
template <typename U>
typename std::enable_if<
  internal::has_ljacinv<U>::value,
  typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::ljacinv() const
{
  return derived().ljacinv();
}

template <class _Derived>
template <typename U>
typename std::enable_if<
  ! internal::has_ljacinv<U>::value,
  typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::ljacinv() const
{
  return derived().ljac().inverse();
}

template <class _Derived>
typename TangentBase<_Derived>::Jacobian
TangentBase<_Derived>::smallAdj() const
{
  return derived().smallAdj();
}

template <typename _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::bracket(
  const TangentBase<_DerivedOther>& b
) const {
  return internal::BracketEvaluator<
    typename internal::traits<_Derived>::Base,
    typename internal::traits<_DerivedOther>::Base
  >(derived(), b.derived()).run();
}

template <typename _Derived>
template <typename _EigenDerived>
bool TangentBase<_Derived>::isApprox(
    const Eigen::MatrixBase<_EigenDerived>& t,
    const Scalar eps) const
{
  using std::min;
  bool result = false;

  if (min(coeffs().norm(), t.norm()) < eps)
  {
    result = ((coeffs() - t).isZero(eps));
  }
  else
  {
    result = (coeffs().isApprox(t, eps));
  }

  return result;
}

template <typename _Derived>
template <typename _DerivedOther>
bool TangentBase<_Derived>::isApprox(
    const TangentBase<_DerivedOther>& t,
    const Scalar eps) const
{
  return isApprox(t.coeffs(), eps);
}

// Operators

template <typename _Derived>
template <typename T>
auto TangentBase<_Derived>::operator <<(T&& v)
->decltype( std::declval<DataType>().operator<<(std::forward<T>(v)) )
{
  return coeffs().operator<<(std::forward<T>(v));
}

// Static helper

template <class _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::Zero()
{
  static const Tangent t(DataType::Zero());
  return t;
}

template <class _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::Random()
{
  return Tangent().setRandom();
}

template <typename _Derived>
typename TangentBase<_Derived>::LieAlg
TangentBase<_Derived>::Generator(const int i)
{
  return internal::GeneratorEvaluator<
      typename internal::traits<_Derived>::Base>::run(i);
}

template <typename _Derived>
typename TangentBase<_Derived>::InnerWeightsMatrix
TangentBase<_Derived>::InnerWeights()
{
  return internal::InnerWeightsEvaluator<
      typename internal::traits<_Derived>::Base>::run();
}

template <typename _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Bracket(
  const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b
) {
  return a.bracket(b);
}

template <typename _Derived>
template <typename _EigenDerived>
typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Vee(
  const Eigen::MatrixBase<_EigenDerived>& alg
) {
  return Tangent().setVee(alg);
}

// Math

template <typename _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::operator -() const
{
  return Tangent(-coeffs());
}

template <typename _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::operator +(const LieGroup& m) const
{
  return m.lplus(derived());
}

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

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

template <typename _Derived, typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
operator +(const TangentBase<_Derived>& ta,
           const TangentBase<_DerivedOther>& tb)
{
  typename TangentBase<_Derived>::Tangent tc(ta);
  return tc += tb;
}

template <typename _Derived, typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
operator -(const TangentBase<_Derived>& ta,
           const TangentBase<_DerivedOther>& tb)
{
  typename TangentBase<_Derived>::Tangent tc(ta);
  return tc -= tb;
}

template <typename _Derived>
template <typename _EigenDerived>
_Derived& TangentBase<_Derived>::operator +=(
    const Eigen::MatrixBase<_EigenDerived>& v)
{
  coeffs() += v;
  return derived();
}

template <typename _Derived>
template <typename _EigenDerived>
_Derived& TangentBase<_Derived>::operator -=(
    const Eigen::MatrixBase<_EigenDerived>& v)
{
  coeffs() -= v;
  return derived();
}

template <typename _Derived, typename _EigenDerived>
typename TangentBase<_Derived>::Tangent
operator +(const TangentBase<_Derived>& t,
           const Eigen::MatrixBase<_EigenDerived>& v)
{
  typename TangentBase<_Derived>::Tangent ret(t);
  return ret += v;
}

template <typename _Derived, typename _EigenDerived>
typename TangentBase<_Derived>::Tangent
operator -(const TangentBase<_Derived>& t,
           const Eigen::MatrixBase<_EigenDerived>& v)
{
  typename TangentBase<_Derived>::Tangent ret(t);
  return ret -= v;
}

template <typename _EigenDerived, typename _Derived>
auto
operator +(const Eigen::MatrixBase<_EigenDerived>& v,
           const TangentBase<_Derived>& t)
-> decltype(v + t.coeffs())
{
  return v + t.coeffs();
}

template <typename _EigenDerived, typename _Derived>
auto
operator -(const Eigen::MatrixBase<_EigenDerived>& v,
           const TangentBase<_Derived>& t)
-> decltype(v - t.coeffs())
{
  return v - t.coeffs();
}

template <typename _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::operator *=(const Scalar scalar)
{
  coeffs() *= scalar;
  return derived();
}

template <typename _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::operator /=(const Scalar scalar)
{
  coeffs() /= scalar;
  return derived();
}

template <typename _Derived>
typename TangentBase<_Derived>::Tangent
operator *(const TangentBase<_Derived>& t,
           const typename _Derived::Scalar scalar)
{
  typename TangentBase<_Derived>::Tangent ret(t);
  return ret *= scalar;
}

template <typename _Derived>
typename TangentBase<_Derived>::Tangent
operator *(const typename _Derived::Scalar scalar,
           const TangentBase<_Derived>& t)
{
  return t * scalar;
}

template <typename _Derived>
typename TangentBase<_Derived>::Tangent
operator /(const TangentBase<_Derived>& t,
           const typename _Derived::Scalar scalar)
{
  typename TangentBase<_Derived>::Tangent ret(t);
  return ret /= scalar;
}

template <class _DerivedOther>
typename TangentBase<_DerivedOther>::Tangent
operator *(const typename TangentBase<_DerivedOther>::Jacobian& J,
           const TangentBase<_DerivedOther>& t)
{
  return typename TangentBase<_DerivedOther>::Tangent(
        typename TangentBase<_DerivedOther>::DataType(J*t.coeffs()));
}

template <typename _Derived, typename _DerivedOther>
bool operator ==(
    const TangentBase<_Derived>& ta,
    const TangentBase<_DerivedOther>& tb)
{
  return ta.isApprox(tb);
}

template <typename _Derived, typename _EigenDerived>
bool operator ==(
    const TangentBase<_Derived>& t,
    const Eigen::MatrixBase<_EigenDerived>& v)
{
  return t.isApprox(v);
}

template <typename _Derived, typename _DerivedOther>
bool operator !=(
    const TangentBase<_Derived>& ta,
    const TangentBase<_DerivedOther>& tb)
{
  return !(ta == tb);
}

template <typename _Derived, typename _EigenDerived>
bool operator !=(
    const TangentBase<_Derived>& t,
    const Eigen::MatrixBase<_EigenDerived>& v)
{
  return !(t == v);
}

// Utils

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

} /* namespace manif */

#endif /* _MANIF_MANIF_TANGENT_BASE_H_ */