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/PreintegrationCombinedParams.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.
 *     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 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 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) {}

  PreintegratedCombinedMeasurements(
      const PreintegrationType& base,
      const Eigen::Matrix<double, 15, 15>& preintMeasCov)
      : PreintegrationType(base), preintMeasCov_(preintMeasCov) {}

  ~PreintegratedCombinedMeasurements() override {}



  void resetIntegration() override;

  void resetIntegration(const gtsam::Matrix6& Q_init);

  Params& p() const { return *std::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:
#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

 public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};

class GTSAM_EXPORT CombinedImuFactor
    : public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                               imuBias::ConstantBias, imuBias::ConstantBias> {
 public:
 private:
  typedef CombinedImuFactor This;
  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                            imuBias::ConstantBias, imuBias::ConstantBias>
      Base;

  PreintegratedCombinedMeasurements _PIM_;

 public:
  // Provide access to Matrix& version of evaluateError:
  using Base::evaluateError;

#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
  typedef typename std::shared_ptr<CombinedImuFactor> shared_ptr;
#else
  typedef std::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,
                       OptionalMatrixType H1, OptionalMatrixType H2,
                       OptionalMatrixType H3, OptionalMatrixType H4,
                       OptionalMatrixType H5,
                       OptionalMatrixType H6) const override;

 private:
#ifdef 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

 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