lie_group_base.hpp

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <Eigen/Core>
 
#include "smooth/version.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
template<typename T>
struct liebase_info
{};
 
template<typename Derived>
class LieGroupBase
{
  Derived & derived() noexcept { return static_cast<Derived &>(*this); }
 
  const Derived & cderived() const noexcept { return static_cast<const Derived &>(*this); }
 
protected:
  LieGroupBase() = default;
 
  using traits = liebase_info<Derived>;
 
  using Impl = typename traits::Impl;
 
  static constexpr bool is_mutable = traits::is_mutable;
 
public:
  static constexpr int RepSize = Impl::RepSize;
  static constexpr int Dof = Impl::Dof;
  static constexpr int Dim = Impl::Dim;
  static constexpr bool IsCommutative = Impl::IsCommutative;
 
  using Scalar = typename traits::Scalar;
  using Matrix = Eigen::Matrix<Scalar, Dim, Dim>;
  using Tangent = Eigen::Matrix<Scalar, Dof, 1>;
  using TangentMap = Eigen::Matrix<Scalar, Dof, Dof>;
  using Hessian = Eigen::Matrix<Scalar, Dof, Dof * Dof>;
  template<typename NewScalar>
  using CastT = typename traits::template PlainObject<NewScalar>;
  using PlainObject = CastT<Scalar>;
 
  template<bool = true>
    requires(is_mutable)
  auto & coeffs() const
  {
    return derived().coeffs();
  }
  const auto & coeffs() const { return cderived().coeffs(); }
 
  template<bool = true>
    requires(is_mutable)
  auto * data() const
  {
    return derived().data();
  }
  const auto * data() const { return cderived().data(); }
 
  template<typename OtherDerived>
    requires(is_mutable && std::is_same_v<Impl, typename liebase_info<OtherDerived>::Impl>)
  Derived & operator=(const LieGroupBase<OtherDerived> & o) noexcept
  {
    derived().coeffs() = static_cast<const OtherDerived &>(o).coeffs();
    return derived();
  }
 
  // Group API
 
  Eigen::Index dof() const noexcept { return Dof; }
 
  void setIdentity() noexcept { Impl::setIdentity(derived().coeffs()); }
 
  void setRandom() noexcept { Impl::setRandom(derived().coeffs()); }
 
  [[nodiscard]] static PlainObject Identity() noexcept
  {
    PlainObject ret;
    ret.setIdentity();
    return ret;
  }
 
  [[nodiscard]] static PlainObject Random() noexcept
  {
    PlainObject ret;
    ret.setRandom();
    return ret;
  }
 
  Matrix matrix() const noexcept
  {
    Matrix ret;
    Impl::matrix(cderived().coeffs(), ret);
    return ret;
  }
 
  template<typename OtherDerived>
    requires(std::is_same_v<Impl, typename liebase_info<OtherDerived>::Impl>)
  bool isApprox(const LieGroupBase<OtherDerived> & o, const Scalar & eps = Eigen::NumTraits<Scalar>::dummy_precision())
    const noexcept
  {
    return cderived().coeffs().isApprox(static_cast<const OtherDerived &>(o).coeffs(), eps);
  }
 
  template<typename NewScalar>
  [[nodiscard]] CastT<NewScalar> cast() const noexcept
  {
    CastT<NewScalar> ret;
    ret.coeffs() = cderived().coeffs().template cast<NewScalar>();
    return ret;
  }
 
  template<typename OtherDerived>
    requires(std::is_same_v<Impl, typename liebase_info<OtherDerived>::Impl>)
  PlainObject operator*(const LieGroupBase<OtherDerived> & o) const noexcept
  {
    PlainObject ret;
    Impl::composition(cderived().coeffs(), static_cast<const OtherDerived &>(o).coeffs(), ret.coeffs());
    return ret;
  }
 
  template<typename OtherDerived>
    requires(is_mutable && std::is_same_v<Impl, typename liebase_info<OtherDerived>::Impl>)
  Derived & operator*=(const LieGroupBase<OtherDerived> & o) noexcept
  {
    derived().coeffs() = (*this * o).coeffs();
    return derived();
  }
 
  [[nodiscard]] PlainObject inverse() const noexcept
  {
    PlainObject ret;
    Impl::inverse(cderived().coeffs(), ret.coeffs());
    return ret;
  }
 
  [[nodiscard]] Tangent log() const noexcept
  {
    Tangent ret;
    Impl::log(cderived().coeffs(), ret);
    return ret;
  }
 
  TangentMap Ad() const noexcept
  {
    if constexpr (IsCommutative) {
      return TangentMap::Identity();
    } else {
      TangentMap ret;
      Impl::Ad(cderived().coeffs(), ret);
      return ret;
    }
  }
 
  template<typename TangentDerived>
  PlainObject operator+(const Eigen::MatrixBase<TangentDerived> & a) const noexcept
  {
    return *this * exp(a);
  }
 
  template<typename TangentDerived>
    requires(is_mutable)
  Derived & operator+=(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    *this *= exp(a);
    return derived();
  }
 
  template<typename OtherDerived>
    requires(std::is_same_v<Impl, typename liebase_info<OtherDerived>::Impl>)
  Tangent operator-(const LieGroupBase<OtherDerived> & xo) const noexcept
  {
    return (xo.inverse() * *this).log();
  }
 
  // Tangent API
 
  template<typename TangentDerived>
  static PlainObject exp(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    PlainObject ret;
    Impl::exp(a, ret.coeffs());
    return ret;
  }
 
  template<typename TangentDerived>
  static Matrix hat(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    Matrix ret;
    Impl::hat(a, ret);
    return ret;
  }
 
  template<typename MatrixDerived>
  static Tangent vee(const Eigen::MatrixBase<MatrixDerived> & A) noexcept
  {
    Tangent ret;
    Impl::vee(A, ret);
    return ret;
  }
 
  template<typename TangentDerived>
  static TangentMap ad(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    if constexpr (IsCommutative) {
      return TangentMap::Zero();
    } else {
      TangentMap ret;
      Impl::ad(a, ret);
      return ret;
    }
  }
 
  template<typename TangentDerived1, typename TangentDerived2>
  static Tangent
  lie_bracket(const Eigen::MatrixBase<TangentDerived1> & a, const Eigen::MatrixBase<TangentDerived2> & b) noexcept
  {
    if constexpr (IsCommutative) {
      return Tangent::Zero();
    } else {
      return ad(a) * b;
    }
  }
 
  template<typename TangentDerived>
  static TangentMap dr_exp(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    if constexpr (IsCommutative) {
      return TangentMap::Identity();
    } else {
      TangentMap ret;
      Impl::dr_exp(a, ret);
      return ret;
    }
  }
 
  template<typename TangentDerived>
  static TangentMap dr_expinv(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    if constexpr (IsCommutative) {
      return TangentMap::Identity();
    } else {
      TangentMap ret;
      Impl::dr_expinv(a, ret);
      return ret;
    }
  }
 
  template<typename TangentDerived>
  static TangentMap dl_exp(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    return dr_exp(-a);
  }
 
  template<typename TangentDerived>
  static TangentMap dl_expinv(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    return dr_expinv(-a);
  }
 
  template<typename TangentDerived>
  static Hessian d2r_exp(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    if constexpr (IsCommutative) {
      return Hessian::Zero();
    } else {
      Hessian ret;
      Impl::d2r_exp(a, ret);
      return ret;
    }
  }
 
  template<typename TangentDerived>
  static Hessian d2r_expinv(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    if constexpr (IsCommutative) {
      return Hessian::Zero();
    } else {
      Hessian ret;
      Impl::d2r_expinv(a, ret);
      return ret;
    }
  }
 
  template<typename TangentDerived>
  static Hessian d2l_exp(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    return -d2r_exp(-a);
  }
 
  template<typename TangentDerived>
  static Hessian d2l_expinv(const Eigen::MatrixBase<TangentDerived> & a) noexcept
  {
    return -d2r_expinv(-a);
  }
};
 
template<typename Stream, typename Derived>
Stream & operator<<(Stream & s, const LieGroupBase<Derived> & g)
{
  s << static_cast<const Derived &>(g).coeffs().transpose();
  return s;
}
 
template<typename T>
class Map;
 
template<typename T>
using MapDispatch = std::conditional_t<
  std::is_base_of_v<Eigen::MatrixBase<std::remove_const_t<T>>, std::remove_const_t<T>>,
  ::Eigen::Map<T>,
  ::smooth::Map<T>>;
 
SMOOTH_END_NAMESPACE