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/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/base/debug.h>
 
namespace gtsam {
 
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#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.
 *
 * 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.
 *
 * 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.
 */
 
class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
 
  friend class ImuFactor;
  friend class ImuFactor2;
 
protected:
 
  Matrix9 preintMeasCov_; 
 
public:
 
  PreintegratedImuMeasurements() {
    preintMeasCov_.setZero();
  }
 
  PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
      PreintegrationType(p, biasHat) {
    preintMeasCov_.setZero();
  }
 
  PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
     : PreintegrationType(base),
       preintMeasCov_(preintMeasCov) {
  }
 
  ~PreintegratedImuMeasurements() override {
  }
 
  void print(const std::string& s = "Preintegrated Measurements:") const override;
 
  bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
 
  void resetIntegration() override;
 
  void integrateMeasurement(const Vector3& measuredAcc,
      const Vector3& measuredOmega, const double dt) override;
 
  void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
                             const Matrix& dts);
 
  Matrix preintMeasCov() const { return preintMeasCov_; }
 
#ifdef GTSAM_TANGENT_PREINTEGRATION
  void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
#endif
 
 private:
#ifdef 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
};
 
class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
    imuBias::ConstantBias> {
private:
 
  typedef ImuFactor This;
  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
      imuBias::ConstantBias> Base;
 
  PreintegratedImuMeasurements _PIM_;
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;
 
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
  typedef typename std::shared_ptr<ImuFactor> shared_ptr;
#else
  typedef std::shared_ptr<ImuFactor> shared_ptr;
#endif
 
  ImuFactor() {}
 
  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      const PreintegratedImuMeasurements& preintegratedMeasurements);
 
  ~ImuFactor() override {
  }
 
  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
  const PreintegratedImuMeasurements& 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, OptionalMatrixType H1, OptionalMatrixType H2,
      OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override;
 
#ifdef GTSAM_TANGENT_PREINTEGRATION
  static PreintegratedImuMeasurements Merge(
      const PreintegratedImuMeasurements& pim01,
      const PreintegratedImuMeasurements& pim12);
 
  static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
#endif
 
 private:
#ifdef 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 ImuFactor
 
class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
private:
 
  typedef ImuFactor2 This;
  typedef NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> Base;
 
  PreintegratedImuMeasurements _PIM_;
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;
 
  ImuFactor2() {}
 
  ImuFactor2(Key state_i, Key state_j, Key bias,
             const PreintegratedImuMeasurements& preintegratedMeasurements);
 
  ~ImuFactor2() override {
  }
 
  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
  const PreintegratedImuMeasurements& preintegratedMeasurements() const {
    return _PIM_;
  }
 
  Vector evaluateError(const NavState& state_i, const NavState& state_j,
                       const imuBias::ConstantBias& bias_i,  //
                       OptionalMatrixType H1, OptionalMatrixType H2,
                       OptionalMatrixType H3) const override;
 
private:
 
#ifdef 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 ImuFactor2
 
template <>
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
 
template <>
struct traits<ImuFactor> : public Testable<ImuFactor> {};
 
template <>
struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
 
}