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 
30 namespace gtsam {
31 
32 #ifdef GTSAM_TANGENT_PREINTEGRATION
33 typedef TangentPreintegration PreintegrationType;
34 #else
35 typedef 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 
72 class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
73 
74  friend class ImuFactor;
75  friend class ImuFactor2;
76 
77 protected:
78 
80 
82 public:
83 
86  preintMeasCov_.setZero();
87  }
88 
94  PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p,
95  const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
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 
112  }
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
144  void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
146 #endif
147 
148  private:
149 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
150  friend class boost::serialization::access;
152  template<class ARCHIVE>
153  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
154  namespace bs = ::boost::serialization;
155  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
156  ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
157  }
158 #endif
159 };
160 
173 class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
174  imuBias::ConstantBias> {
175 private:
176 
177  typedef ImuFactor This;
178  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
180 
182 
183 public:
184 
185  // Provide access to the Matrix& version of evaluateError:
186  using Base::evaluateError;
187 
189 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
190  typedef typename std::shared_ptr<ImuFactor> shared_ptr;
191 #else
192  typedef std::shared_ptr<ImuFactor> shared_ptr;
193 #endif
194 
197 
208  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
209  const PreintegratedImuMeasurements& preintegratedMeasurements);
210 
211  ~ImuFactor() override {
212  }
213 
215  gtsam::NonlinearFactor::shared_ptr clone() const override;
216 
219  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
220  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
221  DefaultKeyFormatter) const override;
222  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
224 
228  return _PIM_;
229  }
230 
233  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
235  const Pose3& pose_j, const Vector3& vel_j,
237  OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override;
238 
239 #ifdef GTSAM_TANGENT_PREINTEGRATION
240  static PreintegratedImuMeasurements Merge(
242  const PreintegratedImuMeasurements& pim01,
243  const PreintegratedImuMeasurements& pim12);
244 
246  static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
247 #endif
248 
249  private:
251 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
252  friend class boost::serialization::access;
253  template<class ARCHIVE>
254  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
255  // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
256  ar & boost::serialization::make_nvp("NoiseModelFactor5",
257  boost::serialization::base_object<Base>(*this));
258  ar & BOOST_SERIALIZATION_NVP(_PIM_);
259  }
260 #endif
261 };
262 // class ImuFactor
263 
268 class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
269 private:
270 
271  typedef ImuFactor2 This;
273 
275 
276 public:
277 
278  // Provide access to the Matrix& version of evaluateError:
279  using Base::evaluateError;
280 
283 
290  ImuFactor2(Key state_i, Key state_j, Key bias,
291  const PreintegratedImuMeasurements& preintegratedMeasurements);
292 
293  ~ImuFactor2() override {
294  }
295 
297  gtsam::NonlinearFactor::shared_ptr clone() const override;
298 
301  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
302  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
303  DefaultKeyFormatter) const override;
304  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
306 
310  return _PIM_;
311  }
312 
315  Vector evaluateError(const NavState& state_i, const NavState& state_j,
317  const imuBias::ConstantBias& bias_i, //
319  OptionalMatrixType H3) const override;
320 
321 private:
322 
323 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
324 
325  friend class boost::serialization::access;
326  template<class ARCHIVE>
327  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
328  // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
329  ar & boost::serialization::make_nvp("NoiseModelFactor3",
330  boost::serialization::base_object<Base>(*this));
331  ar & BOOST_SERIALIZATION_NVP(_PIM_);
332  }
333 #endif
334 };
335 // class ImuFactor2
336 
337 template <>
338 struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
339 
340 template <>
341 struct traits<ImuFactor> : public Testable<ImuFactor> {};
342 
343 template <>
344 struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
345 
346 }
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Definition: ImuFactor.h:227
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
Eigen::Vector3d Vector3
Definition: Vector.h:43
Global debugging flags.
std::string serialize(const T &input)
serializes to a string
Matrix expected
Definition: testMatrix.cpp:971
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition: ImuFactor.h:111
NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias > Base
Definition: ImuFactor.h:179
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
void integrateMeasurements(const vector< ImuMeasurement > &measurements, PIM *pim)
~ImuFactor() override
Definition: ImuFactor.h:211
PreintegratedImuMeasurements(const std::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Definition: ImuFactor.h:94
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
ImuFactor This
Definition: ImuFactor.h:177
const double dt
PreintegratedImuMeasurements _PIM_
Definition: ImuFactor.h:181
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
NoiseModelFactorN< NavState, NavState, imuBias::ConstantBias > Base
Definition: ImuFactor.h:272
PreintegratedImuMeasurements _PIM_
Definition: ImuFactor.h:274
ManifoldPreintegration PreintegrationType
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition: ImuFactor.h:85
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Matrix9 preintMeasCov_
(first-order propagation from measurementCovariance).
Definition: ImuFactor.h:79
static const Vector3 measuredAcc
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:75
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Definition: ImuFactor.h:309
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
ImuFactor2 This
Definition: ImuFactor.h:271
traits
Definition: chartTesting.h:28
static const Vector3 measuredOmega(w, 0, 0)
Non-linear factor base classes.
ofstream os("timeSchurFactors.csv")
std::shared_ptr< This > shared_ptr
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition: ImuFactor.h:141
float * p
~ImuFactor2() override
Definition: ImuFactor.h:293
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Definition: ImuFactor.h:105
const G double tol
Definition: Group.h:86
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
std::shared_ptr< ImuFactor > shared_ptr
Definition: ImuFactor.h:192
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:21