gtsam 4.2.0
gtsam
AHRSFactor.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
20#pragma once
21
22/* GTSAM includes */
26
27namespace gtsam {
28
35
36 protected:
37
38 Vector3 biasHat_;
40
41 friend class AHRSFactor;
42
43 public:
44
47
52 PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
53 const Vector3& biasHat) :
54 PreintegratedRotation(p), biasHat_(biasHat) {
55 resetIntegration();
56 }
57
68 const boost::shared_ptr<Params>& p,
69 const Vector3& bias_hat,
70 double deltaTij,
71 const Rot3& deltaRij,
72 const Matrix3& delRdelBiasOmega,
73 const Matrix3& preint_meas_cov) :
74 PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
75 biasHat_(bias_hat),
76 preintMeasCov_(preint_meas_cov) {}
77
78 Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
79 const Vector3& biasHat() const { return biasHat_; }
80 const Matrix3& preintMeasCov() const { return preintMeasCov_; }
81
83 void print(const std::string& s = "Preintegrated Measurements: ") const;
84
86 bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
87
89 void resetIntegration();
90
100 void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
101
104 Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
105
106 // This function is only used for test purposes
107 // (compare numerical derivatives wrt analytic ones)
108 static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
109 const Vector3& delta_angles);
110
112 PreintegratedAhrsMeasurements(const Vector3& biasHat,
113 const Matrix3& measuredOmegaCovariance)
114 : PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) {
115 p_->gyroscopeCovariance = measuredOmegaCovariance;
116 resetIntegration();
117 }
118
119private:
120
122 friend class boost::serialization::access;
123 template<class ARCHIVE>
124 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
125 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
126 ar & BOOST_SERIALIZATION_NVP(p_);
127 ar & BOOST_SERIALIZATION_NVP(biasHat_);
128 }
129};
130
131class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
132
133 typedef AHRSFactor This;
135
137
139 AHRSFactor() {}
140
141public:
142
144#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
145 typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
146#else
147 typedef boost::shared_ptr<AHRSFactor> shared_ptr;
148#endif
149
157 AHRSFactor(Key rot_i, Key rot_j, Key bias,
158 const PreintegratedAhrsMeasurements& preintegratedMeasurements);
159
160 ~AHRSFactor() override {
161 }
162
164 gtsam::NonlinearFactor::shared_ptr clone() const override;
165
167 void print(const std::string& s, const KeyFormatter& keyFormatter =
168 DefaultKeyFormatter) const override;
169
171 bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
172
175 return _PIM_;
176 }
177
181 Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
182 const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
183 boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
184 boost::none) const override;
185
188 static Rot3 Predict(const Rot3& rot_i, const Vector3& bias,
190
192 AHRSFactor(Key rot_i, Key rot_j, Key bias,
194 const Vector3& omegaCoriolis,
195 const boost::optional<Pose3>& body_P_sensor = boost::none);
196
198 static Rot3 predict(
199 const Rot3& rot_i, const Vector3& bias,
200 const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
201 const boost::optional<Pose3>& body_P_sensor = boost::none);
202
203#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
205 typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
206
207#endif
208
209private:
210
212 friend class boost::serialization::access;
213 template<class ARCHIVE>
214 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
215 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
216 ar
217 & boost::serialization::make_nvp("NoiseModelFactor3",
218 boost::serialization::base_object<Base>(*this));
219 ar & BOOST_SERIALIZATION_NVP(_PIM_);
220 }
221
222};
223// AHRSFactor
224
225} //namespace gtsam
3D Pose
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition: make_shared.h:57
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
std::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
Template to create a binary predicate.
Definition: Testable.h:111
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Definition: Factor.h:68
PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
Definition: AHRSFactor.h:34
Vector3 biasHat_
Angular rate bias values used during preintegration.
Definition: AHRSFactor.h:38
PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
Definition: AHRSFactor.h:112
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
Default constructor, initialize with no measurements.
Definition: AHRSFactor.h:52
Matrix3 preintMeasCov_
Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
Definition: AHRSFactor.h:39
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega, const Matrix3 &preint_meas_cov)
Non-Default constructor, initialize with measurements.
Definition: AHRSFactor.h:67
PreintegratedAhrsMeasurements()
Default constructor, only for serialization and wrappers.
Definition: AHRSFactor.h:46
Definition: AHRSFactor.h:131
const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: AHRSFactor.h:174
boost::shared_ptr< AHRSFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: AHRSFactor.h:147
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegratedRotation.h:31
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition: PreintegratedRotation.h:89
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400