gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
CombinedImuFactor.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
23#pragma once
24
25/* GTSAM includes */
29#include <gtsam/base/Matrix.h>
31
32namespace gtsam {
33
34#ifdef GTSAM_TANGENT_PREINTEGRATION
35typedef TangentPreintegration PreintegrationType;
36#else
37typedef ManifoldPreintegration PreintegrationType;
38#endif
39
40/*
41 * If you are using the factor, please cite:
42 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
43 * conditionally independent sets in factor graphs: a unifying perspective based
44 * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
45 *
46 * REFERENCES:
47 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
48 * Volume 2, 2008.
49 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
50 * High-Dynamic Motion in Built Environments Without Initial Conditions",
51 * TRO, 28(1):61-76, 2012.
52 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
53 * Computation of the Jacobian Matrices", Tech. Report, 2013.
54 * Available in this repo as "PreintegratedIMUJacobians.pdf".
55 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
56 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
57 * Robotics: Science and Systems (RSS), 2015.
58 */
59
66
70 : biasAccCovariance(I_3x3),
71 biasOmegaCovariance(I_3x3),
72 biasAccOmegaInt(I_6x6) {}
73
75 PreintegrationCombinedParams(const Vector3& n_gravity) :
76 PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
77 biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
78
79 }
80
81 // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
82 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
83 return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
84 }
85
86 // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
87 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
88 return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
89 }
90
91 void print(const std::string& s="") const override;
92 bool equals(const PreintegratedRotationParams& other, double tol) const override;
93
94 void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
95 void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
96 void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
97
98 const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
99 const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
100 const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
101
102private:
103
105 friend class boost::serialization::access;
106 template <class ARCHIVE>
107 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
108 namespace bs = ::boost::serialization;
109 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
110 ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
111 ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
112 ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
113 }
114
115public:
117};
118
130
131public:
133
134 protected:
135 /* Covariance matrix of the preintegrated measurements
136 * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
137 * (first-order propagation from *measurementCovariance*).
138 * PreintegratedCombinedMeasurements also include the biases and keep the correlation
139 * between the preintegrated measurements and the biases
140 */
141 Eigen::Matrix<double, 15, 15> preintMeasCov_;
142
143 friend class CombinedImuFactor;
144
145 public:
148
151 preintMeasCov_.setZero();
152 }
153
160 const boost::shared_ptr<Params>& p,
162 : PreintegrationType(p, biasHat) {
163 preintMeasCov_.setZero();
164 }
165
171 PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
172 : PreintegrationType(base),
173 preintMeasCov_(preintMeasCov) {
174 }
175
178
180
183
185 void resetIntegration() override;
186
188 Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
190
194 Matrix preintMeasCov() const { return preintMeasCov_; }
196
200 void print(const std::string& s = "Preintegrated Measurements:") const override;
202 bool equals(const PreintegratedCombinedMeasurements& expected,
203 double tol = 1e-9) const;
205
206
209
220 void integrateMeasurement(const Vector3& measuredAcc,
221 const Vector3& measuredOmega, const double dt) override;
222
224
225 private:
227 friend class boost::serialization::access;
228 template <class ARCHIVE>
229 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
230 namespace bs = ::boost::serialization;
231 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
232 ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
233 }
234
235public:
237};
238
258class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
259 Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
260public:
261
262private:
263
264 typedef CombinedImuFactor This;
265 typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
267
269
270public:
271
273#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
274 typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
275#else
276 typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
277#endif
278
281
293 Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
294 const PreintegratedCombinedMeasurements& preintegratedMeasurements);
295
296 ~CombinedImuFactor() override {}
297
299 gtsam::NonlinearFactor::shared_ptr clone() const override;
300
305 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
306 const CombinedImuFactor&);
308 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
309 DefaultKeyFormatter) const override;
310
312 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
314
318 return _PIM_;
319 }
320
324 Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
325 const Pose3& pose_j, const Vector3& vel_j,
326 const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
327 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
328 boost::none, boost::optional<Matrix&> H3 = boost::none,
329 boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
330 boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
331
332 private:
334 friend class boost::serialization::access;
335 template <class ARCHIVE>
336 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
337 // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
338 ar& boost::serialization::make_nvp(
339 "NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
340 ar& BOOST_SERIALIZATION_NVP(_PIM_);
341 }
342
343public:
345};
346// class CombinedImuFactor
347
348template <>
350 : public Testable<PreintegrationCombinedParams> {};
351
352template <>
354 : public Testable<PreintegratedCombinedMeasurements> {};
355
356template <>
357struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
358
359} // namespace gtsam
typedef and functions to augment Eigen's MatrixXd
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Convenience functions for serializing data structures via boost.serialization.
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
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
Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
Definition CombinedImuFactor.h:62
Matrix6 biasAccOmegaInt
covariance of bias used as initial estimate.
Definition CombinedImuFactor.h:65
PreintegrationCombinedParams(const Vector3 &n_gravity)
See two named constructors below for good values of n_gravity in body frame.
Definition CombinedImuFactor.h:75
PreintegrationCombinedParams()
Default constructor makes uninitialized params struct.
Definition CombinedImuFactor.h:69
Matrix3 biasOmegaCovariance
continuous-time "Covariance" describing gyroscope bias random walk
Definition CombinedImuFactor.h:64
Matrix3 biasAccCovariance
continuous-time "Covariance" describing accelerometer bias random walk
Definition CombinedImuFactor.h:63
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition CombinedImuFactor.h:129
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition CombinedImuFactor.h:171
~PreintegratedCombinedMeasurements() override
Virtual destructor.
Definition CombinedImuFactor.h:177
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
Definition CombinedImuFactor.h:150
Params & p() const
const reference to params, shadows definition in base class
Definition CombinedImuFactor.h:188
PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Default constructor, initializes the class with no measurements.
Definition CombinedImuFactor.h:159
CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle,...
Definition CombinedImuFactor.h:259
boost::shared_ptr< CombinedImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition CombinedImuFactor.h:276
CombinedImuFactor()
Default constructor - only use for serialization.
Definition CombinedImuFactor.h:280
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition CombinedImuFactor.h:317
Definition ImuBias.h:30
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400