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
12/**
13 * @file PreintegratedRotation.h
14 * @author Luca Carlone
15 * @author Stephen Williams
16 * @author Richard Roberts
17 * @author Vadim Indelman
18 * @author David Jensen
19 * @author Frank Dellaert
20 **/
21
22#pragma once
23
24#include <gtsam/base/Matrix.h>
25#include <gtsam/base/std_optional_serialization.h>
26#include <gtsam/geometry/Pose3.h>
27#include "gtsam/dllexport.h"
28
29namespace gtsam {
30
31namespace internal {
32/**
33 * @brief Function object for incremental rotation.
34 * @param measuredOmega The measured angular velocity (as given by the sensor)
35 * @param deltaT The time interval over which the rotation is integrated.
36 * @param body_P_sensor Optional transform between body and IMU.
37 */
38struct GTSAM_EXPORT IncrementalRotation {
39 const Vector3& measuredOmega;
40 const double deltaT;
41 const std::optional<Pose3>& body_P_sensor;
42
43 /**
44 * @brief Integrate angular velocity, but corrected by bias.
45 * @param bias The bias estimate
46 * @param H_bias Jacobian of the rotation w.r.t. bias.
47 * @return The incremental rotation
48 */
49 Rot3 operator()(const Vector3& bias,
50 OptionalJacobian<3, 3> H_bias = {}) const;
51};
52
53} // namespace internal
54
55/// Parameters for pre-integration:
56/// Usage: Create just a single Params and pass a shared pointer to the constructor
57struct GTSAM_EXPORT PreintegratedRotationParams {
58 /// Continuous-time "Covariance" of gyroscope measurements
59 /// The units for stddev are σ = rad/s/√Hz
60 Matrix3 gyroscopeCovariance;
61 std::optional<Vector3> omegaCoriolis; ///< Coriolis constant
62 std::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
63
64 PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
65
66 PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
67 std::optional<Vector3> omega_coriolis = {},
68 std::optional<Pose3> body_P_sensor = {})
69 : gyroscopeCovariance(gyroscope_covariance),
70 omegaCoriolis(omega_coriolis),
71 body_P_sensor(body_P_sensor) {}
72
73 virtual ~PreintegratedRotationParams() {}
74
75 virtual void print(const std::string& s) const;
76 virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
77
78 void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
79 void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis = omega; }
80 void setBodyPSensor(const Pose3& pose) { body_P_sensor = pose; }
81
82 const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
83 std::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
84 std::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
85
86 private:
87#if GTSAM_ENABLE_BOOST_SERIALIZATION
88 /** Serialization function */
89 friend class boost::serialization::access;
90 template<class ARCHIVE>
91 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
92 ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
93 ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
94
95 // Provide support for Eigen::Matrix in std::optional
96 bool omegaCoriolisFlag = omegaCoriolis.has_value();
97 ar & boost::serialization::make_nvp(n: "omegaCoriolisFlag", v&: omegaCoriolisFlag);
98 if (omegaCoriolisFlag) {
99 ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
100 }
101 }
102#endif
103
104#ifdef GTSAM_USE_QUATERNIONS
105 // Align if we are using Quaternions
106public:
107 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
108#endif
109};
110
111/**
112 * PreintegratedRotation is the base class for all PreintegratedMeasurements
113 * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
114 * It includes the definitions of the preintegrated rotation.
115 */
116class GTSAM_EXPORT PreintegratedRotation {
117 public:
118 typedef PreintegratedRotationParams Params;
119
120 protected:
121 /// Parameters
122 std::shared_ptr<Params> p_;
123
124 double deltaTij_; ///< Time interval from i to j
125 Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
126 Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
127
128 public:
129 /// @name Constructors
130 /// @{
131
132 /// Default constructor for serialization
133 PreintegratedRotation() {}
134
135 /// Default constructor, resets integration to zero
136 explicit PreintegratedRotation(const std::shared_ptr<Params>& p) : p_(p) {
137 resetIntegration();
138 }
139
140 /// Explicit initialization of all class members
141 PreintegratedRotation(const std::shared_ptr<Params>& p,
142 double deltaTij, const Rot3& deltaRij,
143 const Matrix3& delRdelBiasOmega)
144 : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
145
146 /// @}
147
148 /// @name Basic utilities
149 /// @{
150
151 /// check parameters equality: checks whether shared pointer points to same Params object.
152 bool matchesParamsWith(const PreintegratedRotation& other) const {
153 return p_ == other.p_;
154 }
155 /// @}
156
157 /// @name Access instance variables
158 /// @{
159 const std::shared_ptr<Params>& params() const { return p_; }
160 const double& deltaTij() const { return deltaTij_; }
161 const Rot3& deltaRij() const { return deltaRij_; }
162 const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; }
163 /// @}
164
165 /// @name Testable
166 /// @{
167 void print(const std::string& s) const;
168 bool equals(const PreintegratedRotation& other, double tol) const;
169 /// @}
170
171 /// @name Main functionality
172 /// @{
173
174 /// Re-initialize PreintegratedMeasurements
175 void resetIntegration();
176
177 /**
178 * @brief Calculate an incremental rotation given the gyro measurement and a
179 * time interval, and update both deltaTij_ and deltaRij_.
180 * @param measuredOmega The measured angular velocity (as given by the sensor)
181 * @param bias The biasHat estimate
182 * @param deltaT The time interval
183 * @param F optional Jacobian of internal compose, used in AhrsFactor.
184 */
185 void integrateGyroMeasurement(const Vector3& measuredOmega,
186 const Vector3& biasHat, double deltaT,
187 OptionalJacobian<3, 3> F = {});
188
189 /**
190 * @brief Return a bias corrected version of the integrated rotation.
191 * @param biasOmegaIncr An increment with respect to biasHat used above.
192 * @param H optional Jacobian of the correction w.r.t. the bias increment.
193 * @note The *key* functionality of this class used in optimizing the bias.
194 */
195 Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
196 OptionalJacobian<3, 3> H = {}) const;
197
198 /// Integrate coriolis correction in body frame Ri
199 Vector3 integrateCoriolis(const Rot3& Ri,
200 OptionalJacobian<3, 3> H = {}) const;
201
202 /// @}
203
204 /// @name Deprecated API
205 /// @{
206
207#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
208 /// @deprecated: use IncrementalRotation functor with sane Jacobian
209 inline Rot3 GTSAM_DEPRECATED incrementalRotation(
210 const Vector3& measuredOmega, const Vector3& bias, double deltaT,
211 OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
212 internal::IncrementalRotation f{.measuredOmega: measuredOmega, .deltaT: deltaT, .body_P_sensor: p_->body_P_sensor};
213 Rot3 incrR = f(bias, D_incrR_integratedOmega);
214 // Backwards compatible "weird" Jacobian, no longer used.
215 if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
216 return incrR;
217 }
218
219 /// @deprecated: use integrateGyroMeasurement from now on
220 /// @note this returned hard-to-understand Jacobian D_incrR_integratedOmega.
221 void GTSAM_DEPRECATED integrateMeasurement(
222 const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
223 OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
224
225#endif
226
227 /// @}
228
229 private:
230#if GTSAM_ENABLE_BOOST_SERIALIZATION
231 /** Serialization function */
232 friend class boost::serialization::access;
233 template <class ARCHIVE>
234 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
235 ar& BOOST_SERIALIZATION_NVP(p_);
236 ar& BOOST_SERIALIZATION_NVP(deltaTij_);
237 ar& BOOST_SERIALIZATION_NVP(deltaRij_);
238 ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
239 }
240#endif
241
242#ifdef GTSAM_USE_QUATERNIONS
243 // Align if we are using Quaternions
244 public:
245 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
246#endif
247};
248
249template <>
250struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
251
252} /// namespace gtsam
253