ImuFactor.cpp
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 
23 
24 /* External or standard includes */
25 #include <ostream>
26 #include <cassert>
27 
28 namespace gtsam {
29 
30 using namespace std;
31 
32 //------------------------------------------------------------------------------
33 // Inner class PreintegratedImuMeasurements
34 //------------------------------------------------------------------------------
35 void PreintegratedImuMeasurements::print(const string& s) const {
37  cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
38 }
39 
40 //------------------------------------------------------------------------------
42  const PreintegratedImuMeasurements& other, double tol) const {
44  && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
45 }
46 
47 //------------------------------------------------------------------------------
50  preintMeasCov_.setZero();
51 }
52 
53 //------------------------------------------------------------------------------
55  const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
56  if (dt <= 0) {
57  throw std::runtime_error(
58  "PreintegratedImuMeasurements::integrateMeasurement: dt <=0");
59  }
60 
61  // Update preintegrated measurements (also get Jacobian)
62  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
63  Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively.
65 
66  // first order covariance propagation:
67  // as in [2] we consider a first order propagation that can be seen as a
68  // prediction phase in EKF
69 
70  // propagate uncertainty
71  // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
72  const Matrix3& aCov = p().accelerometerCovariance;
73  const Matrix3& wCov = p().gyroscopeCovariance;
74  const Matrix3& iCov = p().integrationCovariance;
75 
76  // (1/dt) allows to pass from continuous time noise to discrete time noise
77  // Update the uncertainty on the state (matrix A in [4]).
78  preintMeasCov_ = A * preintMeasCov_ * A.transpose();
79  // These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]).
80  preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
81  preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
82 
83  // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix)
84  preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
85 }
86 
87 //------------------------------------------------------------------------------
89  const Matrix& measuredAccs, const Matrix& measuredOmegas,
90  const Matrix& dts) {
91  assert(
92  measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
93  assert(dts.cols() >= 1);
94  assert(measuredAccs.cols() == dts.cols());
95  assert(measuredOmegas.cols() == dts.cols());
96  size_t n = static_cast<size_t>(dts.cols());
97  for (size_t j = 0; j < n; j++) {
98  integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j));
99  }
100 }
101 
102 //------------------------------------------------------------------------------
103 #ifdef GTSAM_TANGENT_PREINTEGRATION
104 void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
105  Matrix9* H1, Matrix9* H2) {
106  PreintegrationType::mergeWith(pim12, H1, H2);
107  // NOTE(gareth): Temporary P is needed as of Eigen 3.3
108  const Matrix9 P = *H1 * preintMeasCov_ * H1->transpose();
109  preintMeasCov_ = P + *H2 * pim12.preintMeasCov_ * H2->transpose();
110 }
111 #endif
112 
113 //------------------------------------------------------------------------------
114 // ImuFactor methods
115 //------------------------------------------------------------------------------
116 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
117  const PreintegratedImuMeasurements& pim) :
118  Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
119  pose_j, vel_j, bias), _PIM_(pim) {
120 }
121 
122 //------------------------------------------------------------------------------
124  return std::static_pointer_cast<NonlinearFactor>(
125  NonlinearFactor::shared_ptr(new This(*this)));
126 }
127 
128 //------------------------------------------------------------------------------
129 std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
130  f._PIM_.print("preintegrated measurements:\n");
131  os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
132  return os;
133 }
134 
135 //------------------------------------------------------------------------------
136 void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
137  cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key<1>())
138  << "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>())
139  << "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>())
140  << ")\n";
141  cout << *this << endl;
142 }
143 
144 //------------------------------------------------------------------------------
145 bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
146  const This *e = dynamic_cast<const This*>(&other);
147  const bool base = Base::equals(*e, tol);
148  const bool pim = _PIM_.equals(e->_PIM_, tol);
149  return e != nullptr && base && pim;
150 }
151 
152 //------------------------------------------------------------------------------
153 Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
154  const Pose3& pose_j, const Vector3& vel_j,
155  const imuBias::ConstantBias& bias_i, OptionalMatrixType H1,
157  OptionalMatrixType H4, OptionalMatrixType H5) const {
158  return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
159  H1, H2, H3, H4, H5);
160 }
161 
162 //------------------------------------------------------------------------------
163 #ifdef GTSAM_TANGENT_PREINTEGRATION
164 PreintegratedImuMeasurements ImuFactor::Merge(
165  const PreintegratedImuMeasurements& pim01,
166  const PreintegratedImuMeasurements& pim12) {
167  if (!pim01.matchesParamsWith(pim12))
168  throw std::domain_error(
169  "Cannot merge PreintegratedImuMeasurements with different params");
170 
171  if (pim01.params()->body_P_sensor)
172  throw std::domain_error(
173  "Cannot merge PreintegratedImuMeasurements with sensor pose yet");
174 
175  // the bias for the merged factor will be the bias from 01
176  PreintegratedImuMeasurements pim02 = pim01;
177 
178  Matrix9 H1, H2;
179  pim02.mergeWith(pim12, &H1, &H2);
180 
181  return pim02;
182 }
183 
184 //------------------------------------------------------------------------------
185 ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
186  const shared_ptr& f12) {
187  // IMU bias keys must be the same.
188  if (f01->key<5>() != f12->key<5>())
189  throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
190 
191  // expect intermediate pose, velocity keys to matchup.
192  if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>())
193  throw std::domain_error(
194  "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
195 
196  // return new factor
197  auto pim02 =
198  Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
199  return std::make_shared<ImuFactor>(f01->key<1>(), // P0
200  f01->key<2>(), // V0
201  f12->key<3>(), // P2
202  f12->key<4>(), // V2
203  f01->key<5>(), // B
204  pim02);
205 }
206 #endif
207 
208 //------------------------------------------------------------------------------
209 // ImuFactor2 methods
210 //------------------------------------------------------------------------------
212  const PreintegratedImuMeasurements& pim) :
213  Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j,
214  bias), _PIM_(pim) {
215 }
216 
217 //------------------------------------------------------------------------------
219  return std::static_pointer_cast<NonlinearFactor>(
220  NonlinearFactor::shared_ptr(new This(*this)));
221 }
222 
223 //------------------------------------------------------------------------------
224 std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
225  f._PIM_.print("preintegrated measurements:\n");
226  os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
227  return os;
228 }
229 
230 //------------------------------------------------------------------------------
231 void ImuFactor2::print(const string& s,
232  const KeyFormatter& keyFormatter) const {
233  cout << (s.empty() ? s : s + "\n") << "ImuFactor2("
234  << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
235  << keyFormatter(this->key<3>()) << ")\n";
236  cout << *this << endl;
237 }
238 
239 //------------------------------------------------------------------------------
240 bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
241  const This *e = dynamic_cast<const This*>(&other);
242  const bool base = Base::equals(*e, tol);
243  const bool pim = _PIM_.equals(e->_PIM_, tol);
244  return e != nullptr && base && pim;
245 }
246 
247 //------------------------------------------------------------------------------
249  const NavState& state_j,
250  const imuBias::ConstantBias& bias_i, //
252  OptionalMatrixType H3) const {
253  return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
254 }
255 
256 //------------------------------------------------------------------------------
257 
258 }
259 // namespace gtsam
Gaussian
double Gaussian(double mu, double sigma, double z)
Gaussian density function.
Definition: testGaussianMixture.cpp:47
gtsam::ManifoldPreintegration::update
void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
Definition: ManifoldPreintegration.cpp:60
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::PreintegrationBase::computeErrorAndJacobians
Vector9 computeErrorAndJacobians(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H1={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 6 > H3={}, OptionalJacobian< 9, 3 > H4={}, OptionalJacobian< 9, 6 > H5={}) const
Definition: PreintegrationBase.cpp:169
gtsam::ImuFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: ImuFactor.cpp:145
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::PreintegratedImuMeasurements::integrateMeasurement
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Definition: ImuFactor.cpp:54
gtsam::PreintegrationBase::print
virtual void print(const std::string &s="") const
Definition: PreintegrationBase.cpp:50
gtsam::ImuFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: ImuFactor.cpp:136
dt
const double dt
Definition: testVelocityConstraint.cpp:15
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::PreintegratedImuMeasurements::resetIntegration
void resetIntegration() override
Re-initialize PreintegratedIMUMeasurements.
Definition: ImuFactor.cpp:48
gtsam::ImuFactor::shared_ptr
std::shared_ptr< ImuFactor > shared_ptr
Definition: ImuFactor.h:188
gtsam::ImuFactor::ImuFactor
ImuFactor()
Definition: ImuFactor.h:192
gtsam::ImuFactor2::_PIM_
PreintegratedImuMeasurements _PIM_
Definition: ImuFactor.h:270
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::NavState
Definition: NavState.h:38
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::ImuFactor2
Definition: ImuFactor.h:264
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::ImuFactor2::ImuFactor2
ImuFactor2()
Definition: ImuFactor.h:278
gtsam::PreintegratedImuMeasurements::preintMeasCov_
Matrix9 preintMeasCov_
(first-order propagation from measurementCovariance).
Definition: ImuFactor.h:75
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::ImuFactor::_PIM_
PreintegratedImuMeasurements _PIM_
Definition: ImuFactor.h:177
n
int n
Definition: BiCGSTAB_simple.cpp:1
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::ImuFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: ImuFactor.cpp:218
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam::ImuFactor::evaluateError
Vector evaluateError(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override
vector of errors
Definition: ImuFactor.cpp:153
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::KeyFormatter
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
gtsam::Pose3
Definition: Pose3.h:37
gtsam::ImuFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: ImuFactor.cpp:123
gtsam::PreintegratedImuMeasurements::equals
bool equals(const PreintegratedImuMeasurements &expected, double tol=1e-9) const
equals
Definition: ImuFactor.cpp:41
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:68
gtsam::NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias >
gtsam::ImuFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: ImuFactor.cpp:231
gtsam::ImuFactor
Definition: ImuFactor.h:169
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:82
gtsam::ImuFactor::This
ImuFactor This
Definition: ImuFactor.h:173
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::ManifoldPreintegration::resetIntegration
void resetIntegration() override
Definition: ManifoldPreintegration.cpp:36
gtsam::PreintegrationBase::computeError
Vector9 computeError(const NavState &state_i, const NavState &state_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1, OptionalJacobian< 9, 9 > H2, OptionalJacobian< 9, 6 > H3) const
Calculate error given navStates.
Definition: PreintegrationBase.cpp:143
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
gtsam::ImuFactor2::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: ImuFactor.cpp:240
std
Definition: BFloat16.h:88
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
gtsam::ImuFactor2::evaluateError
Vector evaluateError(const NavState &state_i, const NavState &state_j, const imuBias::ConstantBias &bias_i, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
vector of errors
Definition: ImuFactor.cpp:248
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::PreintegratedImuMeasurements::integrateMeasurements
void integrateMeasurements(const Matrix &measuredAccs, const Matrix &measuredOmegas, const Matrix &dts)
Add multiple measurements, in matrix columns.
Definition: ImuFactor.cpp:88
P
static double P[]
Definition: ellpe.c:68
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
Eigen::Matrix< double, 9, 9 >
gtsam::PreintegratedImuMeasurements::print
void print(const std::string &s="Preintegrated Measurements:") const override
print
Definition: ImuFactor.cpp:35
gtsam::ImuFactor2::This
ImuFactor2 This
Definition: ImuFactor.h:267
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::ManifoldPreintegration::equals
bool equals(const ManifoldPreintegration &other, double tol) const
Definition: ManifoldPreintegration.cpp:47
ImuFactor.h
gtsam::PreintegrationBase::params
const std::shared_ptr< Params > & params() const
shared pointer to params
Definition: PreintegrationBase.h:91


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:41