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  * Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza,
41  * "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE
42  * Transactions on Robotics, 2017.
43  *
44  * REFERENCES:
45  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
46  * Volume 2, 2008.
47  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
48  * High-Dynamic Motion in Built Environments Without Initial Conditions",
49  * TRO, 28(1):61-76, 2012.
50  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
51  * Computation of the Jacobian Matrices", Tech. Report, 2013.
52  * Available in this repo as "PreintegratedIMUJacobians.pdf".
53  * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration
54  * on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
55  * Robotics: Science and Systems (RSS), 2015.
56  */
57 
69 
70  friend class ImuFactor;
71  friend class ImuFactor2;
72 
73 protected:
74 
76 
78 public:
79 
82  preintMeasCov_.setZero();
83  }
84 
90  PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p,
91  const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
92  PreintegrationType(p, biasHat) {
93  preintMeasCov_.setZero();
94  }
95 
103  preintMeasCov_(preintMeasCov) {
104  }
105 
108  }
109 
111  void print(const std::string& s = "Preintegrated Measurements:") const override;
112 
114  bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
115 
117  void resetIntegration() override;
118 
129  void integrateMeasurement(const Vector3& measuredAcc,
130  const Vector3& measuredOmega, const double dt) override;
131 
133  void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
134  const Matrix& dts);
135 
137  Matrix preintMeasCov() const { return preintMeasCov_; }
138 
139 #ifdef GTSAM_TANGENT_PREINTEGRATION
140  void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
142 #endif
143 
144  private:
145 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
146  friend class boost::serialization::access;
148  template<class ARCHIVE>
149  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
150  namespace bs = ::boost::serialization;
151  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
152  ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
153  }
154 #endif
155 };
156 
169 class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
170  imuBias::ConstantBias> {
171 private:
172 
173  typedef ImuFactor This;
176 
178 
179 public:
180 
181  // Provide access to the Matrix& version of evaluateError:
182  using Base::evaluateError;
183 
185 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
186  typedef typename std::shared_ptr<ImuFactor> shared_ptr;
187 #else
188  typedef std::shared_ptr<ImuFactor> shared_ptr;
189 #endif
190 
193 
204  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
205  const PreintegratedImuMeasurements& preintegratedMeasurements);
206 
207  ~ImuFactor() override {
208  }
209 
211  gtsam::NonlinearFactor::shared_ptr clone() const override;
212 
215  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
216  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
217  DefaultKeyFormatter) const override;
218  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
220 
224  return _PIM_;
225  }
226 
229  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
231  const Pose3& pose_j, const Vector3& vel_j,
233  OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override;
234 
235 #ifdef GTSAM_TANGENT_PREINTEGRATION
236  static PreintegratedImuMeasurements Merge(
238  const PreintegratedImuMeasurements& pim01,
239  const PreintegratedImuMeasurements& pim12);
240 
242  static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
243 #endif
244 
245  private:
247 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
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 #endif
257 };
258 // class ImuFactor
259 
264 class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
265 private:
266 
267  typedef ImuFactor2 This;
269 
271 
272 public:
273 
274  // Provide access to the Matrix& version of evaluateError:
275  using Base::evaluateError;
276 
279 
286  ImuFactor2(Key state_i, Key state_j, Key bias,
287  const PreintegratedImuMeasurements& preintegratedMeasurements);
288 
289  ~ImuFactor2() override {
290  }
291 
293  gtsam::NonlinearFactor::shared_ptr clone() const override;
294 
297  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
298  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
299  DefaultKeyFormatter) const override;
300  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
302 
306  return _PIM_;
307  }
308 
311  Vector evaluateError(const NavState& state_i, const NavState& state_j,
313  const imuBias::ConstantBias& bias_i, //
315  OptionalMatrixType H3) const override;
316 
317 private:
318 
319 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
320 
321  friend class boost::serialization::access;
322  template<class ARCHIVE>
323  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
324  // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
325  ar & boost::serialization::make_nvp("NoiseModelFactor3",
326  boost::serialization::base_object<Base>(*this));
327  ar & BOOST_SERIALIZATION_NVP(_PIM_);
328  }
329 #endif
330 };
331 // class ImuFactor2
332 
333 template <>
334 struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
335 
336 template <>
337 struct traits<ImuFactor> : public Testable<ImuFactor> {};
338 
339 template <>
340 struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
341 
342 }
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
gtsam::ImuFactor::Base
NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias > Base
Definition: ImuFactor.h:175
ManifoldPreintegration.h
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
dt
const double dt
Definition: testVelocityConstraint.cpp:15
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::ImuFactor::shared_ptr
std::shared_ptr< ImuFactor > shared_ptr
Definition: ImuFactor.h:188
gtsam::ImuFactor::ImuFactor
ImuFactor()
Definition: ImuFactor.h:192
gtsam::ImuFactor2::_PIM_
PreintegratedImuMeasurements _PIM_
Definition: ImuFactor.h:270
gtsam::NavState
Definition: NavState.h:34
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::ImuFactor2
Definition: ImuFactor.h:264
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::ImuFactor2::ImuFactor2
ImuFactor2()
Definition: ImuFactor.h:278
gtsam::PreintegratedImuMeasurements::preintMeasCov_
Matrix9 preintMeasCov_
(first-order propagation from measurementCovariance).
Definition: ImuFactor.h:75
os
ofstream os("timeSchurFactors.csv")
testing::integrateMeasurements
void integrateMeasurements(const vector< ImuMeasurement > &measurements, PIM *pim)
Definition: imuFactorTesting.h:59
gtsam::Factor::shared_ptr
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:76
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Definition: ImuFactor.h:101
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::ImuFactor::_PIM_
PreintegratedImuMeasurements _PIM_
Definition: ImuFactor.h:177
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition: ImuFactor.h:81
gtsam::ImuFactor2::~ImuFactor2
~ImuFactor2() override
Definition: ImuFactor.h:289
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
TangentPreintegration.h
gtsam::KeyFormatter
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
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PreintegratedImuMeasurements::preintMeasCov
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition: ImuFactor.h:137
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:68
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::ImuFactor
Definition: ImuFactor.h:169
gtsam::ImuFactor::~ImuFactor
~ImuFactor() override
Definition: ImuFactor.h:207
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const std::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Definition: ImuFactor.h:90
gtsam::ImuFactor::This
ImuFactor This
Definition: ImuFactor.h:173
gtsam::PreintegrationType
ManifoldPreintegration PreintegrationType
Definition: CombinedImuFactor.h:33
gtsam::equals
Definition: Testable.h:112
gtsam::ManifoldPreintegration
Definition: ManifoldPreintegration.h:33
NonlinearFactor.h
Non-linear factor base classes.
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
Eigen::Matrix< double, 9, 9 >
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:562
gtsam::PreintegratedImuMeasurements::~PreintegratedImuMeasurements
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition: ImuFactor.h:107
gtsam::ImuFactor2::Base
NoiseModelFactorN< NavState, NavState, imuBias::ConstantBias > Base
Definition: ImuFactor.h:268
gtsam::ImuFactor2::This
ImuFactor2 This
Definition: ImuFactor.h:267
gtsam::ImuFactor::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Definition: ImuFactor.h:223
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::ImuFactor2::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Definition: ImuFactor.h:305
debug.h
Global debugging flags.


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:45