ImuFactor.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
 
/* GTSAM includes */
#include <gtsam/base/debug.h>
#include <gtsam/linear/FixedJacobianFactor.h>
#include <gtsam/navigation/LieGroupPreintegration.h>
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <type_traits>  // For std::is_same, std::enable_if
 
namespace gtsam {
 
// Determine default preintegration backend
#ifdef GTSAM_LIEGROUP_PREINTEGRATION
typedef LieGroupPreintegration DefaultPreintegrationType;
#elif defined(GTSAM_TANGENT_PREINTEGRATION)
typedef TangentPreintegration DefaultPreintegrationType;
#else
typedef ManifoldPreintegration DefaultPreintegrationType;
#endif
 
/*
 * If you are using the factor, please cite:
 * Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza,
 * "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE
 * Transactions on Robotics, 2017.
 *
 * REFERENCES:
 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
 *     Volume 2, 2008.
 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
 *     High-Dynamic Motion in Built Environments Without Initial Conditions",
 *     TRO, 28(1):61-76, 2012.
 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
 *     Computation of the Jacobian Matrices", Tech. Report, 2013.
 *     Available in this repo as "PreintegratedIMUJacobians.pdf".
 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration
 * on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
 *     Robotics: Science and Systems (RSS), 2015.
 */

template <class PreintegrationType>
class GTSAM_EXPORT PreintegratedImuMeasurementsT: public PreintegrationType {
 
  template <class PIM> friend class ImuFactorT;
  template <class PIM> friend class ImuFactor2T;
 
protected:
 
  Matrix9 preintMeasCov_; 
 
public:

  PreintegratedImuMeasurementsT()
      : PreintegratedImuMeasurementsT(
            std::make_shared<PreintegrationParams>()) {}

  PreintegratedImuMeasurementsT(const std::shared_ptr<PreintegrationParams>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
      PreintegrationType(p, biasHat) {
    this->resetIntegration();
  }

  PreintegratedImuMeasurementsT(const PreintegrationType& base, const Matrix9& preintMeasCov)
     : PreintegrationType(base),
       preintMeasCov_(preintMeasCov) {
    this->PreintegrationType::resetIntegration();
  }
 
  ~PreintegratedImuMeasurementsT() override {
  }

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

  bool equals(const PreintegratedImuMeasurementsT<PreintegrationType>& expected, double tol = 1e-9) const;

  void resetIntegration() override;

  void integrateMeasurement(const Vector3& measuredAcc,
      const Vector3& measuredOmega, const double dt) override;

  Matrix preintMeasCov() const { return preintMeasCov_; }

  Matrix9 residualCovariance() const {
    // An endpoint attitude is needed only for the rotating-frame lift.
    if (!this->params() || !this->p().omegaCoriolis ||
        this->p().omegaCoriolis->isZero(0.0)) {
      return residualCovarianceAt(Rot3());
    }
    return residualCovarianceAt(
        this->predict(NavState(), this->biasHat()).attitude());
  }

  Matrix9 residualCovarianceAt(const Rot3& predictedAttitude) const {
    Eigen::Matrix<double, 9, 9> physicalChart =
        Eigen::Matrix<double, 9, 9>::Identity();
    if (this->params() && this->p().omegaCoriolis) {
      const Matrix3 rotation = predictedAttitude.matrix();
      physicalChart.template block<3, 3>(6, 3) =
          -rotation.transpose() *
          skewSymmetric(*this->p().omegaCoriolis) * rotation;
    }
    if constexpr (std::is_same_v<PreintegrationType,
                                 TangentPreintegration>) {
      Matrix9 chartJacobian;
      internal::navStateComponentWiseRetract(
          NavState(), this->preintegrated_, {}, &chartJacobian);
      physicalChart *= chartJacobian;
    }
    return physicalChart * preintMeasCov_ * physicalChart.transpose();
  }

  template <typename PB = PreintegrationType,
             // This method is only callable when PreintegrationType is TangentPreintegration.
             typename = typename std::enable_if<std::is_same<PB, TangentPreintegration>::value>::type>
  void mergeWith(const PreintegratedImuMeasurementsT<TangentPreintegration>& pim12, Matrix9* H1, Matrix9* H2) {
    // The `this->PreintegrationType::mergeWith` implies calling TangentPreintegration's mergeWith.
    // Since pim12 is PreintegratedImuMeasurementsT<TangentPreintegration>, it is a TangentPreintegration.
    this->PreintegrationType::mergeWith(pim12, H1, H2);
    // NOTE(gareth): Temporary P is needed as of Eigen 3.3
    const Matrix9 P = *H1 * preintMeasCov_ * H1->transpose();
    preintMeasCov_ = P + *H2 * pim12.preintMeasCov_ * H2->transpose();
  }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION  
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    namespace bs = ::boost::serialization;
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
    ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
  }
#endif
};
 
// For backward compatibility (so that the compiler flag GTSAM_TANGENT_PREINTEGRATION still
// controls which class PreintegratedImuMeasurements uses):
using PreintegratedImuMeasurements = PreintegratedImuMeasurementsT<DefaultPreintegrationType>;

template <class PIM = PreintegratedImuMeasurements>
class GTSAM_EXPORT ImuFactorT
    : public NoiseModelFactorT<Vector9, Pose3, Vector3, Pose3, Vector3,
                               imuBias::ConstantBias> {
private:
 
  typedef ImuFactorT<PIM> This;
  typedef NoiseModelFactorT<Vector9, Pose3, Vector3, Pose3, Vector3,
                            imuBias::ConstantBias>
      Base;
 
  PIM pim_;
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  typedef std::shared_ptr<This> shared_ptr;
 

  ImuFactorT() {}

  ImuFactorT(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      const PIM& preintegratedMeasurements)
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovariance()),
             pose_i, vel_i, pose_j, vel_j, bias),
        pim_(preintegratedMeasurements) {}

  template <class Measurement = PIM>
  ImuFactorT(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
             const Measurement& preintegratedMeasurements,
             const Rot3& predictedAttitude)
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovarianceAt(
                     predictedAttitude)),
             pose_i, vel_i, pose_j, vel_j, bias),
        pim_(preintegratedMeasurements) {}
 
  ~ImuFactorT() override {
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::make_shared<This>(*this);
  }

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

 
  const PIM& preintegratedMeasurements() const {
    return pim_;
  }


  Vector9 evaluateError(const Pose3& pose_i, const Vector3& vel_i,
                        const Pose3& pose_j, const Vector3& vel_j,
                        const imuBias::ConstantBias& bias_i,
                        OptionalMatrixType H1, OptionalMatrixType H2,
                        OptionalMatrixType H3, OptionalMatrixType H4,
                        OptionalMatrixType H5) const override;

  template <typename MethodPIMArg = PIM,
    // This method is only callable when PIM is PreintegratedImuMeasurementsT<TangentPreintegration>.
    typename = typename std::enable_if<
        std::is_same<MethodPIMArg, PreintegratedImuMeasurementsT<TangentPreintegration>>::value
    >::type
  >
  static MethodPIMArg Merge(
    const MethodPIMArg& pim01,
    const MethodPIMArg& pim12
  ) {
    // When this template is instantiated:
    // 1. MethodPIMArg = PIM. It's mirrored to avoid error C7637 from strict compilers.
    // 2. The SFINAE condition ensures MethodPIMArg IS PreintegratedImuMeasurementsT<TangentPreintegration>.
    // So, arguments are const PreintegratedImuMeasurementsT<TangentPreintegration>&
    // and return is PreintegratedImuMeasurementsT<TangentPreintegration>.
 
    if (!pim01.matchesParamsWith(pim12))
      throw std::domain_error(
          "Cannot merge PreintegratedImuMeasurements with different params");
 
    if (pim01.p_->body_P_sensor)
      throw std::domain_error(
          "Cannot merge PreintegratedImuMeasurements with sensor pose yet");
 
    // the bias for the merged factor will be the bias from 01
    MethodPIMArg pim02 = pim01;
 
    Matrix9 H1, H2;
    pim02.mergeWith(pim12, &H1, &H2);
 
    return pim02;
  }

  template <
    typename MethodPIMArg = PIM,
    // This method is only callable when PIM is PreintegratedImuMeasurementsT<TangentPreintegration>.
    typename = typename std::enable_if<
        std::is_same<MethodPIMArg, PreintegratedImuMeasurementsT<TangentPreintegration>>::value
    >::type
  >
  static typename ImuFactorT<MethodPIMArg>::shared_ptr Merge(
    const typename ImuFactorT<MethodPIMArg>::shared_ptr& f01,
    const typename ImuFactorT<MethodPIMArg>::shared_ptr& f12
  ) {
    // When this template is instantiated:
    // 1. MethodPIMArg = PIM. It's mirrored to avoid error C7637 from strict compilers.
    // 2. The SFINAE condition ensures MethodPIMArg IS PreintegratedImuMeasurementsT<TangentPreintegration>.
    // So, ImuFactorT<MethodPIMArg> is effectively ImuFactorT<PIM>, which is `This`.
    // The signature effectively becomes:
    // static typename This::shared_ptr Merge(const typename This::shared_ptr&, const typename This::shared_ptr&)
 
  // IMU bias keys must be the same.
  if (f01->template key<5>() != f12->template key<5>())
    throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
 
  // expect intermediate pose, velocity keys to matchup.
  if (f01->template key<3>() != f12->template key<1>() || f01->template key<4>() != f12->template key<2>())
    throw std::domain_error(
        "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
 
  // return new factor
  auto pim02 = This::Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
 
  return std::make_shared<This>( // `This` is ImuFactorT<MethodPIMArg> (i.e. ImuFactorT<PIM>)
      f01->template key<1>(),  // P0
      f01->template key<2>(),  // V0
      f12->template key<3>(),  // P2
      f12->template key<4>(),  // V2
      f01->template key<5>(),  // B
      pim02);
  }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
    ar & boost::serialization::make_nvp("NoiseModelFactor5",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(pim_);
  }
#endif
};
// class ImuFactorT
 
// For backward compatibility:
using ImuFactor = ImuFactorT<>;
 
// operator<< for ImuFactorT
template <class PIM>
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const ImuFactorT<PIM>& f);

template <class PIM = PreintegratedImuMeasurements>
class GTSAM_EXPORT ImuFactor2T
    : public NoiseModelFactorT<Vector9, NavState, NavState,
                               imuBias::ConstantBias> {
private:
 
  typedef ImuFactor2T<PIM> This;
  typedef NoiseModelFactorT<Vector9, NavState, NavState,
                            imuBias::ConstantBias>
      Base;
 
  PIM pim_;
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  ImuFactor2T() {}

  ImuFactor2T(Key state_i, Key state_j, Key bias,
             const PIM& preintegratedMeasurements)
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovariance()),
             state_i, state_j, bias),
        pim_(preintegratedMeasurements) {}

  template <class Measurement = PIM>
  ImuFactor2T(Key state_i, Key state_j, Key bias,
              const Measurement& preintegratedMeasurements,
              const Rot3& predictedAttitude)
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovarianceAt(
                     predictedAttitude)),
             state_i, state_j, bias),
        pim_(preintegratedMeasurements) {}
 
  ~ImuFactor2T() override {
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::make_shared<This>(*this);
  }
 

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

 
  const PIM& preintegratedMeasurements() const {
    return pim_;
  }


  Vector9 evaluateError(const NavState& state_i, const NavState& state_j,
                        const imuBias::ConstantBias& bias_i,  //
                        OptionalMatrixType H1, OptionalMatrixType H2,
                        OptionalMatrixType H3) const override;
 
private:
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
    ar & boost::serialization::make_nvp("NoiseModelFactor3",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(pim_);
  }
#endif
};
// class ImuFactor2T
 
// For backward compatibility:
using ImuFactor2 = ImuFactor2T<>;
 
// operator<< for ImuFactor2T
template <class PIM>
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const ImuFactor2T<PIM>& f);
 
template <class PreintegrationType>
struct traits<PreintegratedImuMeasurementsT<PreintegrationType>> : public Testable<PreintegratedImuMeasurementsT<PreintegrationType>> {};
 
template <class PIM>
struct traits<ImuFactorT<PIM>> : public Testable<ImuFactorT<PIM>> {};
 
template <class PIM>
struct traits<ImuFactor2T<PIM>> : public Testable<ImuFactor2T<PIM>> {};
 
}