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  std::shared_ptr<Params> p_;
48 
51 
53  double deltaTij_;
54 
57 
59  virtual ~PreintegrationBase() {}
60 
61  public:
64 
70  PreintegrationBase(const std::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 
87  return p_.get() == other.p_.get();
88  }
89 
91  const std::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 = {},
133  OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = {},
134  OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = {}) 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 = {}) const = 0;
152 
154  NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
155  OptionalJacobian<9, 9> H1 = {},
156  OptionalJacobian<9, 6> H2 = {}) const;
157 
159  Vector9 computeError(const NavState& state_i, const NavState& state_j,
160  const imuBias::ConstantBias& bias_i,
161  OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
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,
170  const imuBias::ConstantBias& bias_i,
171  OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {},
172  OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {},
173  OptionalJacobian<9, 6> H5 = {}) const;
174 
175  private:
176 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
177 
178  friend class boost::serialization::access;
179  template<class ARCHIVE>
180  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
181  ar & BOOST_SERIALIZATION_NVP(p_);
182  ar & BOOST_SERIALIZATION_NVP(biasHat_);
183  ar & BOOST_SERIALIZATION_NVP(deltaTij_);
184  }
185 #endif
186 
187  public:
189 };
190 
191 }
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
relicense.update
def update(text)
Definition: relicense.py:46
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::PreintegrationParams
Definition: PreintegrationParams.h:25
B
Definition: test_numpy_dtypes.cpp:299
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::PreintegrationBase
Definition: PreintegrationBase.h:41
gtsam::NavState
Definition: NavState.h:34
gtsam::PreintegrationBase::Bias
imuBias::ConstantBias Bias
Definition: PreintegrationBase.h:43
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
Definition: ImuBias.h:59
os
ofstream os("timeSchurFactors.csv")
gtsam::PreintegrationBase::biasHatVector
Vector6 biasHatVector() const
Definition: PreintegrationBase.h:113
gtsam::PreintegrationBase::~PreintegrationBase
virtual ~PreintegrationBase()
Virtual destructor for serialization.
Definition: PreintegrationBase.h:59
gtsam::PreintegrationBase::biasHat_
Bias biasHat_
Acceleration and gyro bias used for preintegration.
Definition: PreintegrationBase.h:50
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::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
A
Definition: test_numpy_dtypes.cpp:298
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam::PreintegrationBase::p
Params & p() const
const reference to params
Definition: PreintegrationBase.h:96
gtsam::PreintegrationBase::p_
std::shared_ptr< Params > p_
Definition: PreintegrationBase.h:47
gtsam::PreintegrationBase::Params
PreintegrationParams Params
Definition: PreintegrationBase.h:44
ImuBias.h
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::PreintegrationBase::deltaTij_
double deltaTij_
Time interval from i to j.
Definition: PreintegrationBase.h:53
gtsam::PreintegrationBase::PreintegrationBase
PreintegrationBase()
Default constructor for serialization.
Definition: PreintegrationBase.h:56
gtsam::PreintegrationBase::biasHat
const imuBias::ConstantBias & biasHat() const
Definition: PreintegrationBase.h:104
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
Eigen::Matrix< double, 9, 9 >
Pose3
Definition: testDependencies.h:3
gtsam::PreintegrationBase::deltaTij
double deltaTij() const
Definition: PreintegrationBase.h:105
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
PreintegrationParams.h
gtsam::PreintegrationBase::params
const std::shared_ptr< Params > & params() const
shared pointer to params
Definition: PreintegrationBase.h:91


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:25