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