gtsam  4.1.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 */
25 #include <gtsam/geometry/Pose3.h>
26 
27 namespace gtsam {
28 
35 
36  protected:
37 
38  Vector3 biasHat_;
39  Matrix3 preintMeasCov_;
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 
96  void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
97 
100  Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
101 
102  // This function is only used for test purposes
103  // (compare numerical derivatives wrt analytic ones)
104  static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
105  const Vector3& delta_angles);
106 
108  PreintegratedAhrsMeasurements(const Vector3& biasHat,
109  const Matrix3& measuredOmegaCovariance)
111  biasHat_(biasHat) {
112  p_->gyroscopeCovariance = measuredOmegaCovariance;
113  resetIntegration();
114  }
115 
116 private:
117 
119  friend class boost::serialization::access;
120  template<class ARCHIVE>
121  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
122  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
123  ar & BOOST_SERIALIZATION_NVP(p_);
124  ar & BOOST_SERIALIZATION_NVP(biasHat_);
125  }
126 };
127 
128 class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
129 
130  typedef AHRSFactor This;
132 
134 
136  AHRSFactor() {}
137 
138 public:
139 
141 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
142  typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
143 #else
144  typedef boost::shared_ptr<AHRSFactor> shared_ptr;
145 #endif
146 
154  AHRSFactor(Key rot_i, Key rot_j, Key bias,
155  const PreintegratedAhrsMeasurements& preintegratedMeasurements);
156 
157  virtual ~AHRSFactor() {
158  }
159 
161  gtsam::NonlinearFactor::shared_ptr clone() const override;
162 
164  void print(const std::string& s, const KeyFormatter& keyFormatter =
165  DefaultKeyFormatter) const override;
166 
168  bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
169 
172  return _PIM_;
173  }
174 
177  Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
179  const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
180  boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
181  boost::none) const override;
182 
185  static Rot3 Predict(
186  const Rot3& rot_i, const Vector3& bias,
187  const PreintegratedAhrsMeasurements preintegratedMeasurements);
188 
191 
193  AHRSFactor(Key rot_i, Key rot_j, Key bias,
194  const PreintegratedMeasurements& preintegratedMeasurements,
195  const Vector3& omegaCoriolis,
196  const boost::optional<Pose3>& body_P_sensor = boost::none);
197 
199  static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
200  const PreintegratedMeasurements preintegratedMeasurements,
201  const Vector3& omegaCoriolis,
202  const boost::optional<Pose3>& body_P_sensor = boost::none);
203 
204 private:
205 
207  friend class boost::serialization::access;
208  template<class ARCHIVE>
209  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
210  ar
211  & boost::serialization::make_nvp("NoiseModelFactor3",
212  boost::serialization::base_object<Base>(*this));
213  ar & BOOST_SERIALIZATION_NVP(_PIM_);
214  }
215 
216 };
217 // AHRSFactor
218 
219 } //namespace gtsam
gtsam::PreintegratedRotation
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition: PreintegratedRotation.h:87
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::AHRSFactor
Definition: AHRSFactor.h:128
gtsam::PreintegratedAhrsMeasurements::preintMeasCov_
Matrix3 preintMeasCov_
Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
Definition: AHRSFactor.h:39
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
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
Pose3.h
3D Pose
gtsam::AHRSFactor::PreintegratedMeasurements
PreintegratedAhrsMeasurements PreintegratedMeasurements
Definition: AHRSFactor.h:190
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
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
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::PreintegratedAhrsMeasurements::biasHat_
Vector3 biasHat_
Angular rate bias values used during preintegration.
Definition: AHRSFactor.h:38
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::AHRSFactor::shared_ptr
boost::shared_ptr< AHRSFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: AHRSFactor.h:144
gtsam::Rot3
Definition: Rot3.h:59
gtsam::AHRSFactor::preintegratedMeasurements
const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: AHRSFactor.h:171
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::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
Definition: AHRSFactor.h:108
gtsam::PreintegratedAhrsMeasurements
PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
Definition: AHRSFactor.h:34
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
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements()
Default constructor, only for serialization and wrappers.
Definition: AHRSFactor.h:46
PreintegratedRotation.h
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
Default constructor, initialize with no measurements.
Definition: AHRSFactor.h:52
gtsam::make_shared
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