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/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/serialization.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.
 *
 * 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.
 * [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.
 */
 
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
  Matrix3 biasAccCovariance;    
  Matrix3 biasOmegaCovariance;  
  Matrix6 biasAccOmegaInt;     
 
  PreintegrationCombinedParams()
      : biasAccCovariance(I_3x3),
        biasOmegaCovariance(I_3x3),
        biasAccOmegaInt(I_6x6) {}
 
  PreintegrationCombinedParams(const Vector3& n_gravity) :
    PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
    biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
 
  }
 
  // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
  static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
    return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
  }
 
  // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
  static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
    return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
  }
 
  void print(const std::string& s="") const override;
  bool equals(const PreintegratedRotationParams& other, double tol) const override;
 
  void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
  void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
  void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
  
  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
  
private:
 
  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(PreintegrationParams);
    ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
    ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
    ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
  }
 
public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
 
class GTSAM_EXPORT PreintegratedCombinedMeasurements : 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_;
 
  friend class CombinedImuFactor;
 
 public:
 
  PreintegratedCombinedMeasurements() {
    preintMeasCov_.setZero();
  }
 
  PreintegratedCombinedMeasurements(
      const boost::shared_ptr<Params>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
      : PreintegrationType(p, biasHat) {
    preintMeasCov_.setZero();
  }
 
  PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
     : PreintegrationType(base),
       preintMeasCov_(preintMeasCov) {
  }
 
  ~PreintegratedCombinedMeasurements() override {}
 
 
 
  void resetIntegration() override;
 
  Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
 
  Matrix preintMeasCov() const { return preintMeasCov_; }
 
  void print(const std::string& s = "Preintegrated Measurements:") const override;
  bool equals(const PreintegratedCombinedMeasurements& expected,
              double tol = 1e-9) const;
 
 
 
  void integrateMeasurement(const Vector3& measuredAcc,
      const Vector3& measuredOmega, const double dt) override;
 
 
 private:
  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_);
  }
 
public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
 
class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
    Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
public:
 
private:
 
  typedef CombinedImuFactor This;
  typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
      imuBias::ConstantBias, imuBias::ConstantBias> Base;
 
  PreintegratedCombinedMeasurements _PIM_;
 
public:
 
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
  typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
#else
  typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
 
  CombinedImuFactor() {}
 
  CombinedImuFactor(
      Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
      const PreintegratedCombinedMeasurements& preintegratedMeasurements);
 
  ~CombinedImuFactor() override {}
 
  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
                                               const CombinedImuFactor&);
  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
 
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
  const PreintegratedCombinedMeasurements& 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,
      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
          boost::none, boost::optional<Matrix&> H3 = boost::none,
      boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
          boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
 
 private:
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
    ar& BOOST_SERIALIZATION_NVP(_PIM_);
  }
 
public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// class CombinedImuFactor
 
template <>
struct traits<PreintegrationCombinedParams>
    : public Testable<PreintegrationCombinedParams> {};
 
template <>
struct traits<PreintegratedCombinedMeasurements>
    : public Testable<PreintegratedCombinedMeasurements> {};
 
template <>
struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
 
}  // namespace gtsam
 
BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams)