gtsam  4.1.0
gtsam
ManifoldPreintegration.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
22 #pragma once
23 
26 
27 namespace gtsam {
28 
33 class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
34  protected:
35 
43  Matrix3 delPdelBiasAcc_;
45  Matrix3 delVdelBiasAcc_;
47 
50  resetIntegration();
51  }
52 
53 public:
56 
62  ManifoldPreintegration(const boost::shared_ptr<Params>& p,
63  const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
64 
66 
70  void resetIntegration() override;
71 
73 
76  NavState deltaXij() const override { return deltaXij_; }
77  Rot3 deltaRij() const override { return deltaXij_.attitude(); }
78  Vector3 deltaPij() const override { return deltaXij_.position(); }
79  Vector3 deltaVij() const override { return deltaXij_.velocity(); }
80 
81  Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }
82  Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; }
83  Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; }
84  Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; }
85  Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; }
86 
89  bool equals(const ManifoldPreintegration& other, double tol) const;
91 
94 
99  void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
100  Matrix9* A, Matrix93* B, Matrix93* C) override;
101 
105  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
106  OptionalJacobian<9, 6> H = boost::none) const override;
107 
109  virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
110  return boost::shared_ptr<ManifoldPreintegration>();
111  }
112 
114 
115 private:
117  friend class boost::serialization::access;
118  template<class ARCHIVE>
119  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
120  namespace bs = ::boost::serialization;
121  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
122  ar & BOOST_SERIALIZATION_NVP(deltaXij_);
123  ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
124  ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
125  ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
126  ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
127  ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
128  }
129 };
130 
131 }
gtsam::ManifoldPreintegration::delPdelBiasAcc_
Matrix3 delPdelBiasAcc_
Jacobian of preintegrated position w.r.t. acceleration bias.
Definition: ManifoldPreintegration.h:43
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition: PreintegrationBase.h:41
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:30
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::ManifoldPreintegration::ManifoldPreintegration
ManifoldPreintegration()
Default constructor for serialization.
Definition: ManifoldPreintegration.h:49
gtsam::ManifoldPreintegration::clone
virtual boost::shared_ptr< ManifoldPreintegration > clone() const
Dummy clone for MATLAB.
Definition: ManifoldPreintegration.h:109
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ManifoldPreintegration::deltaXij_
NavState deltaXij_
Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into a...
Definition: ManifoldPreintegration.h:41
gtsam::ManifoldPreintegration::delVdelBiasOmega_
Matrix3 delVdelBiasOmega_
Jacobian of preintegrated velocity w.r.t. angular rate bias.
Definition: ManifoldPreintegration.h:46
gtsam::ManifoldPreintegration::delVdelBiasAcc_
Matrix3 delVdelBiasAcc_
Jacobian of preintegrated velocity w.r.t. acceleration bias.
Definition: ManifoldPreintegration.h:45
gtsam::ManifoldPreintegration::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition: ManifoldPreintegration.h:42
PreintegrationBase.h
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition: ManifoldPreintegration.h:33
gtsam::ManifoldPreintegration::delPdelBiasOmega_
Matrix3 delPdelBiasOmega_
Jacobian of preintegrated position w.r.t. angular rate bias.
Definition: ManifoldPreintegration.h:44