gtsam 4.1.1
gtsam
PreintegratedRotation.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
25#include <gtsam/base/Matrix.h>
26
27namespace gtsam {
28
31struct GTSAM_EXPORT PreintegratedRotationParams {
35 boost::optional<Vector3> omegaCoriolis;
36 boost::optional<Pose3> body_P_sensor;
37
38 PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
39
40 PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
41 boost::optional<Vector3> omega_coriolis)
42 : gyroscopeCovariance(gyroscope_covariance) {
43 if (omega_coriolis)
44 omegaCoriolis.reset(omega_coriolis.get());
45 }
46
47 virtual ~PreintegratedRotationParams() {}
48
49 virtual void print(const std::string& s) const;
50 virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
51
52 void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
53 void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); }
54 void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); }
55
56 const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
57 boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
58 boost::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
59
60 private:
62 friend class boost::serialization::access;
63 template<class ARCHIVE>
64 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
65 namespace bs = ::boost::serialization;
66 ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
67 ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
68
69 // Provide support for Eigen::Matrix in boost::optional
70 bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
71 ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
72 if (omegaCoriolisFlag) {
73 ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
74 }
75 }
76
77#ifdef GTSAM_USE_QUATERNIONS
78 // Align if we are using Quaternions
79public:
81#endif
82};
83
89class GTSAM_EXPORT PreintegratedRotation {
90 public:
92
93 protected:
95 boost::shared_ptr<Params> p_;
96
97 double deltaTij_;
100
103
104 public:
107
109 explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
110 resetIntegration();
111 }
112
114 PreintegratedRotation(const boost::shared_ptr<Params>& p,
115 double deltaTij, const Rot3& deltaRij,
116 const Matrix3& delRdelBiasOmega)
117 : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
118
120
123
125 void resetIntegration();
126
128 bool matchesParamsWith(const PreintegratedRotation& other) const {
129 return p_ == other.p_;
130 }
132
135 const boost::shared_ptr<Params>& params() const {
136 return p_;
137 }
138 const double& deltaTij() const {
139 return deltaTij_;
140 }
141 const Rot3& deltaRij() const {
142 return deltaRij_;
143 }
144 const Matrix3& delRdelBiasOmega() const {
145 return delRdelBiasOmega_;
146 }
148
151 void print(const std::string& s) const;
152 bool equals(const PreintegratedRotation& other, double tol) const;
154
157
161 Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
162 OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
163
166 void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
167 OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
168 OptionalJacobian<3, 3> F = boost::none);
169
171 Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
172 OptionalJacobian<3, 3> H = boost::none) const;
173
175 Vector3 integrateCoriolis(const Rot3& rot_i) const;
176
178
179 private:
181 friend class boost::serialization::access;
182 template <class ARCHIVE>
183 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
184 ar& BOOST_SERIALIZATION_NVP(p_);
185 ar& BOOST_SERIALIZATION_NVP(deltaTij_);
186 ar& BOOST_SERIALIZATION_NVP(deltaRij_);
187 ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
188 }
189
190#ifdef GTSAM_USE_QUATERNIONS
191 // Align if we are using Quaternions
192 public:
194#endif
195};
196
197template <>
198struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
199
200}
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:277
typedef and functions to augment Eigen's MatrixXd
3D Pose
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
Definition: Rot3.h:59
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegratedRotation.h:31
boost::optional< Pose3 > body_P_sensor
The pose of the sensor in the body frame.
Definition: PreintegratedRotation.h:36
boost::optional< Vector3 > omegaCoriolis
Coriolis constant.
Definition: PreintegratedRotation.h:35
Matrix3 gyroscopeCovariance
Continuous-time "Covariance" of gyroscope measurements The units for stddev are σ = rad/s/√Hz.
Definition: PreintegratedRotation.h:34
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition: PreintegratedRotation.h:89
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition: PreintegratedRotation.h:99
boost::shared_ptr< Params > p_
Parameters.
Definition: PreintegratedRotation.h:95
PreintegratedRotation(const boost::shared_ptr< Params > &p, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega)
Explicit initialization of all class members.
Definition: PreintegratedRotation.h:114
PreintegratedRotation(const boost::shared_ptr< Params > &p)
Default constructor, resets integration to zero.
Definition: PreintegratedRotation.h:109
double deltaTij_
Time interval from i to j.
Definition: PreintegratedRotation.h:97
bool matchesParamsWith(const PreintegratedRotation &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition: PreintegratedRotation.h:128
PreintegratedRotation()
Default constructor for serialization.
Definition: PreintegratedRotation.h:102
Rot3 deltaRij_
Preintegrated relative orientation (in frame i)
Definition: PreintegratedRotation.h:98