gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
ImuFactor.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
24/* GTSAM includes */
28#include <gtsam/base/debug.h>
29
30namespace gtsam {
31
32#ifdef GTSAM_TANGENT_PREINTEGRATION
33typedef TangentPreintegration PreintegrationType;
34#else
35typedef ManifoldPreintegration PreintegrationType;
36#endif
37
38/*
39 * If you are using the factor, please cite:
40 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
41 * conditionally independent sets in factor graphs: a unifying perspective based
42 * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
43 *
44 * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
45 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
46 * Robotics: Science and Systems (RSS), 2015.
47 *
48 * REFERENCES:
49 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
50 * Volume 2, 2008.
51 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
52 * High-Dynamic Motion in Built Environments Without Initial Conditions",
53 * TRO, 28(1):61-76, 2012.
54 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
55 * Computation of the Jacobian Matrices", Tech. Report, 2013.
56 * Available in this repo as "PreintegratedIMUJacobians.pdf".
57 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
58 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
59 * Robotics: Science and Systems (RSS), 2015.
60 */
61
73
74 friend class ImuFactor;
75 friend class ImuFactor2;
76
77protected:
78
81
82public:
83
86 preintMeasCov_.setZero();
87 }
88
94 PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
96 PreintegrationType(p, biasHat) {
97 preintMeasCov_.setZero();
98 }
99
105 PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
106 : PreintegrationType(base),
107 preintMeasCov_(preintMeasCov) {
108 }
109
113
115 void print(const std::string& s = "Preintegrated Measurements:") const override;
116
118 bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
119
121 void resetIntegration() override;
122
133 void integrateMeasurement(const Vector3& measuredAcc,
134 const Vector3& measuredOmega, const double dt) override;
135
137 void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
138 const Matrix& dts);
139
141 Matrix preintMeasCov() const { return preintMeasCov_; }
142
143#ifdef GTSAM_TANGENT_PREINTEGRATION
145 void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
146#endif
147
148 private:
150 friend class boost::serialization::access;
151 template<class ARCHIVE>
152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
153 namespace bs = ::boost::serialization;
154 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
155 ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
156 }
157};
158
171class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
172 imuBias::ConstantBias> {
173private:
174
175 typedef ImuFactor This;
176 typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
178
180
181public:
182
184#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
185 typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
186#else
187 typedef boost::shared_ptr<ImuFactor> shared_ptr;
188#endif
189
192
203 ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
204 const PreintegratedImuMeasurements& preintegratedMeasurements);
205
206 ~ImuFactor() override {
207 }
208
210 gtsam::NonlinearFactor::shared_ptr clone() const override;
211
214 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
215 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
216 DefaultKeyFormatter) const override;
217 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
219
223 return _PIM_;
224 }
225
229 Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
230 const Pose3& pose_j, const Vector3& vel_j,
231 const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
232 boost::none, boost::optional<Matrix&> H2 = boost::none,
233 boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
234 boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
235
236#ifdef GTSAM_TANGENT_PREINTEGRATION
238 static PreintegratedImuMeasurements Merge(
239 const PreintegratedImuMeasurements& pim01,
240 const PreintegratedImuMeasurements& pim12);
241
243 static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
244#endif
245
246 private:
248 friend class boost::serialization::access;
249 template<class ARCHIVE>
250 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
251 // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
252 ar & boost::serialization::make_nvp("NoiseModelFactor5",
253 boost::serialization::base_object<Base>(*this));
254 ar & BOOST_SERIALIZATION_NVP(_PIM_);
255 }
256};
257// class ImuFactor
258
263class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
264private:
265
266 typedef ImuFactor2 This;
268
270
271public:
272
275
282 ImuFactor2(Key state_i, Key state_j, Key bias,
283 const PreintegratedImuMeasurements& preintegratedMeasurements);
284
285 ~ImuFactor2() override {
286 }
287
289 gtsam::NonlinearFactor::shared_ptr clone() const override;
290
293 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
294 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
295 DefaultKeyFormatter) const override;
296 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
298
302 return _PIM_;
303 }
304
308 Vector evaluateError(const NavState& state_i, const NavState& state_j,
309 const imuBias::ConstantBias& bias_i, //
310 boost::optional<Matrix&> H1 = boost::none,
311 boost::optional<Matrix&> H2 = boost::none,
312 boost::optional<Matrix&> H3 = boost::none) const override;
313
314private:
315
317 friend class boost::serialization::access;
318 template<class ARCHIVE>
319 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
320 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
321 ar & boost::serialization::make_nvp("NoiseModelFactor3",
322 boost::serialization::base_object<Base>(*this));
323 ar & BOOST_SERIALIZATION_NVP(_PIM_);
324 }
325};
326// class ImuFactor2
327
328template <>
329struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
330
331template <>
332struct traits<ImuFactor> : public Testable<ImuFactor> {};
333
334template <>
335struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
336
337}
Global debugging flags.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
Template to create a binary predicate.
Definition Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
Definition ImuBias.h:30
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition ImuFactor.h:72
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition ImuFactor.h:111
PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Constructor, initializes the class with no measurements.
Definition ImuFactor.h:94
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition ImuFactor.h:105
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition ImuFactor.h:85
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition ImuFactor.h:141
Matrix9 preintMeasCov_
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
Definition ImuFactor.h:79
ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous t...
Definition ImuFactor.h:172
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:222
ImuFactor()
Default constructor - only use for serialization.
Definition ImuFactor.h:191
boost::shared_ptr< ImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition ImuFactor.h:187
ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
Definition ImuFactor.h:263
ImuFactor2()
Default constructor - only use for serialization.
Definition ImuFactor.h:274
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:301
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400