ProductLieGroup.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/Lie.h>
#include <gtsam/base/Testable.h>
 
#include <algorithm>
#include <array>
#include <iostream>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <utility>  // pair
#include <vector>
 
namespace gtsam {
 
namespace internal {

template <typename T>
struct ProductLieGroupIsVector : std::false_type {};
template <int N>
struct ProductLieGroupIsVector<Eigen::Matrix<double, N, 1>> : std::true_type {};
 
}  // namespace internal

template <typename G, typename H>
class ProductLieGroup
    : public std::pair<G, H>,
      public LieGroup<ProductLieGroup<G, H>,
                      internal::dimensionSum(traits<G>::dimension,
                                             traits<H>::dimension)> {
  GTSAM_CONCEPT_ASSERT(IsLieGroup<G>);
  GTSAM_CONCEPT_ASSERT(IsLieGroup<H>);
  GTSAM_CONCEPT_ASSERT(IsTestable<G>);
  GTSAM_CONCEPT_ASSERT(IsTestable<H>);
 
 public:
  using This = ProductLieGroup<G, H>;
  using Base = std::pair<G, H>;
  using LieBase = LieGroup<This, internal::dimensionSum(traits<G>::dimension,
                                                        traits<H>::dimension)>;
 
 protected:
  inline constexpr static int n = traits<G>::dimension;
  inline constexpr static int m = traits<H>::dimension;
  inline constexpr static bool firstDynamic = n == Eigen::Dynamic;
  inline constexpr static bool secondDynamic = m == Eigen::Dynamic;
 
 public:
  using LieBase::Dim;
  using LieBase::dimension;
  using TangentVector = typename LieBase::TangentVector;
  using ChartJacobian = typename LieBase::ChartJacobian;
  using Jacobian = typename LieBase::Jacobian;
  using Jacobian1 = typename traits<G>::Jacobian;
  using Jacobian2 = typename traits<H>::Jacobian;

  struct ChartAtOrigin {
    static This Retract(const TangentVector& v, ChartJacobian Hv = {}) {
      if constexpr (dimension == Eigen::Dynamic) {
        const This identity = This::Expmap(TangentVector::Zero(v.size()));
        return identity.retract(v, {}, Hv);
      } else {
        return This::Identity().retract(v, {}, Hv);
      }
    }
 
    static TangentVector Local(const This& value, ChartJacobian Hv = {}) {
      if constexpr (dimension == Eigen::Dynamic) {
        const This identity(traits<G>::Between(value.first, value.first),
                            traits<H>::Between(value.second, value.second));
        return identity.localCoordinates(value, {}, Hv);
      } else {
        return This::Identity().localCoordinates(value, {}, Hv);
      }
    }
  };
 
 public:

  ProductLieGroup() : Base(defaultIdentity<G>(), defaultIdentity<H>()) {}

  ProductLieGroup(const G& g, const H& h) : Base(g, h) {}

  ProductLieGroup(const Base& base) : Base(base) {}

 
  typedef multiplicative_group_tag group_flavor;

  static ProductLieGroup Identity() { return ProductLieGroup(); }

  ProductLieGroup operator*(const ProductLieGroup& other) const;

  ProductLieGroup inverse() const;
 
  using LieBase::between;
  using LieBase::compose;
  using LieBase::expmap;
  using LieBase::inverse;
  using LieBase::logmap;


  size_t dim() const { return firstDim() + secondDim(); }

  ProductLieGroup retract(const TangentVector& v, ChartJacobian H1 = {},
                          ChartJacobian H2 = {}) const;

  TangentVector localCoordinates(const ProductLieGroup& g,
                                 ChartJacobian H1 = {},
                                 ChartJacobian H2 = {}) const;


  static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {});

  static ProductLieGroup Expmap(
      const Eigen::Ref<const typename traits<G>::TangentVector>& v1,
      const Eigen::Ref<const typename traits<H>::TangentVector>& v2,
      OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> H1 = {},
      OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> H2 = {});

  static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {});

  static TangentVector LocalCoordinates(const ProductLieGroup& p,
                                        ChartJacobian Hp = {}) {
    return Logmap(p, Hp);
  }

  Jacobian AdjointMap() const;

  static Jacobian adjointMap(const TangentVector& xi);

 
 protected:
  template <typename T>
  static T defaultIdentity();
 
  size_t firstDim() const { return traits<G>::GetDimension(this->first); }
  size_t secondDim() const { return traits<H>::GetDimension(this->second); }

  template <typename T, int ComponentDimension = traits<T>::dimension>
  static typename traits<T>::TangentVector tangentSegment(
      const TangentVector& v, size_t start, size_t d);

  template <typename T>
  static typename traits<T>::Jacobian componentAdjointMap(
      const typename traits<T>::TangentVector& xi);

  static TangentVector makeTangentVector(
      const typename traits<G>::TangentVector& v1,
      const typename traits<H>::TangentVector& v2, size_t d1, size_t d2);

  static Jacobian zeroJacobian(size_t d);

  void checkMatchingDimensions(const ProductLieGroup& other,
                               const char* operation) const;
 
 public:
  void print(const std::string& s = "") const;
 
  bool equals(const ProductLieGroup& other, double tol = 1e-9) const {
    return traits<G>::Equals(this->first, other.first, tol) &&
           traits<H>::Equals(this->second, other.second, tol);
  }
};

template <typename T, int N>
struct PowerLieGroupJacobianStorage {
  using type = std::array<T, N>;
};
 
template <typename T>
struct PowerLieGroupJacobianStorage<T, Eigen::Dynamic> {
  using type = std::vector<T>;
};
 
template <typename G, int N, typename Derived>
class PowerLieGroupBase
    : public LieGroup<Derived,
                      internal::dimensionProduct(N, traits<G>::dimension)> {
 protected:
  static constexpr bool isDynamic = (N == Eigen::Dynamic);
  static constexpr int n = traits<G>::dimension;
 
 public:
  using LieBase =
      LieGroup<Derived, internal::dimensionProduct(N, traits<G>::dimension)>;
  typedef multiplicative_group_tag group_flavor;
  using LieBase::Dim;
  using LieBase::dimension;
  using TangentVector = typename LieBase::TangentVector;
  using ChartJacobian = typename LieBase::ChartJacobian;
  using Jacobian = typename LieBase::Jacobian;

  struct ChartAtOrigin {
    static Derived Retract(const TangentVector& v, ChartJacobian H = {}) {
      if constexpr (isDynamic) {
        if (v.size() % n != 0) {
          throw std::invalid_argument(
              "PowerLieGroup::Retract tangent dimension must be divisible by "
              "base group dimension");
        }
        const size_t count =
            static_cast<size_t>(v.size() / static_cast<Eigen::Index>(n));
        return Derived(count).retract(v, {}, H);
      } else {
        return Derived::Identity().retract(v, {}, H);
      }
    }
 
    static TangentVector Local(const Derived& value, ChartJacobian H = {}) {
      if constexpr (isDynamic) {
        return Derived(value.size()).localCoordinates(value, {}, H);
      } else {
        return Derived::Identity().localCoordinates(value, {}, H);
      }
    }
  };
 
  using BaseJacobian = typename traits<G>::Jacobian;
  using JacobianStorage =
      typename PowerLieGroupJacobianStorage<BaseJacobian, N>::type;
 
 protected:
  const Derived& derived() const { return static_cast<const Derived&>(*this); }

  Derived& derived() { return static_cast<Derived&>(*this); }

  static size_t totalDimension(size_t count) {
    return count * static_cast<size_t>(n);
  }

  static Eigen::Index offset(size_t i) {
    return static_cast<Eigen::Index>(i * static_cast<size_t>(n));
  }

  size_t componentCount() const {
    if constexpr (isDynamic) {
      return derived().size();
    } else {
      return N;
    }
  }

  static void checkDynamicTangentSize(const TangentVector& v, size_t count,
                                      const char* operation);

  void checkMatchingCounts(const Derived& other, const char* operation) const;

  static typename traits<G>::TangentVector tangentSegment(
      const TangentVector& v, size_t i);

  static Derived makeResult(size_t count);

  static JacobianStorage makeJacobianStorage(size_t count);

  static void assignTangentSegment(TangentVector& v, size_t i,
                                   const typename traits<G>::TangentVector& vi);

  template <typename MatrixType>
  static void assignJacobianBlock(MatrixType& H, size_t i,
                                  const BaseJacobian& block);

  static void fillJacobianBlocks(ChartJacobian H,
                                 const JacobianStorage& jacobians,
                                 size_t count);
 
 public:
  size_t dim() const { return totalDimension(componentCount()); }

  Derived operator*(const Derived& other) const;

  Derived inverse() const;
 
  using LieBase::between;
  using LieBase::compose;
  using LieBase::expmap;
  using LieBase::inverse;
  using LieBase::logmap;

  Derived retract(const TangentVector& v, ChartJacobian H1 = {},
                  ChartJacobian H2 = {}) const;

  TangentVector localCoordinates(const Derived& g, ChartJacobian H1 = {},
                                 ChartJacobian H2 = {}) const;

  static Derived Expmap(const TangentVector& v, ChartJacobian Hv = {});

  static TangentVector Logmap(const Derived& p, ChartJacobian Hp = {});

  static TangentVector LocalCoordinates(const Derived& p,
                                        ChartJacobian Hp = {}) {
    return Logmap(p, Hp);
  }

  Jacobian AdjointMap() const;

  void print(const std::string& s = "") const;

  bool equals(const Derived& other, double tol = 1e-9) const;
 
 protected:
  static TangentVector zeroTangent(size_t count);

  static Jacobian zeroJacobian(size_t count);
};

template <typename G, int N>
class PowerLieGroup : public std::array<G, N>,
                      public PowerLieGroupBase<G, N, PowerLieGroup<G, N>> {
  static_assert(N >= 1, "PowerLieGroup requires N >= 1");
  GTSAM_CONCEPT_ASSERT(IsLieGroup<G>);
  GTSAM_CONCEPT_ASSERT(IsTestable<G>);
  static_assert(traits<G>::dimension != Eigen::Dynamic,
                "PowerLieGroup requires a fixed-size base group");
 
 public:
  typedef std::array<G, N> Base;
  typedef PowerLieGroupBase<G, N, PowerLieGroup> Helper;
  using Helper::Dim;
  using Helper::dimension;
  using typename Helper::BaseJacobian;
  using typename Helper::ChartJacobian;
  using typename Helper::Jacobian;
  using typename Helper::TangentVector;
 
 public:

  PowerLieGroup() { this->fill(traits<G>::Identity()); }

  PowerLieGroup(const Base& elements) : Base(elements) {}

  PowerLieGroup(const std::initializer_list<G>& elements);


  static PowerLieGroup Identity() { return PowerLieGroup(); }

};

template <typename G>
class PowerLieGroup<G, Eigen::Dynamic>
    : public std::vector<G>,
      public PowerLieGroupBase<G, Eigen::Dynamic,
                               PowerLieGroup<G, Eigen::Dynamic>> {
  GTSAM_CONCEPT_ASSERT(IsLieGroup<G>);
  GTSAM_CONCEPT_ASSERT(IsTestable<G>);
  static_assert(traits<G>::dimension != Eigen::Dynamic,
                "PowerLieGroup requires a fixed-size base group");
 
 public:
  typedef std::vector<G> Base;
  typedef PowerLieGroupBase<G, Eigen::Dynamic, PowerLieGroup> Helper;
  using Helper::Dim;
  using Helper::dimension;
  using typename Helper::BaseJacobian;
  using typename Helper::ChartJacobian;
  using typename Helper::Jacobian;
  using typename Helper::TangentVector;
 
 public:

  PowerLieGroup() = default;

  explicit PowerLieGroup(size_t count) : Base(count, traits<G>::Identity()) {}

  PowerLieGroup(const Base& elements) : Base(elements) {}

  PowerLieGroup(const std::initializer_list<G>& elements) : Base(elements) {}


  static PowerLieGroup Identity() { return PowerLieGroup(); }

};

template <typename G, typename H>
struct traits<ProductLieGroup<G, H>>
    : internal::LieGroup<ProductLieGroup<G, H>> {};

template <typename G, int N>
struct traits<PowerLieGroup<G, N>> : internal::LieGroup<PowerLieGroup<G, N>> {};
 
}  // namespace gtsam
 
#include <gtsam/base/ProductLieGroup-inl.h>