gtsam  4.1.0
gtsam
PreintegrationParams.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 
17 #pragma once
18 
20 #include <boost/make_shared.hpp>
21 
22 namespace gtsam {
23 
30  Vector3 n_gravity;
31 
35  accelerometerCovariance(I_3x3),
36  integrationCovariance(I_3x3),
37  use2ndOrderCoriolis(false),
38  n_gravity(0, 0, -1) {}
39 
42  PreintegrationParams(const Vector3& n_gravity)
44  accelerometerCovariance(I_3x3),
45  integrationCovariance(I_3x3),
46  use2ndOrderCoriolis(false),
47  n_gravity(n_gravity) {}
48 
49  // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
50  static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
51  return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, g)));
52  }
53 
54  // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
55  static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
56  return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
57  }
58 
59  void print(const std::string& s="") const override;
60  bool equals(const PreintegratedRotationParams& other, double tol) const override;
61 
62  void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
63  void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
64  void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; }
65 
66  const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
67  const Matrix3& getIntegrationCovariance() const { return integrationCovariance; }
68  const Vector3& getGravity() const { return n_gravity; }
69  bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; }
70 
71 protected:
72 
74  friend class boost::serialization::access;
75  template<class ARCHIVE>
76  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
77  namespace bs = ::boost::serialization;
78  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
79  ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
80  ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
81  ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
82  ar & BOOST_SERIALIZATION_NVP(n_gravity);
83  }
84 
85 #ifdef GTSAM_USE_QUATERNIONS
86  // Align if we are using Quaternions
87 public:
89 #endif
90 };
91 
92 } // namespace gtsam
gtsam::PreintegrationParams::n_gravity
Vector3 n_gravity
Gravity vector in nav frame.
Definition: PreintegrationParams.h:30
gtsam::PreintegrationParams::PreintegrationParams
PreintegrationParams(const Vector3 &n_gravity)
The Params constructor insists on getting the navigation frame gravity vector For convenience,...
Definition: PreintegrationParams.h:42
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::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::PreintegrationParams::accelerometerCovariance
Matrix3 accelerometerCovariance
continuous-time "Covariance" of accelerometer
Definition: PreintegrationParams.h:27
gtsam::PreintegrationParams::use2ndOrderCoriolis
bool use2ndOrderCoriolis
Whether to use second order Coriolis integration.
Definition: PreintegrationParams.h:29
gtsam::PreintegratedRotationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegratedRotation.h:31
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::PreintegrationParams::integrationCovariance
Matrix3 integrationCovariance
continuous-time "Covariance" describing integration uncertainty
Definition: PreintegrationParams.h:28
PreintegratedRotation.h
gtsam::PreintegrationParams::PreintegrationParams
PreintegrationParams()
Default constructor for serialization only.
Definition: PreintegrationParams.h:33
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegrationParams.h:26