65 void resetIntegration()
override;
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_); }
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_; }
83 bool equals(
const TangentPreintegration& other,
double tol)
const;
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);
102 void update(
const Vector3& measuredAcc,
const Vector3& measuredOmega,
103 const double dt, Matrix9* A, Matrix93* B, Matrix93* C)
override;
108 Vector9 biasCorrectedDelta(
const imuBias::ConstantBias& bias_i,
109 OptionalJacobian<9, 6> H = boost::none)
const override;
112 static Vector9 Compose(
const Vector9& zeta01,
const Vector9& zeta12,
114 OptionalJacobian<9, 9> H1 = boost::none,
115 OptionalJacobian<9, 9> H2 = boost::none);
119 void mergeWith(
const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
123 virtual boost::shared_ptr<TangentPreintegration>
clone()
const {
124 return boost::shared_ptr<TangentPreintegration>();
131 friend class boost::serialization::access;
132 template<
class ARCHIVE>
133 void serialize(ARCHIVE & ar,
const unsigned int ) {
134 namespace bs = ::boost::serialization;
136 ar & BOOST_SERIALIZATION_NVP(preintegrated_);
137 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
138 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);