gtsam  4.1.0
gtsam
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 
32 namespace gtsam {
33 
34 #ifdef GTSAM_TANGENT_PREINTEGRATION
35 typedef TangentPreintegration PreintegrationType;
36 #else
37 typedef 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  * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
55  * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
56  * Robotics: Science and Systems (RSS), 2015.
57  */
58 
64  Matrix6 biasAccOmegaInt;
65 
69  : biasAccCovariance(I_3x3),
70  biasOmegaCovariance(I_3x3),
71  biasAccOmegaInt(I_6x6) {}
72 
74  PreintegrationCombinedParams(const Vector3& n_gravity) :
75  PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
76  biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
77 
78  }
79 
80  // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
81  static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
82  return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
83  }
84 
85  // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
86  static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
87  return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
88  }
89 
90  void print(const std::string& s="") const override;
91  bool equals(const PreintegratedRotationParams& other, double tol) const override;
92 
93  void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
94  void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
95  void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
96 
97  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
98  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
99  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
100 
101 private:
102 
104  friend class boost::serialization::access;
105  template <class ARCHIVE>
106  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
107  namespace bs = ::boost::serialization;
108  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
109  ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
110  ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
111  ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
112  }
113 
114 public:
116 };
117 
129 
130 public:
132 
133  protected:
134  /* Covariance matrix of the preintegrated measurements
135  * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
136  * (first-order propagation from *measurementCovariance*).
137  * PreintegratedCombinedMeasurements also include the biases and keep the correlation
138  * between the preintegrated measurements and the biases
139  */
140  Eigen::Matrix<double, 15, 15> preintMeasCov_;
141 
142  friend class CombinedImuFactor;
143 
144  public:
147 
150  preintMeasCov_.setZero();
151  }
152 
159  const boost::shared_ptr<Params>& p,
160  const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
161  : PreintegrationType(p, biasHat) {
162  preintMeasCov_.setZero();
163  }
164 
170  PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
171  : PreintegrationType(base),
172  preintMeasCov_(preintMeasCov) {
173  }
174 
177 
179 
182 
184  void resetIntegration() override;
185 
187  Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
189 
193  Matrix preintMeasCov() const { return preintMeasCov_; }
195 
199  void print(const std::string& s = "Preintegrated Measurements:") const override;
201  bool equals(const PreintegratedCombinedMeasurements& expected,
202  double tol = 1e-9) const;
204 
205 
208 
218  void integrateMeasurement(const Vector3& measuredAcc,
219  const Vector3& measuredOmega, const double dt) override;
220 
222 
223  private:
225  friend class boost::serialization::access;
226  template <class ARCHIVE>
227  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
228  namespace bs = ::boost::serialization;
229  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
230  ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
231  }
232 
233 public:
235 };
236 
256 class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
257  Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
258 public:
259 
260 private:
261 
262  typedef CombinedImuFactor This;
263  typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
265 
267 
268 public:
269 
271 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
272  typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
273 #else
274  typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
275 #endif
276 
279 
291  Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
292  const PreintegratedCombinedMeasurements& preintegratedMeasurements);
293 
294  virtual ~CombinedImuFactor() {}
295 
297  gtsam::NonlinearFactor::shared_ptr clone() const override;
298 
301  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
304  const CombinedImuFactor&);
306  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
307  DefaultKeyFormatter) const override;
308 
310  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
312 
316  return _PIM_;
317  }
318 
321  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
323  const Pose3& pose_j, const Vector3& vel_j,
324  const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
325  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
326  boost::none, boost::optional<Matrix&> H3 = boost::none,
327  boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
328  boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
329 
330  private:
332  friend class boost::serialization::access;
333  template <class ARCHIVE>
334  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
335  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
336  ar& BOOST_SERIALIZATION_NVP(_PIM_);
337  }
338 
339 public:
341 };
342 // class CombinedImuFactor
343 
344 template <>
346  : public Testable<PreintegrationCombinedParams> {};
347 
348 template <>
350  : public Testable<PreintegratedCombinedMeasurements> {};
351 
352 template <>
353 struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
354 
355 } // namespace gtsam
356 
gtsam::CombinedImuFactor::CombinedImuFactor
CombinedImuFactor()
Default constructor - only use for serialization.
Definition: CombinedImuFactor.h:278
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams()
Default constructor makes uninitialized params struct.
Definition: CombinedImuFactor.h:68
gtsam::NoiseModelFactor6
A convenient base class for creating your own NoiseModelFactor with 6 variables.
Definition: NonlinearFactor.h:673
gtsam::PreintegratedCombinedMeasurements
Definition: CombinedImuFactor.h:128
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:30
gtsam::CombinedImuFactor::shared_ptr
boost::shared_ptr< CombinedImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: CombinedImuFactor.h:274
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::CombinedImuFactor::preintegratedMeasurements
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: CombinedImuFactor.h:315
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
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::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Default constructor, initializes the class with no measurements.
Definition: CombinedImuFactor.h:158
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::PreintegratedCombinedMeasurements::~PreintegratedCombinedMeasurements
virtual ~PreintegratedCombinedMeasurements()
Virtual destructor.
Definition: CombinedImuFactor.h:176
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
Definition: CombinedImuFactor.h:149
gtsam::PreintegratedCombinedMeasurements::p
Params & p() const
const reference to params, shadows definition in base class
Definition: CombinedImuFactor.h:187
NonlinearFactor.h
Non-linear factor base classes.
TangentPreintegration.h
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::Pose3
Definition: Pose3.h:37
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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::PreintegrationCombinedParams
Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
Definition: CombinedImuFactor.h:61
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition: CombinedImuFactor.h:170
gtsam::PreintegrationCombinedParams::biasOmegaCovariance
Matrix3 biasOmegaCovariance
continuous-time "Covariance" describing gyroscope bias random walk
Definition: CombinedImuFactor.h:63
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition: ManifoldPreintegration.h:33
gtsam::PreintegrationCombinedParams::biasAccOmegaInt
Matrix6 biasAccOmegaInt
covariance of bias used for pre-integration
Definition: CombinedImuFactor.h:64
ManifoldPreintegration.h
BOOST_CLASS_EXPORT_KEY
BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams)
Add Boost serialization export key (declaration) for derived class.
gtsam::PreintegrationCombinedParams::biasAccCovariance
Matrix3 biasAccCovariance
continuous-time "Covariance" describing accelerometer bias random walk
Definition: CombinedImuFactor.h:62
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams(const Vector3 &n_gravity)
See two named constructors below for good values of n_gravity in body frame.
Definition: CombinedImuFactor.h:74
serialization.h
Convenience functions for serializing data structures via boost.serialization.
gtsam::CombinedImuFactor
Definition: CombinedImuFactor.h:257
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegrationParams.h:26