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 ManifoldPreintegration : public PreintegrationBase {
 protected:

  NavState deltaXij_;
  Matrix3 delRdelBiasOmega_; 
  Matrix3 delPdelBiasAcc_;   
  Matrix3 delPdelBiasOmega_; 
  Matrix3 delVdelBiasAcc_;   
  Matrix3 delVdelBiasOmega_; 

  ManifoldPreintegration() {
    resetIntegration();
  }

public:

  ManifoldPreintegration(const boost::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 = boost::none) const override;

  virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
    return boost::shared_ptr<ManifoldPreintegration>();
  }


private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    namespace bs = ::boost::serialization;
    ar & BOOST_SERIALIZATION_NVP(p_);
    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
    ar & BOOST_SERIALIZATION_NVP(deltaXij_);
    ar & BOOST_SERIALIZATION_NVP(biasHat_);
    ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
    ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
    ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
    ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
    ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
  }
};

}