TangentLieGroup.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/GroupAction.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Testable.h>
 
#include <iostream>
#include <string>
#include <type_traits>
#include <utility>
 
namespace gtsam {
namespace internal {

template <typename T, typename = void>
struct TangentLieGroupHasAdjointMap : std::false_type {};
template <typename T>
struct TangentLieGroupHasAdjointMap<
    T, std::void_t<decltype(T::adjointMap(
           std::declval<const typename traits<T>::TangentVector&>()))>>
    : std::true_type {};

template <typename G>
struct TangentLieGroupJacobian {
  static constexpr bool available = false;
  static constexpr bool expmapAvailable = false;
};
 
}  // namespace internal

template <typename G>
struct AdjointAction : public GroupAction<AdjointAction<G>, G,
                                          typename traits<G>::TangentVector> {
  static constexpr ActionType type = ActionType::Left;
  using TangentVector = typename traits<G>::TangentVector;
  static constexpr int n = traits<G>::dimension;
  static_assert(internal::TangentLieGroupHasAdjointMap<G>::value,
                "AdjointAction<G> requires G::adjointMap(TangentVector)");
 
  TangentVector operator()(const G& g, const TangentVector& v,
                           OptionalJacobian<n, n> Hg = {},
                           OptionalJacobian<n, n> Hv = {}) const {
    const typename traits<G>::Jacobian Ad = traits<G>::AdjointMap(g);
    if (Hv) *Hv = Ad;
    if (Hg) *Hg = -(Ad * G::adjointMap(v));
    return Ad * v;
  }
 
  static typename traits<G>::Jacobian generator(const TangentVector& u) {
    return G::adjointMap(u);
  }
};

template <typename G>
class TangentLieGroup
    : public std::pair<G, typename traits<G>::TangentVector>,
      public LieGroup<TangentLieGroup<G>,
                      internal::dimensionProduct(2, traits<G>::dimension)> {
  GTSAM_CONCEPT_ASSERT(IsLieGroup<G>);
  GTSAM_CONCEPT_ASSERT(IsTestable<G>);
  static_assert(traits<G>::dimension != Eigen::Dynamic,
                "TangentLieGroup requires a fixed-dimensional base group");
  static_assert(internal::TangentLieGroupHasAdjointMap<G>::value,
                "TangentLieGroup requires G::adjointMap(TangentVector)");
 
 private:
  inline constexpr static int n = traits<G>::dimension;
 
 public:
  using This = TangentLieGroup<G>;
  using Base = std::pair<G, typename traits<G>::TangentVector>;
  using LieBase = LieGroup<This, internal::dimensionProduct(2, n)>;
  using LieBase::Dim;
  using LieBase::dimension;
  using BaseTangent = typename traits<G>::TangentVector;
  using BaseJacobian = typename traits<G>::Jacobian;
  using TangentVector = typename LieBase::TangentVector;
  using Jacobian = typename LieBase::Jacobian;
  using ChartJacobian = typename LieBase::ChartJacobian;
  using ChartAtOrigin = internal::ChartAtIdentity<This, dimension>;
  using SplitJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
  using group_flavor = multiplicative_group_tag;
 
  using LieBase::between;
  using LieBase::compose;
  using LieBase::expmap;
  using LieBase::inverse;
  using LieBase::logmap;
 
  TangentLieGroup() : Base(traits<G>::Identity(), BaseTangent::Zero()) {}
  TangentLieGroup(const G& g, const BaseTangent& v) : Base(g, v) {}
  TangentLieGroup(const Base& base) : Base(base) {}
 
  static TangentLieGroup Identity() { return TangentLieGroup(); }
  constexpr size_t dim() const { return dimension; }
 
  TangentLieGroup operator*(const TangentLieGroup& other) const;
  TangentLieGroup inverse() const;
  TangentLieGroup retract(const TangentVector& xi, ChartJacobian H1 = {},
                          ChartJacobian H2 = {}) const;
  TangentVector localCoordinates(const TangentLieGroup& other,
                                 ChartJacobian H1 = {},
                                 ChartJacobian H2 = {}) const;
 
  static TangentLieGroup Expmap(const TangentVector& xi, ChartJacobian H = {});
  static TangentLieGroup Expmap(const Eigen::Ref<const BaseTangent>& u,
                                const Eigen::Ref<const BaseTangent>& v,
                                SplitJacobian H1 = {}, SplitJacobian H2 = {});
  static TangentVector Logmap(const TangentLieGroup& p, ChartJacobian H = {});
  static TangentVector LocalCoordinates(const TangentLieGroup& p,
                                        ChartJacobian H = {}) {
    return Logmap(p, H);
  }
 
  Jacobian AdjointMap() const;
  static Jacobian adjointMap(const TangentVector& xi);
 
  void print(const std::string& s = "") const;
  bool equals(const TangentLieGroup& other, double tol = 1e-9) const {
    return traits<G>::Equals(this->first, other.first, tol) &&
           traits<BaseTangent>::Equals(this->second, other.second, tol);
  }
 
 private:
  static std::pair<BaseTangent, BaseTangent> split(const TangentVector& xi);
  static TangentVector join(const BaseTangent& u, const BaseTangent& v);
  static Jacobian rightJacobian(const TangentVector& xi);
};
 
template <typename G>
struct traits<TangentLieGroup<G>> : internal::LieGroup<TangentLieGroup<G>> {};
 
}  // namespace gtsam
 
#include <gtsam/base/TangentLieGroup-inl.h>