gtsam  4.1.0
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 
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  * [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 
76 protected:
77 
78  Matrix9 preintMeasCov_;
79 
81 public:
82 
85  preintMeasCov_.setZero();
86  }
87 
93  PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
94  const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
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
139  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 
166 class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
167  imuBias::ConstantBias> {
168 private:
169 
170  typedef ImuFactor This;
171  typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
173 
175 
176 public:
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 
196  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
197  const PreintegratedImuMeasurements& preintegratedMeasurements);
198 
199  virtual ~ImuFactor() {
200  }
201 
203  gtsam::NonlinearFactor::shared_ptr clone() const override;
204 
207  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
208  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
209  DefaultKeyFormatter) const override;
210  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
212 
216  return _PIM_;
217  }
218 
221  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
223  const Pose3& pose_j, const Vector3& vel_j,
224  const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
225  boost::none, boost::optional<Matrix&> H2 = boost::none,
226  boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
227  boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
228 
229 #ifdef GTSAM_TANGENT_PREINTEGRATION
230  static PreintegratedImuMeasurements Merge(
232  const PreintegratedImuMeasurements& pim01,
233  const PreintegratedImuMeasurements& pim12);
234 
236  static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
237 #endif
238 
239  private:
241  friend class boost::serialization::access;
242  template<class ARCHIVE>
243  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
244  ar & boost::serialization::make_nvp("NoiseModelFactor5",
245  boost::serialization::base_object<Base>(*this));
246  ar & BOOST_SERIALIZATION_NVP(_PIM_);
247  }
248 };
249 // class ImuFactor
250 
255 class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
256 private:
257 
258  typedef ImuFactor2 This;
260 
262 
263 public:
264 
267 
274  ImuFactor2(Key state_i, Key state_j, Key bias,
275  const PreintegratedImuMeasurements& preintegratedMeasurements);
276 
277  virtual ~ImuFactor2() {
278  }
279 
281  gtsam::NonlinearFactor::shared_ptr clone() const override;
282 
285  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
286  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
287  DefaultKeyFormatter) const override;
288  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
290 
294  return _PIM_;
295  }
296 
299  Vector evaluateError(const NavState& state_i, const NavState& state_j,
301  const imuBias::ConstantBias& bias_i, //
302  boost::optional<Matrix&> H1 = boost::none,
303  boost::optional<Matrix&> H2 = boost::none,
304  boost::optional<Matrix&> H3 = boost::none) const override;
305 
306 private:
307 
309  friend class boost::serialization::access;
310  template<class ARCHIVE>
311  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
312  ar & boost::serialization::make_nvp("NoiseModelFactor3",
313  boost::serialization::base_object<Base>(*this));
314  ar & BOOST_SERIALIZATION_NVP(_PIM_);
315  }
316 };
317 // class ImuFactor2
318 
319 template <>
320 struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
321 
322 template <>
323 struct traits<ImuFactor> : public Testable<ImuFactor> {};
324 
325 template <>
326 struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
327 
328 }
gtsam::ImuFactor2::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: ImuFactor.h:293
gtsam::PreintegratedImuMeasurements::~PreintegratedImuMeasurements
virtual ~PreintegratedImuMeasurements()
Virtual destructor.
Definition: ImuFactor.h:110
gtsam::ImuFactor::shared_ptr
boost::shared_ptr< ImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: ImuFactor.h:182
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::ImuFactor::ImuFactor
ImuFactor()
Default constructor - only use for serialization.
Definition: ImuFactor.h:186
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:30
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
gtsam::NoiseModelFactor5
A convenient base class for creating your own NoiseModelFactor with 5 variables.
Definition: NonlinearFactor.h:588
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:71
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Constructor, initializes the class with no measurements.
Definition: ImuFactor.h:93
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::PreintegratedImuMeasurements::preintMeasCov_
Matrix9 preintMeasCov_
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
Definition: ImuFactor.h:78
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
NonlinearFactor.h
Non-linear factor base classes.
gtsam::ImuFactor2
Definition: ImuFactor.h:255
TangentPreintegration.h
gtsam::PreintegratedImuMeasurements::preintMeasCov
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition: ImuFactor.h:136
gtsam::Pose3
Definition: Pose3.h:37
gtsam::ImuFactor2::ImuFactor2
ImuFactor2()
Default constructor - only use for serialization.
Definition: ImuFactor.h:266
gtsam::KeyFormatter
boost::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::ImuFactor
Definition: ImuFactor.h:167
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition: ImuFactor.h:104
debug.h
Global debugging flags.
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition: ManifoldPreintegration.h:33
ManifoldPreintegration.h
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition: ImuFactor.h:84
gtsam::ImuFactor::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: ImuFactor.h:215