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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:21