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

  Vector9 preintegrated() const {
    return NavState().localCoordinates(deltaXij_);
  }
 
  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>();
  }

 
 protected:
  virtual void updateFactor(const Vector3& bodyAcceleration,
                            const Vector3& bodyOmega, double dt,
                            OptionalJacobian<9, 9> F = {},
                            OptionalJacobian<9, 3> G1 = {},
                            OptionalJacobian<9, 3> G2 = {});

  virtual void updateBiasJacobians(const Rot3& oldRotation,
                                   const Vector3& bodyAcceleration,
                                   const Vector3& bodyOmega, double dt,
                                   const Matrix9& stateTransition,
                                   const Matrix93& accelerationJacobian,
                                   const Matrix93& omegaJacobian);
 
 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(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
};
 
}