LieGroupEKF.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/navigation/ManifoldEKF.h> // Include the base class
#include <gtsam/base/Lie.h> // Include for Lie group traits and operations
 
#include <Eigen/Dense>
#include <type_traits>
#include <functional> // For std::function
 
namespace gtsam {
 
  template <typename G>
  class LieGroupEKF : public ManifoldEKF<G> {
  public:
    using Base = ManifoldEKF<G>; 
    static constexpr int Dim = Base::Dim; 
    using TangentVector = typename Base::TangentVector; 
    using Jacobian = typename Base::Jacobian; // Eigen::Matrix<double, Dim, Dim>
    using Covariance = typename Base::Covariance; // Eigen::Matrix<double, Dim, Dim>
 
    LieGroupEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {
      static_assert(IsLieGroup<G>::value, "Template parameter G must be a GTSAM Lie Group");
    }
 
    template <typename Dynamics>
    using enable_if_dynamics = std::enable_if_t<
      !std::is_convertible_v<Dynamics, TangentVector>&&
      std::is_invocable_r_v<TangentVector, Dynamics, const G&,
      OptionalJacobian<Dim, Dim>&>>;
 
    template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
    G predictMean(Dynamics&& f, double dt, OptionalJacobian<Dim, Dim> A = {}) const {
      Jacobian Df, Dexp; // Eigen::Matrix<double, Dim, Dim>
 
      if constexpr (std::is_same_v<G, Matrix>) {
        // Specialize to Matrix case as its Expmap is not defined.
        TangentVector xi = f(this->X_, A ? &Df : nullptr);
        const Matrix nextX = traits<Matrix>::Retract(this->X_, xi * dt, A ? &Dexp : nullptr); // just addition
        if (A) {
          const Matrix I_n = Matrix::Identity(this->n_, this->n_);
          *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix
        }
        return nextX;
      }
      else {
        TangentVector xi = f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX)
        G U = traits<G>::Expmap(xi * dt, A ? &Dexp : nullptr);
        if (A) {
          // State transition Jacobian for left-invariant EKF:
          *A = traits<G>::Inverse(U).AdjointMap() + Dexp * Df * dt;
        }
        return this->X_.compose(U);
      }
    }
 
 
    template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
    void predict(Dynamics&& f, double dt, const Covariance& Q) {
      Jacobian A;
      if constexpr (Dim == Eigen::Dynamic) {
        A.resize(this->n_, this->n_);
      }
      this->X_ = predictMean(std::forward<Dynamics>(f), dt, A);
      this->P_ = A * this->P_ * A.transpose() + Q;
    }
 
    template<typename Control, typename Dynamics>
    using enable_if_full_dynamics = std::enable_if_t<
      std::is_invocable_r_v<TangentVector, Dynamics, const G&, const Control&, OptionalJacobian<Dim, Dim>&>
    >;
 
    template <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
    G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian<Dim, Dim> A = {}) const {
      return predictMean([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, A);
    }
 
 
    template <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
    void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) {
      return predict([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, Q);
    }
 
  }; // LieGroupEKF
 
}  // namespace gtsam