gtsam  4.1.0
gtsam
TangentPreintegration.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 
18 #pragma once
19 
21 
22 namespace gtsam {
23 
28 class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase {
29  protected:
30 
35  Vector9 preintegrated_;
38 
41  resetIntegration();
42  }
43 
44 public:
47 
53  TangentPreintegration(const boost::shared_ptr<Params>& p,
54  const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
55 
58  }
59 
61 
65  void resetIntegration() override;
66 
68 
71  Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
72  Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
73  Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
74  NavState deltaXij() const override { return NavState().retract(preintegrated_); }
75 
76  const Vector9& preintegrated() const { return preintegrated_; }
77  Vector3 theta() const { return preintegrated_.head<3>(); }
78  const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
79  const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
80 
83  bool equals(const TangentPreintegration& other, double tol) const;
85 
88 
89  // Update integrated vector on tangent manifold preintegrated with acceleration
90  // Static, functional version.
91  static Vector9 UpdatePreintegrated(const Vector3& a_body,
92  const Vector3& w_body, const double dt,
93  const Vector9& preintegrated,
94  OptionalJacobian<9, 9> A = boost::none,
95  OptionalJacobian<9, 3> B = boost::none,
96  OptionalJacobian<9, 3> C = boost::none);
97 
102  void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
103  const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;
104 
108  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
109  OptionalJacobian<9, 6> H = boost::none) const override;
110 
111  // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
112  static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
113  double deltaT12,
114  OptionalJacobian<9, 9> H1 = boost::none,
115  OptionalJacobian<9, 9> H2 = boost::none);
116 
119  void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
121 
123  virtual boost::shared_ptr<TangentPreintegration> clone() const {
124  return boost::shared_ptr<TangentPreintegration>();
125  }
126 
128 
129 private:
131  friend class boost::serialization::access;
132  template<class ARCHIVE>
133  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
134  namespace bs = ::boost::serialization;
135  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
136  ar & BOOST_SERIALIZATION_NVP(preintegrated_);
137  ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
138  ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);
139  }
140 
141 public:
143 };
144 
145 }
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition: PreintegrationBase.h:41
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:30
gtsam::TangentPreintegration::~TangentPreintegration
virtual ~TangentPreintegration()
Virtual destructor.
Definition: TangentPreintegration.h:57
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::TangentPreintegration::preintegrated_
Vector9 preintegrated_
Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta,...
Definition: TangentPreintegration.h:35
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::TangentPreintegration
Integrate on the 9D tangent space of the NavState manifold.
Definition: TangentPreintegration.h:28
gtsam::TangentPreintegration::clone
virtual boost::shared_ptr< TangentPreintegration > clone() const
Dummy clone for MATLAB.
Definition: TangentPreintegration.h:123
gtsam::TangentPreintegration::preintegrated_H_biasOmega_
Matrix93 preintegrated_H_biasOmega_
Jacobian of preintegrated_ w.r.t. angular rate bias.
Definition: TangentPreintegration.h:37
PreintegrationBase.h
gtsam::TangentPreintegration::preintegrated_H_biasAcc_
Matrix93 preintegrated_H_biasAcc_
Jacobian of preintegrated_ w.r.t. acceleration bias.
Definition: TangentPreintegration.h:36
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates using Rodrigues' formula.
Definition: Rot3.h:377
gtsam::TangentPreintegration::TangentPreintegration
TangentPreintegration()
Default constructor for serialization.
Definition: TangentPreintegration.h:40