WnoaInterpolator.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/nonlinear/Marginals.h>
#include <gtsam/nonlinear/WnoaFactor.h>
#include <gtsam/nonlinear/WnoaStateData.h>
 
#include <functional>
#include <map>
#include <optional>
#include <set>
#include <utility>
#include <vector>
 
namespace gtsam {

using StateDataInterval =
    std::pair<std::optional<StateData>, std::optional<StateData>>;

template <typename PoseType>
struct PoseVelocity {
  PoseType pose;
  typename traits<PoseType>::TangentVector vel;

  std::pair<PoseType, typename traits<PoseType>::TangentVector> asPair() const {
    return std::make_pair(pose, vel);
  }
};

template <typename PoseType>
struct TimestampedPoseVelocity {
  PoseVelocity<PoseType> poseVel;
  double timestamp;
 
  TimestampedPoseVelocity(PoseType pose,
                          typename traits<PoseType>::TangentVector vel,
                          double time)
      : poseVel{pose, vel}, timestamp(time) {}
 
  TimestampedPoseVelocity(PoseVelocity<PoseType> pv, double time)
      : poseVel(pv), timestamp(time) {}
};

typedef std::map<Key, Matrix> InterpCovarianceMap;

template <typename PoseType>
class GTSAM_EXPORT Interpolator {
 protected:
  static constexpr int dim = traits<PoseType>::dimension;
  using VelocityType = typename traits<PoseType>::TangentVector;
  using MatrixN = Eigen::Matrix<double, dim, dim>;
  using VectorN = Eigen::Matrix<double, dim, 1>;
  using Matrix2N = Eigen::Matrix<double, 2 * dim, 2 * dim>;
  using Vector2N = Eigen::Matrix<double, 2 * dim, 1>;
  using MatrixNx2N = Eigen::Matrix<double, dim, 2 * dim>;
 
  VectorN q_psd_diag_;  // Diagonal power Spectral Density for WNOA
  std::function<Matrix(double dt)> transitionFunction_;
  std::function<Matrix(double dt, const VectorN& q_psd_diag)>
      covarianceFunction_;
  std::function<Matrix(double dt, const VectorN& q_psd_diag)>
      inverseCovarianceFunction_;
  // Todo: need to make the below two functions generalize to cases with no
  // velocities, e.g. WNOV
  std::function<Matrix(const std::pair<PoseType, VelocityType>&,
                       const std::pair<PoseType, VelocityType>&, double)>
      computeJacobianPrev_;
  std::function<Matrix(const std::pair<PoseType, VelocityType>&,
                       const std::pair<PoseType, VelocityType>&, double)>
      computeJacobianNext_;
 
 public:
  // Type aliases
  using StateDataSet = std::set<StateData>;
  using PoseVel = PoseVelocity<PoseType>;
  using TimestampedPoseVel = TimestampedPoseVelocity<PoseType>;
  using LambdaPsiMats = std::pair<Matrix2N, Matrix2N>;
  using LocalStateVecs = std::pair<VectorN, VectorN>;
  struct StateJacobians {
    MatrixN dxi_dTk;
    MatrixN dxi_dTkp1;
    MatrixN dxidot_dTk;
    MatrixN dxidot_dTkp1;
    MatrixN dxidotkp1_dvarpikp1;
  };
 
 
  Interpolator() = delete;

  Interpolator(
      const VectorN& q_psd_diag,
      std::function<Matrix(double dt)> transitionFunction,
      std::function<Matrix(double dt, const VectorN& q_psd_diag)>
          covarianceFunction,
      std::function<Matrix(double dt, const VectorN& q_psd_diag)>
          inverseCovarianceFunction,
      std::function<Matrix(const std::pair<PoseType, VelocityType>&,
                           const std::pair<PoseType, VelocityType>&, double)>
          computeJacobianPrev,
      std::function<Matrix(const std::pair<PoseType, VelocityType>&,
                           const std::pair<PoseType, VelocityType>&, double)>
          computeJacobianNext);

  Interpolator(const VectorN& q_psd_diag);

  PoseVel interpolatePoseAndVelocity(
      const std::optional<TimestampedPoseVel>& Tvarpi_k,
      const std::optional<TimestampedPoseVel>& Tvarpi_kp1, double t_tau,
      OptionalMatrixVecType H = nullptr,
      const std::shared_ptr<Matrix>& mainSolveMarginalMatrix = nullptr,
      Matrix* covarianceOut = nullptr,
      const std::shared_ptr<const LambdaPsiMats>& LambdaPsiPreComp = nullptr,
      const std::shared_ptr<const LocalStateVecs>& localStateVecsPreComp =
          nullptr,
      const std::shared_ptr<const StateJacobians>& stateJacobiansPreComp =
          nullptr) const;

  Values interpolatePosesAndVelocities(
      const NonlinearFactorGraph& mainSolveGraph,
      const Values& mainSolveSolution, const StateDataSet& mainSolveStates,
      const StateDataSet& interpolatedStates,
      std::shared_ptr<InterpCovarianceMap> covarianceMapOut = nullptr) const;

  Matrix2N computeConditionalCov(const TimestampedPoseVel& pvk,
                                 const TimestampedPoseVel& pvkp1,
                                 const TimestampedPoseVel& pvtau,
                                 OptionalMatrixType Lambda = nullptr,
                                 OptionalMatrixType Psi = nullptr) const;

  std::pair<Matrix, Matrix> getLambdaPsi(double t_k, double t_kp1,
                                         double t_tau) const;

  std::pair<Matrix, Matrix> getLambdaPsiGeneral(double t_k, double t_kp1,
                                                double t_tau) const;
  LocalStateVecs computeLocalStateVecs(const TimestampedPoseVel& pvk,
                                       const TimestampedPoseVel& pvkp1,
                                       StateJacobians* jacs = nullptr) const;
 
 protected:
  PoseVel interpolateBoundaryLeft(
      const PoseVelocity<PoseType>& poseVel_k,
      OptionalMatrixVecType H = nullptr,
      const std::shared_ptr<Matrix>& mainSolveMarginalMatrix = nullptr,
      Matrix* covarianceOut = nullptr) const;

  PoseVel interpolateBoundaryRight(
      const PoseVelocity<PoseType>& poseVel_kp1,
      OptionalMatrixVecType H = nullptr,
      const std::shared_ptr<Matrix>& mainSolveMarginalMatrix = nullptr,
      Matrix* covarianceOut = nullptr) const;

  PoseVel extrapolatePoseAndVelocity(
      const PoseVelocity<PoseType>& poseVel, double t_diff,
      OptionalMatrixVecType H = nullptr,
      const std::shared_ptr<Matrix>& mainSolveMarginalMatrix = nullptr,
      Matrix* covarianceOut = nullptr) const;

  PoseVel interpolatePoseAndVelocity_(
      const TimestampedPoseVel& tPoseVel_k,
      const TimestampedPoseVel& tPoseVel_kp1, double t_tau,
      OptionalMatrixVecType H = nullptr,
      const std::shared_ptr<Matrix>& mainSolveMarginalMatrix = nullptr,
      Matrix* covarianceOut = nullptr,
      const std::shared_ptr<const LambdaPsiMats>& LambdaPsiPreComp = nullptr,
      const std::shared_ptr<const LocalStateVecs>& localStateVecsPreComp =
          nullptr,
      const std::shared_ptr<const StateJacobians>& stateJacobiansPreComp =
          nullptr) const;

  void formLocalStateAndJacobians_(
      const TimestampedPoseVel& tPoseVel_k,
      const TimestampedPoseVel& tPoseVel_kp1,
      const std::shared_ptr<const LocalStateVecs>& localStateVecsPreComp,
      const std::shared_ptr<const StateJacobians>& stateJacobiansPreComp,
      VectorN* xi_dot_k, VectorN* xi_kp1, VectorN* xi_dot_kp1,
      StateJacobians* jacs = nullptr) const;

  void interpolateLocalState_(
      double t_k, double t_kp1, double t_tau, const VectorN& xi_dot_k,
      const VectorN& xi_kp1, const VectorN& xi_dot_kp1,
      const std::shared_ptr<const LambdaPsiMats>& LambdaPsiPreComp,
      Matrix2N* Lambda, Matrix2N* Psi, VectorN* xi_tau,
      VectorN* xidot_tau) const;

  PoseVel mapLocalStateToManifold_(const PoseType& T_k, const VectorN& xi_tau,
                                   const VectorN& xidot_tau,
                                   MatrixN* right_jac_tau,
                                   MatrixN* dTtau_dTk = nullptr,
                                   MatrixN* dTtau_dxitau = nullptr) const;

  void computeCompleteJacobians_(const Matrix2N& Lambda, const Matrix2N& Psi,
                                 const VectorN& xidot_tau,
                                 const MatrixN& right_jac_tau,
                                 const MatrixN& dTtau_dTk,
                                 const MatrixN& dTtau_dxitau,
                                 const StateJacobians& jacs,
                                 OptionalMatrixVecType H) const;

  void computeInterpolationCovariance_(
      const TimestampedPoseVel& tPoseVel_k,
      const TimestampedPoseVel& tPoseVel_kp1, const PoseVel& poseVel_tau,
      double t_tau, const Matrix2N& Lambda, const Matrix2N& Psi,
      const std::shared_ptr<Matrix>& mainSolveMarginalMatrix,
      Matrix* covarianceOut) const;

  static std::map<StateDataInterval, std::shared_ptr<Matrix>>
  ComputeJointMarginals(
      const std::map<StateDataInterval, std::vector<StateData>>& queryBuckets,
      const std::unique_ptr<Marginals>& marginals);

  static Matrix ConstructMatrixFromJointMarginal(
      const JointMarginal& blockMatrix, const KeyVector& keyVector,
      size_t blockSize);
};
 
}  // namespace gtsam