gtsam 4.1.1
gtsam
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 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
57 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
58 * Robotics: Science and Systems (RSS), 2015.
59 */
60
72
73 friend class ImuFactor;
74 friend class ImuFactor2;
75
76protected:
77
80
81public:
82
85 preintMeasCov_.setZero();
86 }
87
93 PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
95 PreintegrationType(p, biasHat) {
96 preintMeasCov_.setZero();
97 }
98
104 PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
105 : PreintegrationType(base),
106 preintMeasCov_(preintMeasCov) {
107 }
108
111 }
112
114 void print(const std::string& s = "Preintegrated Measurements:") const override;
115
117 bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
118
120 void resetIntegration() override;
121
128 void integrateMeasurement(const Vector3& measuredAcc,
129 const Vector3& measuredOmega, const double dt) override;
130
132 void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
133 const Matrix& dts);
134
136 Matrix preintMeasCov() const { return preintMeasCov_; }
137
138#ifdef GTSAM_TANGENT_PREINTEGRATION
140 void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
141#endif
142
143 private:
145 friend class boost::serialization::access;
146 template<class ARCHIVE>
147 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
148 namespace bs = ::boost::serialization;
149 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
150 ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
151 }
152};
153
166class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
167 imuBias::ConstantBias> {
168private:
169
170 typedef ImuFactor This;
171 typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
173
175
176public:
177
179#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
180 typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
181#else
182 typedef boost::shared_ptr<ImuFactor> shared_ptr;
183#endif
184
187
198 ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
199 const PreintegratedImuMeasurements& preintegratedMeasurements);
200
201 ~ImuFactor() override {
202 }
203
205 gtsam::NonlinearFactor::shared_ptr clone() const override;
206
209 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
210 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
211 DefaultKeyFormatter) const override;
212 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
214
218 return _PIM_;
219 }
220
224 Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
225 const Pose3& pose_j, const Vector3& vel_j,
226 const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
227 boost::none, boost::optional<Matrix&> H2 = boost::none,
228 boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
229 boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
230
231#ifdef GTSAM_TANGENT_PREINTEGRATION
233 static PreintegratedImuMeasurements Merge(
234 const PreintegratedImuMeasurements& pim01,
235 const PreintegratedImuMeasurements& pim12);
236
238 static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
239#endif
240
241 private:
243 friend class boost::serialization::access;
244 template<class ARCHIVE>
245 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
246 ar & boost::serialization::make_nvp("NoiseModelFactor5",
247 boost::serialization::base_object<Base>(*this));
248 ar & BOOST_SERIALIZATION_NVP(_PIM_);
249 }
250};
251// class ImuFactor
252
257class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
258private:
259
260 typedef ImuFactor2 This;
262
264
265public:
266
269
276 ImuFactor2(Key state_i, Key state_j, Key bias,
277 const PreintegratedImuMeasurements& preintegratedMeasurements);
278
279 ~ImuFactor2() override {
280 }
281
283 gtsam::NonlinearFactor::shared_ptr clone() const override;
284
287 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
288 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
289 DefaultKeyFormatter) const override;
290 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
292
296 return _PIM_;
297 }
298
302 Vector evaluateError(const NavState& state_i, const NavState& state_j,
303 const imuBias::ConstantBias& bias_i, //
304 boost::optional<Matrix&> H1 = boost::none,
305 boost::optional<Matrix&> H2 = boost::none,
306 boost::optional<Matrix&> H3 = boost::none) const override;
307
308private:
309
311 friend class boost::serialization::access;
312 template<class ARCHIVE>
313 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
314 ar & boost::serialization::make_nvp("NoiseModelFactor3",
315 boost::serialization::base_object<Base>(*this));
316 ar & BOOST_SERIALIZATION_NVP(_PIM_);
317 }
318};
319// class ImuFactor2
320
321template <>
322struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
323
324template <>
325struct traits<ImuFactor> : public Testable<ImuFactor> {};
326
327template <>
328struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
329
330}
Global debugging flags.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
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
Definition: Pose3.h:37
This is the base class for all factor types.
Definition: Factor.h:56
Definition: ImuBias.h:30
Definition: ImuFactor.h:71
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition: ImuFactor.h:110
PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Constructor, initializes the class with no measurements.
Definition: ImuFactor.h:93
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition: ImuFactor.h:104
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition: ImuFactor.h:84
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition: ImuFactor.h:136
Matrix9 preintMeasCov_
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
Definition: ImuFactor.h:78
Definition: ImuFactor.h:167
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: ImuFactor.h:217
ImuFactor()
Default constructor - only use for serialization.
Definition: ImuFactor.h:186
boost::shared_ptr< ImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: ImuFactor.h:182
Definition: ImuFactor.h:257
ImuFactor2()
Default constructor - only use for serialization.
Definition: ImuFactor.h:268
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: ImuFactor.h:295
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 3 variables.
Definition: NonlinearFactor.h:444
A convenient base class for creating your own NoiseModelFactor with 5 variables.
Definition: NonlinearFactor.h:602