lie_sparse.hpp

Go to the documentation of this file.

// Copyright (C) 2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <Eigen/Sparse>
 
#include "detail/utils.hpp"
#include "lie_groups.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
namespace traits {
 
template<typename G>
struct lie_sparse
{};
 
}  // namespace traits
 
 
template<LieGroup G>
inline std::array<Eigen::SparseMatrix<Scalar<G>>, Dof<G>> generators_sparse =
  []() -> std::array<Eigen::SparseMatrix<Scalar<G>>, Dof<G>> {
  std::array<Eigen::SparseMatrix<Scalar<G>>, Dof<G>> ret;
  for (auto i = 0u; i < Dof<G>; ++i) {
    ret[i] = ad<G>(Tangent<G>::Unit(i)).sparseView();
    ret[i].makeCompressed();
  }
  return ret;
}();
 
template<LieGroup G>
inline Eigen::SparseMatrix<Scalar<G>> ad_sparse_pattern = [] {
  Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G>);
  if constexpr (!IsCommutative<G>) {
    for (const auto & gen : generators_sparse<G>) { ret += gen; }
  }
  ret.makeCompressed();
  ret.coeffs().setZero();
  return ret;
}();
 
template<LieGroup G>
void ad_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a);
 
template<LieGroup G>
inline Eigen::SparseMatrix<Scalar<G>> d_exp_sparse_pattern = [] {
  Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G>);
  if constexpr (IsCommutative<G>) {
    // identity matrix
    for (auto i = 0u; i < Dof<G>; ++i) { ret.insert(i, i) = Scalar<G>(1); }
  } else if constexpr (requires { traits::lie_sparse<G>::d_exp_sparse_pattern; }) {
    // use specialized method if one exists
    ret = traits::lie_sparse<G>::d_exp_sparse_pattern;
  } else {
    // fall back on dense pattern
    for (auto i = 0u; i < Dof<G>; ++i) {
      for (auto j = 0u; j < Dof<G>; ++j) { ret.insert(i, j) = (i == j) ? Scalar<G>(1) : Scalar<G>(0); }
    }
  }
  ret.makeCompressed();
  return ret;
}();
 
template<LieGroup G, bool Inv = false>
void dr_exp_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0);
 
template<LieGroup G>
void dr_expinv_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0);
 
template<LieGroup G>
inline Eigen::SparseMatrix<Scalar<G>> d2_exp_sparse_pattern = [] {
  Eigen::SparseMatrix<Scalar<G>> ret(Dof<G>, Dof<G> * Dof<G>);
  if constexpr (IsCommutative<G>) {
    // zero matrix--do nothing
  } else if constexpr (requires { traits::lie_sparse<G>::d2_exp_sparse_pattern; }) {
    // use spacialized method
    ret = traits::lie_sparse<G>::d2_exp_sparse_pattern;
  } else {
    // dense pattern
    ret = Eigen::Matrix<Scalar<G>, Dof<G>, Dof<G> * Dof<G>>::Ones().sparseView();
  }
  ret.makeCompressed();
  return ret;
}();
 
template<LieGroup G, bool Inv = false>
void d2r_exp_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0);
 
template<LieGroup G>
inline void d2r_expinv_sparse(Eigen::SparseMatrix<Scalar<G>> & sp, const Tangent<G> & a, Eigen::Index i0 = 0);
 
SMOOTH_END_NAMESPACE
 
#include "detail/lie_group_sparse_impl.hpp"