CombinedImuFactor.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/navigation/LieGroupPreintegration.h>
#include <gtsam/navigation/PreintegrationCombinedParams.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
#include <type_traits>
 
namespace gtsam {
 
#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:
 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
 * conditionally independent sets in factor graphs: a unifying perspective based
 * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
 *
 * 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 PreintegratedCombinedMeasurementsT : public PreintegrationType {
 public:
  typedef PreintegrationCombinedParams Params;
 
 protected:
  /* Covariance matrix of the preintegrated measurements
   * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc
   * BiasOmega] (first-order propagation from *measurementCovariance*).
   * PreintegratedCombinedMeasurements also include the biases and keep the
   * correlation between the preintegrated measurements and the biases
   */
  Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
  template <class PIM> friend class CombinedImuFactorT;
 
 public:

  PreintegratedCombinedMeasurementsT() { this->resetIntegration(); }

  PreintegratedCombinedMeasurementsT(
      const std::shared_ptr<Params>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(),
      const Eigen::Matrix<double, 15, 15>& preintMeasCov =
          Eigen::Matrix<double, 15, 15>::Zero())
      : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {
    this->PreintegrationType::resetIntegration();
  }

  PreintegratedCombinedMeasurementsT(
      const PreintegrationType& base,
      const Eigen::Matrix<double, 15, 15>& preintMeasCov)
      : PreintegrationType(base), preintMeasCov_(preintMeasCov) {
    this->PreintegrationType::resetIntegration();
  }

  ~PreintegratedCombinedMeasurementsT() override {}



  void resetIntegration() override;

  Params& p() const { return *std::static_pointer_cast<Params>(this->p_); }

  Matrix preintMeasCov() const { return preintMeasCov_; }

  Matrix 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());
  }

  Matrix residualCovarianceAt(const Rot3& predictedAttitude) const {
    Eigen::Matrix<double, 15, 15> physicalChart =
        Eigen::Matrix<double, 15, 15>::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;
    }
    Eigen::Matrix<double, 15, 15> chartJacobian =
        Eigen::Matrix<double, 15, 15>::Identity();
    chartJacobian.bottomRightCorner<6, 6>() = -I_6x6;
 
    if constexpr (std::is_same_v<PreintegrationType, TangentPreintegration>) {
      Matrix9 preintegrationChartJacobian;
      internal::navStateComponentWiseRetract(
          NavState(), this->preintegrated_, {},
          &preintegrationChartJacobian);
      chartJacobian.topLeftCorner<9, 9>() = preintegrationChartJacobian;
    }
 
    physicalChart *= chartJacobian;
    return physicalChart * preintMeasCov_ * physicalChart.transpose();
  }


  void print(
      const std::string& s = "Preintegrated Measurements:") const override;
  bool equals(const PreintegratedCombinedMeasurementsT<PreintegrationType>& expected,
              double tol = 1e-9) const;


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

 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
  void resetIntegration(const gtsam::Matrix6& Q_init) {
    std::cerr << "Warning: setBiasAccOmegaInit() is deprecated and no longer used." << std::endl;
    PreintegrationType::resetIntegration();
    preintMeasCov_.setZero();
  }
#endif
 
 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:
using PreintegratedCombinedMeasurements = PreintegratedCombinedMeasurementsT<DefaultPreintegrationType>;

template <class PIM = PreintegratedCombinedMeasurements>
class GTSAM_EXPORT CombinedImuFactorT
    : public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                               imuBias::ConstantBias, imuBias::ConstantBias> {
 public:
 private:
  typedef CombinedImuFactorT<PIM> This;
  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                            imuBias::ConstantBias, imuBias::ConstantBias>
      Base;
 
  PIM pim_;
 
 public:
  // Provide access to Matrix& version of evaluateError:
  using Base::evaluateError;

  typedef std::shared_ptr<This> shared_ptr;

  CombinedImuFactorT() {}

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

  template <class Measurement = PIM>
  CombinedImuFactorT(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
                     Key bias_j,
                     const Measurement& preintegratedMeasurements,
                     const Rot3& predictedAttitude)
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovarianceAt(
                     predictedAttitude)),
             pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
        pim_(preintegratedMeasurements) {}
 
  ~CombinedImuFactorT() 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_;
  }


  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
                       const Pose3& pose_j, const Vector3& vel_j,
                       const imuBias::ConstantBias& bias_i,
                       const imuBias::ConstantBias& bias_j,
                       OptionalMatrixType H1, OptionalMatrixType H2,
                       OptionalMatrixType H3, OptionalMatrixType H4,
                       OptionalMatrixType H5,
                       OptionalMatrixType H6) const override;
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
    ar& BOOST_SERIALIZATION_NVP(pim_);
  }
#endif
};
// class CombinedImuFactorT
 
// For backward compatibility:
using CombinedImuFactor = CombinedImuFactorT<>;
 
// operator<< for CombinedImuFactorT
template <class PIM>
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const CombinedImuFactorT<PIM>& f);
 
namespace internal {
template <class PIM>
Vector combinedImuError(const PIM& pim, const Pose3& pose_i,
    const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
    const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
    const Vector3& n_gravity, OptionalMatrixType H1, OptionalMatrixType H2,
    OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5,
    OptionalMatrixType H6, Matrix93* D_r_gvec) {
  // error wrt bias evolution model (random walk)
  Matrix6 Hbias_i, Hbias_j;
  Vector6 fbias = traits<imuBias::ConstantBias>::Between(bias_j, bias_i,
      H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector();
 
  Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i;
  Matrix93 D_r_vel_i, D_r_vel_j;
 
  // error wrt preintegrated measurements
  Vector9 r_Rpv = internal::preintegrationErrorAndJacobians(
      pim, pose_i, vel_i, pose_j, vel_j, bias_i, n_gravity,
      H1 ? &D_r_pose_i : nullptr, H2 ? &D_r_vel_i : nullptr,
      H3 ? &D_r_pose_j : nullptr, H4 ? &D_r_vel_j : nullptr,
      H5 ? &D_r_bias_i : nullptr, D_r_gvec);
 
  // if we need the jacobians
  if (H1) {
    H1->resize(15, 6);
    H1->block<9, 6>(0, 0) = D_r_pose_i;
    // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
    H1->block<6, 6>(9, 0).setZero();
  }
  if (H2) {
    H2->resize(15, 3);
    H2->block<9, 3>(0, 0) = D_r_vel_i;
    // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
    H2->block<6, 3>(9, 0).setZero();
  }
  if (H3) {
    H3->resize(15, 6);
    H3->block<9, 6>(0, 0) = D_r_pose_j;
    // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
    H3->block<6, 6>(9, 0).setZero();
  }
  if (H4) {
    H4->resize(15, 3);
    H4->block<9, 3>(0, 0) = D_r_vel_j;
    // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
    H4->block<6, 3>(9, 0).setZero();
  }
  if (H5) {
    H5->resize(15, 6);
    H5->block<9, 6>(0, 0) = D_r_bias_i;
    // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
    H5->block<6, 6>(9, 0) = Hbias_i;
  }
  if (H6) {
    H6->resize(15, 6);
    H6->block<9, 6>(0, 0).setZero();
    // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
    H6->block<6, 6>(9, 0) = Hbias_j;
  }
 
  // overall error
  Vector r(15);
  r << r_Rpv, fbias;  // vector of size 15
  return r;
}
}  // namespace internal
 
template <>
struct traits<PreintegrationCombinedParams>
    : public Testable<PreintegrationCombinedParams> {};
 
template <class PreintegrationType>
struct traits<PreintegratedCombinedMeasurementsT<PreintegrationType>>
    : public Testable<PreintegratedCombinedMeasurementsT<PreintegrationType>> {};
 
template <class PIM>
struct traits<CombinedImuFactorT<PIM>> : public Testable<CombinedImuFactorT<PIM>> {};
 
}  // namespace gtsam