gtsam  4.1.0
gtsam
PreintegrationBase.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 
28 
29 #include <iosfwd>
30 #include <string>
31 #include <utility>
32 
33 namespace gtsam {
34 
41 class GTSAM_EXPORT PreintegrationBase {
42  public:
45 
46  protected:
47  boost::shared_ptr<Params> p_;
48 
51 
53  double deltaTij_;
54 
57 
59  virtual ~PreintegrationBase() {}
60 
61  public:
64 
70  PreintegrationBase(const boost::shared_ptr<Params>& p,
71  const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
72 
74 
78  virtual void resetIntegration() = 0;
79 
83  void resetIntegrationAndSetBias(const Bias& biasHat);
84 
86  bool matchesParamsWith(const PreintegrationBase& other) const {
87  return p_.get() == other.p_.get();
88  }
89 
91  const boost::shared_ptr<Params>& params() const {
92  return p_;
93  }
94 
96  Params& p() const {
97  return *p_;
98  }
99 
101 
104  const imuBias::ConstantBias& biasHat() const { return biasHat_; }
105  double deltaTij() const { return deltaTij_; }
106 
107  virtual Vector3 deltaPij() const = 0;
108  virtual Vector3 deltaVij() const = 0;
109  virtual Rot3 deltaRij() const = 0;
110  virtual NavState deltaXij() const = 0;
111 
112  // Exposed for MATLAB
113  Vector6 biasHatVector() const { return biasHat_.vector(); }
115 
118  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
119  virtual void print(const std::string& s="") const;
121 
124 
130  std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
131  const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
132  OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
133  OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
134  OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
135 
141  virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
142  const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0;
143 
145  virtual void integrateMeasurement(const Vector3& measuredAcc,
146  const Vector3& measuredOmega, const double dt);
147 
150  virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
151  OptionalJacobian<9, 6> H = boost::none) const = 0;
152 
154  NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
155  OptionalJacobian<9, 9> H1 = boost::none,
156  OptionalJacobian<9, 6> H2 = boost::none) const;
157 
159  Vector9 computeError(const NavState& state_i, const NavState& state_j,
160  const imuBias::ConstantBias& bias_i,
162  OptionalJacobian<9, 6> H3) const;
163 
168  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
169  const Pose3& pose_j, const Vector3& vel_j,
171  boost::none, OptionalJacobian<9, 3> H2 = boost::none,
172  OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
173  boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
174 
175  private:
177  friend class boost::serialization::access;
178  template<class ARCHIVE>
179  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
180  ar & BOOST_SERIALIZATION_NVP(p_);
181  ar & BOOST_SERIALIZATION_NVP(biasHat_);
182  ar & BOOST_SERIALIZATION_NVP(deltaTij_);
183  }
184 
185  public:
187 };
188 
189 }
ImuBias.h
gtsam::PreintegrationBase::matchesParamsWith
bool matchesParamsWith(const PreintegrationBase &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition: PreintegrationBase.h:86
gtsam::PreintegrationBase::update
virtual void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C)=0
Update preintegrated measurements and get derivatives It takes measured quantities in the j frame Mod...
gtsam::PreintegrationBase::biasHat_
Bias biasHat_
Acceleration and gyro bias used for preintegration.
Definition: PreintegrationBase.h:50
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition: PreintegrationBase.h:41
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam::PreintegrationBase::params
const boost::shared_ptr< Params > & params() const
shared pointer to params
Definition: PreintegrationBase.h:91
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:30
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::PreintegrationBase::deltaTij_
double deltaTij_
Time interval from i to j.
Definition: PreintegrationBase.h:53
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::PreintegrationBase::biasCorrectedDelta
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H=boost::none) const =0
Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU me...
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::PreintegrationBase::p
Params & p() const
const reference to params
Definition: PreintegrationBase.h:96
PreintegrationParams.h
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PreintegrationBase::~PreintegrationBase
virtual ~PreintegrationBase()
Virtual destructor for serialization.
Definition: PreintegrationBase.h:59
gtsam::PreintegrationBase::PreintegrationBase
PreintegrationBase()
Default constructor for serialization.
Definition: PreintegrationBase.h:56
NoiseModel.h
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition: ImuBias.h:52
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegrationParams.h:26