ManifoldPreintegration.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
 
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationBase.h>
 
namespace gtsam {
 
class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
 protected:
 
  NavState deltaXij_;
  Matrix3 delRdelBiasOmega_; 
  Matrix3 delPdelBiasAcc_;   
  Matrix3 delPdelBiasOmega_; 
  Matrix3 delVdelBiasAcc_;   
  Matrix3 delVdelBiasOmega_; 
 
  ManifoldPreintegration() {
    resetIntegration();
  }
 
public:
 
  ManifoldPreintegration(const std::shared_ptr<Params>& p,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
 
 
  void resetIntegration() override;
 
 
  NavState deltaXij() const override { return deltaXij_; }
  Rot3     deltaRij() const override { return deltaXij_.attitude(); }
  Vector3  deltaPij() const override { return deltaXij_.position(); }
  Vector3  deltaVij() const override { return deltaXij_.velocity(); }
 
  Matrix3  delRdelBiasOmega() const { return delRdelBiasOmega_; }
  Matrix3  delPdelBiasAcc() const { return delPdelBiasAcc_; }
  Matrix3  delPdelBiasOmega() const { return delPdelBiasOmega_; }
  Matrix3  delVdelBiasAcc() const { return delVdelBiasAcc_; }
  Matrix3  delVdelBiasOmega() const { return delVdelBiasOmega_; }
 
  bool equals(const ManifoldPreintegration& other, double tol) const;
 
 
  void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
              Matrix9* A, Matrix93* B, Matrix93* C) override;
 
  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
      OptionalJacobian<9, 6> H = {}) const override;
 
  virtual std::shared_ptr<ManifoldPreintegration> clone() const {
    return std::shared_ptr<ManifoldPreintegration>();
  }
 
 
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(PreintegrationBase);
    ar & BOOST_SERIALIZATION_NVP(deltaXij_);
    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
    ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
    ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
    ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
    ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
  }
#endif
};
 
}