testImuFactor.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 
21 // #define ENABLE_TIMING // uncomment for timing results
22 
25 #include <gtsam/geometry/Pose3.h>
26 #include <gtsam/nonlinear/Values.h>
28 #include <gtsam/linear/Sampler.h>
31 
33 #include <list>
34 
35 #include "imuFactorTesting.h"
36 
37 namespace testing {
38 // Create default parameters with Z-down and above noise parameters
39 static std::shared_ptr<PreintegrationParams> Params() {
40  auto p = PreintegrationParams::MakeSharedD(kGravity);
41  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
42  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
43  p->integrationCovariance = 0.0001 * I_3x3;
44  return p;
45 }
46 }
47 
48 /* ************************************************************************* */
49 namespace {
50 // Auxiliary functions to test evaluate error in ImuFactor
51 /* ************************************************************************* */
52 Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
53  const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
54  const Bias& bias) {
55  return Rot3::Expmap(
56  factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3));
57 }
58 
59 } // namespace
60 
61 /* ************************************************************************* */
62 TEST(ImuFactor, PreintegratedMeasurementsConstruction) {
63  // Actual pre-integrated values
65  EXPECT(assert_equal(Rot3(), actual.deltaRij()));
66  EXPECT(assert_equal(kZero, actual.deltaPij()));
67  EXPECT(assert_equal(kZero, actual.deltaVij()));
68  DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
69 }
70 
71 /* ************************************************************************* */
72 TEST(ImuFactor, PreintegratedMeasurementsReset) {
73 
74  auto p = testing::Params();
75  // Create a preintegrated measurement struct and integrate
77  Vector3 measuredAcc(0.5, 1.0, 0.5);
78  Vector3 measuredOmega(0.1, 0.3, 0.1);
79  double deltaT = 1.0;
81 
82  // reset and make sure that it is the same as a fresh one
83  pimActual.resetIntegration();
85 
86  // Now create one with a different bias ..
87  Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
88  PreintegratedImuMeasurements pimExpected(p, nonZeroBias);
89 
90  // integrate again, then reset to a new bias
92  pimActual.resetIntegrationAndSetBias(nonZeroBias);
93  CHECK(assert_equal(pimActual, pimExpected));
94 }
95 
96 /* ************************************************************************* */
97 TEST(ImuFactor, Accelerating) {
98  const double a = 0.2, v = 50;
99 
100  // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
101  // The body itself has Z axis pointing down
102  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
103  const Point3 initial_position(10, 20, 0);
104  const Vector3 initial_velocity(v, 0, 0);
105 
106  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
107  Vector3(a, 0, 0));
108 
109  const double T = 3.0; // seconds
110  ScenarioRunner runner(scenario, testing::Params(), T / 10);
111 
113  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
114 
115  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
116  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
117 }
118 
119 /* ************************************************************************* */
120 TEST(ImuFactor, PreintegratedMeasurements) {
121  // Measurements
122  const double a = 0.1, w = M_PI / 100.0;
123  Vector3 measuredAcc(a, 0.0, 0.0);
124  Vector3 measuredOmega(w, 0.0, 0.0);
125  double deltaT = 0.5;
126 
127  // Expected pre-integrated values
128  Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0);
129  Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0);
130  Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
131 
132  // Actual pre-integrated values
135  EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij()));
136  EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij()));
137  EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
138  DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);
139 
140  // Check derivatives of computeError
141  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
142  NavState x1, x2 = actual.predict(x1, bias);
143 
144  {
145  Matrix9 aH1, aH2;
146  Matrix96 aH3;
147  actual.computeError(x1, x2, bias, aH1, aH2, aH3);
148  std::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
149  std::bind(&PreintegrationBase::computeError, actual,
150  std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
151  nullptr, nullptr, nullptr);
155  }
156 
157  // Integrate again
158  Vector3 expectedDeltaR2(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
159  Vector3 expectedDeltaP2(0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0);
160  Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) +
161  Rot3::Expmap(expectedDeltaR1) * measuredAcc * 0.5;
162 
163  // Actual pre-integrated values
165  EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij()));
166  EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij()));
167  EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
168  DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
169 }
170 /* ************************************************************************* */
171 // Common linearization point and measurements for tests
172 namespace common {
173 static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
174  Point3(5.0, 1.0, 0));
175 static const Vector3 v1(Vector3(0.5, 0.0, 0.0));
176 static const NavState state1(x1, v1);
177 
178 // Measurements
179 static const double w = M_PI / 100;
180 static const Vector3 measuredOmega(w, 0, 0);
182  -kGravityAlongNavZDown);
183 static const double deltaT = 1.0;
184 
185 static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0),
186  Point3(5.5, 1.0, 0));
187 static const Vector3 v2(Vector3(0.5, 0.0, 0.0));
188 static const NavState state2(x2, v2);
189 } // namespace common
190 
191 /* ************************************************************************* */
192 TEST(ImuFactor, PreintegrationBaseMethods) {
193  using namespace common;
194  auto p = testing::Params();
195  p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
196  p->use2ndOrderCoriolis = true;
197 
198  PreintegratedImuMeasurements pim(p, kZeroBiasHat);
201 
202  // biasCorrectedDelta
203  Matrix96 actualH;
204  pim.biasCorrectedDelta(kZeroBias, actualH);
205  Matrix expectedH = numericalDerivative11<Vector9, Bias>(
206  std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
207  std::placeholders::_1, nullptr), kZeroBias);
208  EXPECT(assert_equal(expectedH, actualH));
209 
210  Matrix9 aH1;
211  Matrix96 aH2;
212  NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
213  Matrix eH1 = numericalDerivative11<NavState, NavState>(
214  std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1,
215  kZeroBias, nullptr, nullptr), state1);
216  EXPECT(assert_equal(eH1, aH1));
217  Matrix eH2 = numericalDerivative11<NavState, Bias>(
218  std::bind(&PreintegrationBase::predict, pim, state1,
219  std::placeholders::_1, nullptr, nullptr), kZeroBias);
220  EXPECT(assert_equal(eH2, aH2));
221 }
222 
223 /* ************************************************************************* */
224 TEST(ImuFactor, MultipleMeasurements) {
225  using namespace common;
226 
228  expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
229  expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
230 
231  Matrix32 acc,gyro;
232  Matrix12 dts;
233  acc << measuredAcc, measuredAcc;
234  gyro << measuredOmega, measuredOmega;
235  dts << deltaT, deltaT;
236  PreintegratedImuMeasurements actual(testing::Params(), kZeroBiasHat);
237  actual.integrateMeasurements(acc,gyro,dts);
238 
239  EXPECT(assert_equal(expected,actual));
240 }
241 
242 /* ************************************************************************* */
243 TEST(ImuFactor, ErrorAndJacobians) {
244  using namespace common;
246 
248  EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias)));
249 
250  // Create factor
251  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
252 
253  // Expected error
254  Vector expectedError(9);
255  expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
256  EXPECT(
257  assert_equal(expectedError,
258  factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
259 
260  Values values;
261  values.insert(X(1), x1);
262  values.insert(V(1), v1);
263  values.insert(X(2), x2);
264  values.insert(V(2), v2);
265  values.insert(B(1), kZeroBias);
266  EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
267 
268  // Make sure linearization is correct
269  double diffDelta = 1e-7;
270  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
271 
272  // Actual Jacobians
273  Matrix H1a, H2a, H3a, H4a, H5a;
274  (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a,
275  H5a);
276 
277  // Make sure rotation part is correct when error is interpreted as axis-angle
278  // Jacobians are around zero, so the rotation part is the same as:
279  Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
280  std::bind(&evaluateRotationError, factor, std::placeholders::_1, v1, x2, v2, kZeroBias),
281  x1);
282  EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
283 
284  Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
285  std::bind(&evaluateRotationError, factor, x1, v1, std::placeholders::_1, v2, kZeroBias),
286  x2);
287  EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
288 
289  // Evaluate error with wrong values
290  Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
291  values.update(V(2), v2_wrong);
292  expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901;
293  EXPECT(
294  assert_equal(expectedError,
295  factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2));
296  EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2));
297 
298  // Make sure the whitening is done correctly
299  Matrix cov = pim.preintMeasCov();
300  Matrix R = RtR(cov.inverse());
301  Vector whitened = R * expectedError;
302  EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-4));
303 
304  // Make sure linearization is correct
305  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
306 }
307 
308 /* ************************************************************************* */
309 TEST(ImuFactor, ErrorAndJacobianWithBiases) {
310  using common::x1;
311  using common::v1;
312  using common::v2;
313  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
314  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
315  Point3(5.5, 1.0, -50.0));
316 
317  // Measurements
319  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
320  Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
321  + Vector3(0.2, 0.0, 0.0);
322  double deltaT = 1.0;
323 
324  auto p = testing::Params();
325  p->omegaCoriolis = kNonZeroOmegaCoriolis;
326 
327  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
328  PreintegratedImuMeasurements pim(p, biasHat);
330 
331  // Make sure of biasCorrectedDelta
332  Matrix96 actualH;
333  pim.biasCorrectedDelta(bias, actualH);
334  Matrix expectedH = numericalDerivative11<Vector9, Bias>(
335  std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
336  std::placeholders::_1, nullptr), bias);
337  EXPECT(assert_equal(expectedH, actualH));
338 
339  // Create factor
340  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
341 
342  Values values;
343  values.insert(X(1), x1);
344  values.insert(V(1), v1);
345  values.insert(X(2), x2);
346  values.insert(V(2), v2);
347  values.insert(B(1), bias);
348 
349  // Make sure linearization is correct
350  double diffDelta = 1e-7;
351  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
352 }
353 
354 /* ************************************************************************* */
355 TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
356  using common::x1;
357  using common::v1;
358  using common::v2;
359  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
360  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
361  Point3(5.5, 1.0, -50.0));
362 
363  // Measurements
365  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
366  Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
367  + Vector3(0.2, 0.0, 0.0);
368  double deltaT = 1.0;
369 
370  auto p = testing::Params();
371  p->omegaCoriolis = kNonZeroOmegaCoriolis;
372  p->use2ndOrderCoriolis = true;
373 
375  Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)));
377 
378  // Create factor
379  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
380 
381  Values values;
382  values.insert(X(1), x1);
383  values.insert(V(1), v1);
384  values.insert(X(2), x2);
385  values.insert(V(2), v2);
386  values.insert(B(1), bias);
387 
388  // Make sure linearization is correct
389  double diffDelta = 1e-7;
390  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
391 }
392 
393 /* ************************************************************************* */
394 TEST(ImuFactor, PartialDerivative_wrt_Bias) {
395  // Linearization point
396  Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
397 
398  // Measurements
399  Vector3 measuredOmega(0.1, 0, 0);
400  double deltaT = 0.5;
401 
402  auto evaluateRotation = [=](const Vector3 biasOmega) {
403  return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
404  };
405 
406  // Compute numerical derivatives
407  Matrix expectedDelRdelBiasOmega =
408  numericalDerivative11<Rot3, Vector3>(evaluateRotation, biasOmega);
409 
410  const Matrix3 Jr =
411  Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
412 
413  Matrix3 actualDelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
414 
415  // Compare Jacobians
416  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9));
417 }
418 
419 /* ************************************************************************* */
420 TEST(ImuFactor, PartialDerivativeLogmap) {
421  // Linearization point
422  Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias
423 
424  // Measurements
425  Vector3 deltaTheta(0, 0, 0);
426 
427  auto evaluateLogRotation = [=](const Vector3 delta) {
428  return Rot3::Logmap(
429  Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
430  };
431 
432  // Compute numerical derivatives
433  Matrix expectedDelFdelTheta =
434  numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltaTheta);
435 
436  Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat);
437 
438  // Compare Jacobians
439  EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta));
440 }
441 
442 /* ************************************************************************* */
443 TEST(ImuFactor, fistOrderExponential) {
444  // Linearization point
445  Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
446 
447  // Measurements
448  Vector3 measuredOmega(0.1, 0, 0);
449  double deltaT = 1.0;
450 
451  // change w.r.t. linearization point
452  double alpha = 0.0;
453  Vector3 deltaBiasOmega;
454  deltaBiasOmega << alpha, alpha, alpha;
455 
456  const Matrix3 Jr = Rot3::ExpmapDerivative(
457  (measuredOmega - biasOmega) * deltaT);
458 
459  Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
460 
461  const Matrix expectedRot = Rot3::Expmap(
462  (measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix();
463 
464  const Matrix3 hatRot =
465  Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
466  const Matrix3 actualRot = hatRot
467  * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
468 
469  // This is a first order expansion so the equality is only an approximation
470  EXPECT(assert_equal(expectedRot, actualRot));
471 }
472 
473 /* ************************************************************************* */
475  const Vector3& measuredAcc, const Vector3& measuredOmega) {
477  Vector3 correctedOmega = pim.biasHat().correctGyroscope(measuredOmega);
478  return pim.correctMeasurementsBySensorPose(correctedAcc, correctedOmega).first;
479 }
480 
481 TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
482  const Rot3 nRb = Rot3::Expmap(Vector3(0, 0, M_PI / 4.0));
483  const Point3 p1(5.0, 1.0, -50.0);
484  const Vector3 v1(0.5, 0.0, 0.0);
485 
486  const Vector3 a = nRb * Vector3(0.2, 0.0, 0.0);
488  Vector3(0, 0, M_PI / 10.0 + 0.3));
489 
490  auto p = testing::Params();
491  p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)),
492  Point3(0.1, 0.05, 0.01));
493  p->omegaCoriolis = kNonZeroOmegaCoriolis;
494 
495  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
496 
497  const double T = 3.0; // seconds
498  ScenarioRunner runner(scenario, p, T / 10);
499 
500  // PreintegratedImuMeasurements pim = runner.integrate(T);
501  // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
502  //
503  // Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
504  // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
505  //
507  Pose3 x1(nRb, p1);
508 
509  // Measurements
512 
513  // Get mean prediction from "ground truth" measurements
514  const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
515  const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
516  PreintegratedImuMeasurements pim(p, biasHat);
517 
518  // Check updatedDeltaXij derivatives
519  Matrix3 D_correctedAcc_measuredOmega = Z_3x3;
521  nullptr, D_correctedAcc_measuredOmega, nullptr);
522  Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
523  std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1),
524  measuredOmega, 1e-6);
525  EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
526 
527  double dt = 0.1;
528 
529 // TODO(frank): revive derivative tests
530 // Matrix93 G1, G2;
531 // Vector9 preint =
532 // pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, {}, G1, G2);
533 //
534 // Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
535 // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
536 // std::placeholders::_1, std::placeholders::_2,
537 // dt, {}, {}, {}), measuredAcc,
538 // measuredOmega, 1e-6);
539 // EXPECT(assert_equal(expectedG1, G1, 1e-5));
540 //
541 // Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
542 // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
543 // std::placeholders::_1, std::placeholders::_2,
544 // dt, {}, {}, {}), measuredAcc,
545 // measuredOmega, 1e-6);
546 // EXPECT(assert_equal(expectedG2, G2, 1e-5));
547 
548  Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
549 
550  // integrate at least twice to get position information
551  // otherwise factor cov noise from preint_cov is not positive definite
554 
555  // Create factor
556  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
557 
558  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
559  Point3(5.5, 1.0, -50.0));
560  Vector3 v2(Vector3(0.5, 0.0, 0.0));
561 
562  Values values;
563  values.insert(X(1), x1);
564  values.insert(V(1), v1);
565  values.insert(X(2), x2);
566  values.insert(V(2), v2);
567  values.insert(B(1), bias);
568 
569  // Make sure linearization is correct
570  double diffDelta = 1e-8;
571  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
572 }
573 
574 /* ************************************************************************* */
575 TEST(ImuFactor, PredictPositionAndVelocity) {
576  gttic(PredictPositionAndVelocity);
577  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
578 
579  // Measurements
581  measuredOmega << 0, 0, 0; // M_PI/10.0+0.3;
583  measuredAcc << 0, 1, -kGravity;
584  double deltaT = 0.001;
585 
587  Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
588 
589  for (int i = 0; i < 1000; ++i)
591 
592  // Create factor
593  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
594 
595  // Predict
596  Pose3 x1;
597  Vector3 v1(0, 0.0, 0.0);
598  NavState actual = pim.predict(NavState(x1, v1), bias);
599  NavState expected(Rot3(), Point3(0, 0.5, 0), Vector3(0, 1, 0));
600  EXPECT(assert_equal(expected, actual));
601 }
602 
603 /* ************************************************************************* */
604 TEST(ImuFactor, PredictRotation) {
605  gttic(PredictRotation);
606  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
607 
608  // Measurements
610  measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3;
612  measuredAcc << 0, 0, -kGravity;
613  double deltaT = 0.001;
614 
616  Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
617 
618  for (int i = 0; i < 1000; ++i)
620 
621  // Create factor
622  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
623 
624  // Predict
625  NavState actual = pim.predict(NavState(), bias);
626  NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0,0,0), Z_3x1);
627  EXPECT(assert_equal(expected, actual));
628 }
629 
630 /* ************************************************************************* */
631 TEST(ImuFactor, PredictArbitrary) {
632  gttic(PredictArbitrary);
633  Pose3 x1;
634  const Vector3 v1(0, 0, 0);
635 
637  Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
638 
639  const double T = 3.0; // seconds
640  ScenarioRunner runner(scenario, testing::Params(), T / 10);
641  //
642  // PreintegratedImuMeasurements pim = runner.integrate(T);
643  // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
644  //
645  // Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
646  // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
648 
649  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
650 
651  // Measurements
654 
655  auto p = testing::Params();
656  p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
657  PreintegratedImuMeasurements pim(p, biasHat);
658  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
659 // EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, {}, measuredAcc, measuredOmega,
660 // Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
661 
662  double dt = 0.001;
663  for (int i = 0; i < 1000; ++i)
665 
666  // Create factor
667  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
668 
669  // Predict
670  NavState actual = pim.predict(NavState(x1, v1), bias);
671 
672  // Regression test for Imu Refactor
673  Rot3 expectedR( //
674  +0.903715275, -0.250741668, 0.347026393, //
675  +0.347026393, 0.903715275, -0.250741668, //
676  -0.250741668, 0.347026393, 0.903715275);
677  Point3 expectedT(-0.516077031, 0.57842919, 0.0876478403);
678  Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571);
679  NavState expected(expectedR, expectedT, expectedV);
680  EXPECT(assert_equal(expected, actual, 1e-7));
681 }
682 
683 /* ************************************************************************* */
684 TEST(ImuFactor, bodyPSensorNoBias) {
685  gttic(bodyPSensorNoBias);
686  Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
687 
688  // Rotate sensor (z-down) to body (same as navigation) i.e. z-up
689  auto p = testing::Params();
690  p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
691  p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));
692 
693  // Measurements
694  // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
695  Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10);
696  // Acc measurement is acceleration of sensor in the sensor frame, when stationary,
697  // table exerts an equal and opposite force w.r.t gravity
698  Vector3 s_accMeas(0, 0, -kGravity);
699  double dt = 0.001;
700 
702 
703  for (int i = 0; i < 1000; ++i)
704  pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt);
705 
706  // Create factor
707  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
708 
709  // Predict
710  NavState actual = pim.predict(NavState(), bias);
711 
712  Pose3 expectedPose(Rot3::Ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
713  EXPECT(assert_equal(expectedPose, actual.pose()));
714 
715  Vector3 expectedVelocity(0, 0, 0);
716  EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
717 }
718 
719 /* ************************************************************************* */
724 
725 TEST(ImuFactor, bodyPSensorWithBias) {
726  gttic(bodyPSensorWithBias);
727  using noiseModel::Diagonal;
728  typedef Bias Bias;
729 
730  int numPoses = 10;
731  Vector6 noiseBetweenBiasSigma;
732  noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
733  3.0e-6, 3.0e-6);
734  SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
735 
736  // Measurements
737  // Sensor frame is z-down
738  // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
739  Vector3 measuredOmega(0, 0.01, 0);
740  // Acc measurement is acceleration of sensor in the sensor frame, when stationary,
741  // table exerts an equal and opposite force w.r.t gravity
742  Vector3 measuredAcc(0, 0, -kGravity);
743 
744  auto p = testing::Params();
745  p->n_gravity = Vector3(0, 0, -kGravity);
746  p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0));
747  p->accelerometerCovariance = 1e-7 * I_3x3;
748  p->gyroscopeCovariance = 1e-8 * I_3x3;
749  p->integrationCovariance = 1e-9 * I_3x3;
750  double deltaT = 0.005;
751 
752  // Specify noise values on priors
753  Vector6 priorNoisePoseSigmas(
754  (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished());
755  Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished());
756  Vector6 priorNoiseBiasSigmas(
757  (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished());
758  SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
759  SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas);
760  SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
761  Vector3 zeroVel(0, 0, 0);
762 
763  // Create a factor graph with priors on initial pose, velocity and bias
765  Values values;
766 
767  graph.addPrior(X(0), Pose3(), priorNoisePose);
768  values.insert(X(0), Pose3());
769 
770  graph.addPrior(V(0), zeroVel, priorNoiseVel);
771  values.insert(V(0), zeroVel);
772 
773  // The key to this test is that we specify the bias, in the sensor frame, as known a priori
774  // We also create factors below that encode our assumption that this bias is constant over time
775  // In theory, after optimization, we should recover that same bias estimate
776  Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
777  graph.addPrior(B(0), priorBias, priorNoiseBias);
778  values.insert(B(0), priorBias);
779 
780  // Now add IMU factors and bias noise models
781  Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
782  for (int i = 1; i < numPoses; i++) {
783  PreintegratedImuMeasurements pim(p, priorBias);
784  for (int j = 0; j < 200; ++j)
786 
787  // Create factors
788  graph.emplace_shared<ImuFactor>(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim);
789  graph.emplace_shared<BetweenFactor<Bias> >(B(i - 1), B(i), zeroBias, biasNoiseModel);
790 
791  values.insert(X(i), Pose3());
792  values.insert(V(i), zeroVel);
793  values.insert(B(i), priorBias);
794  }
795 
796  // Finally, optimize, and get bias at last time step
798  Bias biasActual = result.at<Bias>(B(numPoses - 1));
799 
800  // And compare it with expected value (our prior)
801  Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
802  EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
803 }
804 
805 /* ************************************************************************* */
806 #ifdef GTSAM_TANGENT_PREINTEGRATION
807 static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
808 
809 struct ImuFactorMergeTest {
810  std::shared_ptr<PreintegrationParams> p_;
811  const ConstantTwistScenario forward_, loop_;
812 
813  ImuFactorMergeTest()
814  : p_(PreintegrationParams::MakeSharedU(kGravity)),
815  forward_(kZero, Vector3(kVelocity, 0, 0)),
816  loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
817  // arbitrary noise values
818  p_->gyroscopeCovariance = I_3x3 * 0.01;
819  p_->accelerometerCovariance = I_3x3 * 0.03;
820  }
821 
822  int TestScenario(TestResult& result_, const std::string& name_,
823  const Scenario& scenario,
824  const Bias& bias01,
825  const Bias& bias12, double tol) {
826  // Test merge by creating a 01, 12, and 02 PreintegratedRotation,
827  // then checking the merge of 01-12 matches 02.
828  PreintegratedImuMeasurements pim01(p_, bias01);
829  PreintegratedImuMeasurements pim12(p_, bias12);
830  PreintegratedImuMeasurements pim02_expected(p_, bias01);
831 
832  double deltaT = 0.01;
833  ScenarioRunner runner(scenario, p_, deltaT);
834  // TODO(frank) can this loop just go into runner ?
835  for (int i = 0; i < 100; i++) {
836  double t = i * deltaT;
837  // integrate the measurements appropriately
838  Vector3 accel_meas = runner.actualSpecificForce(t);
839  Vector3 omega_meas = runner.actualAngularVelocity(t);
840  pim02_expected.integrateMeasurement(accel_meas, omega_meas, deltaT);
841  if (i < 50) {
842  pim01.integrateMeasurement(accel_meas, omega_meas, deltaT);
843  } else {
844  pim12.integrateMeasurement(accel_meas, omega_meas, deltaT);
845  }
846  }
847  auto actual_pim02 = ImuFactor::Merge(pim01, pim12);
848  EXPECT(assert_equal(pim02_expected.preintegrated(),
849  actual_pim02.preintegrated(), tol));
850  EXPECT(assert_equal(pim02_expected, actual_pim02, tol));
851 
852  auto factor01 =
853  std::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
854  auto factor12 =
855  std::make_shared<ImuFactor>(X(1), V(1), X(2), V(2), B(0), pim12);
856  auto factor02_expected = std::make_shared<ImuFactor>(
857  X(0), V(0), X(2), V(2), B(0), pim02_expected);
858 
859  ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);
860  EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol));
861  return result_.getFailureCount();
862  }
863 
864  void TestScenarios(TestResult& result_, const std::string& name_,
865  const Bias& bias01,
866  const Bias& bias12, double tol) {
867  for (auto scenario : {forward_, loop_})
868  TestScenario(result_, name_, scenario, bias01, bias12, tol);
869  }
870 };
871 
872 /* ************************************************************************* */
873 // Test case with zero biases
874 TEST(ImuFactor, MergeZeroBias) {
875  ImuFactorMergeTest mergeTest;
876  mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
877 }
878 
879 // Test case with identical biases: we expect an exact answer.
880 TEST(ImuFactor, MergeConstantBias) {
881  ImuFactorMergeTest mergeTest;
882  Bias bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03));
883  mergeTest.TestScenarios(result_, name_, bias, bias, 1e-4);
884 }
885 
886 // Test case with different biases where we expect there to be some variation.
887 TEST(ImuFactor, MergeChangingBias) {
888  ImuFactorMergeTest mergeTest;
889  mergeTest.TestScenarios(result_, name_,
890  Bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03)),
891  Bias(Vector3(0.01, 0.02, 0.03), Vector3(0.03, -0.02, 0.01)), 1e-1);
892 }
893 
894 // Test case with non-zero coriolis
895 TEST(ImuFactor, MergeWithCoriolis) {
896  ImuFactorMergeTest mergeTest;
897  mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
898  mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
899 }
900 #endif
901 
902 /* ************************************************************************* */
903 // Same values as pre-integration test but now testing covariance
904 TEST(ImuFactor, CheckCovariance) {
905  gttic(CheckCovariance);
906  // Measurements
907  Vector3 measuredAcc(0.1, 0.0, 0.0);
908  Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
909  double deltaT = 0.5;
910 
914  expected << 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, 0, //
915  0, 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, //
916  0, 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, //
917  0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, 0, //
918  0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, //
919  0, 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, //
920  0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, 0, //
921  0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, //
922  0, 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06;
924 }
925 
926 /* ************************************************************************* */
927 int main() {
928  TestResult tr;
930 #ifdef ENABLE_TIMING
931  tictoc_print_();
932 #endif
933  return result;
934 }
935 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:292
testing
Definition: benchmark.h:20
kGyroSigma
static const double kGyroSigma
Definition: testScenarioRunner.cpp:32
gtsam::ScenarioRunner::estimateCovariance
Matrix9 estimateCovariance(double T, size_t N=1000, const Bias &estimatedBias=Bias()) const
Compute a Monte Carlo estimate of the predict covariance using N samples.
Definition: ScenarioRunner.cpp:55
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
EXPECT_CORRECT_FACTOR_JACOBIANS
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Definition: factorTesting.h:114
kAccelSigma
static const double kAccelSigma
Definition: testScenarioRunner.cpp:33
ScenarioRunner.h
Simple class to test navigation scenarios.
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:136
gtsam::PreintegratedImuMeasurements::integrateMeasurement
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Definition: ImuFactor.cpp:53
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
common::v1
static const Vector3 v1(Vector3(0.5, 0.0, 0.0))
TestHarness.h
kVelocity
static const Velocity3 kVelocity(0.4, 0.5, 0.6)
gtsam::PreintegrationParams
Definition: PreintegrationParams.h:25
gtsam::ScenarioRunner::integrate
PreintegratedImuMeasurements integrate(double T, const Bias &estimatedBias=Bias(), bool corrupted=false) const
Integrate measurements for T seconds into a PIM.
Definition: ScenarioRunner.cpp:30
gtsam::ScenarioRunner
Definition: ScenarioRunner.h:40
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::ManifoldPreintegration::deltaRij
Rot3 deltaRij() const override
Definition: ManifoldPreintegration.h:77
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:47
gtsam::HybridValues::update
HybridValues & update(const VectorValues &values)
Definition: HybridValues.cpp:135
common::w
static const double w
Definition: testImuFactor.cpp:179
gtsam::ImuFactor::shared_ptr
std::shared_ptr< ImuFactor > shared_ptr
Definition: ImuFactor.h:188
gtsam::ConstantTwistScenario
Definition: Scenario.h:60
gtsam::PreintegrationBase::correctMeasurementsBySensorPose
std::pair< Vector3, Vector3 > correctMeasurementsBySensorPose(const Vector3 &unbiasedAcc, const Vector3 &unbiasedOmega, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedAcc={}, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedOmega={}, OptionalJacobian< 3, 3 > correctedOmega_H_unbiasedOmega={}) const
Definition: PreintegrationBase.cpp:59
E1::B
@ B
gtsam::NavState
Definition: NavState.h:34
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
X
#define X
Definition: icosphere.cpp:20
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::imuBias::ConstantBias::correctAccelerometer
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: ImuBias.h:76
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NoiseModelFactorN::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Definition: NonlinearFactor.h:606
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:135
screwPose2::expectedR
Rot2 expectedR
Definition: testPose2.cpp:149
common::x1
static const Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, 0))
TestableAssertions.h
Provides additional testing facilities for common data structures.
TestResult::getFailureCount
int getFailureCount()
Definition: TestResult.h:35
gtsam::ScenarioRunner::actualAngularVelocity
Vector3 actualAngularVelocity(double t) const
Definition: ScenarioRunner.h:73
common::deltaT
static const double deltaT
Definition: testImuFactor.cpp:183
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
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:152
common::v2
static const Vector3 v2(Vector3(0.5, 0.0, 0.0))
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
BetweenFactor.h
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:259
gtsam::Pose3
Definition: Pose3.h:37
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::PreintegratedImuMeasurements::preintMeasCov
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition: ImuFactor.h:137
common::measuredOmega
static const Vector3 measuredOmega(w, 0, 0)
gtsam::PreintegratedImuMeasurements
Definition: ImuFactor.h:68
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::RtR
Matrix RtR(const Matrix &A)
Definition: Matrix.cpp:528
gtsam::PreintegrationBase::predict
NavState predict(const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 6 > H2={}) const
Predict state at time j.
Definition: PreintegrationBase.cpp:115
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::ManifoldPreintegration::deltaVij
Vector3 deltaVij() const override
Definition: ManifoldPreintegration.h:79
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:226
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::ScenarioRunner::predict
NavState predict(const PreintegratedImuMeasurements &pim, const Bias &estimatedBias=Bias()) const
Predict predict given a PIM.
Definition: ScenarioRunner.cpp:49
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
kGravity
const double kGravity
Definition: ImuFactorsExample2.cpp:36
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
forward::scenario
ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0))
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::ImuFactor
Definition: ImuFactor.h:169
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
Eigen::Triplet< double >
common::state2
static const NavState state2(x2, v2)
common::x2
static const Pose3 x2(Rot3::RzRyRx(M_PI/12.0+w, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, 0))
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
TestResult
Definition: TestResult.h:26
gtsam::AcceleratingScenario
Accelerating from an arbitrary initial state, with optional rotation.
Definition: Scenario.h:83
imuFactorTesting.h
Common testing infrastructure.
main
int main()
Definition: testImuFactor.cpp:927
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
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:141
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
Bias
imuBias::ConstantBias Bias
Definition: testImuBias.cpp:27
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
CHECK
#define CHECK(condition)
Definition: Test.h:108
p
float * p
Definition: Tutorial_Map_using.cpp:9
kZero
static const Vector kZero
Definition: testLPSolver.cpp:37
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
gtsam::PreintegratedImuMeasurements::integrateMeasurements
void integrateMeasurements(const Matrix &measuredAccs, const Matrix &measuredOmegas, const Matrix &dts)
Add multiple measurements, in matrix columns.
Definition: ImuFactor.cpp:87
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::NavState::velocity
const Velocity3 & velocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:62
gtsam::ScenarioRunner::actualSpecificForce
Vector3 actualSpecificForce(double t) const
Definition: ScenarioRunner.h:76
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::PreintegrationBase::biasHat
const imuBias::ConstantBias & biasHat() const
Definition: PreintegrationBase.h:104
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
Eigen::Matrix< double, 9, 9 >
gtsam::ManifoldPreintegration::deltaPij
Vector3 deltaPij() const override
Definition: ManifoldPreintegration.h:78
gtsam::imuBias::ConstantBias::correctGyroscope
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: ImuBias.h:85
gtsam::PreintegrationBase::resetIntegrationAndSetBias
void resetIntegrationAndSetBias(const Bias &biasHat)
Definition: PreintegrationBase.cpp:53
Sampler.h
sampling from a NoiseModel
M_PI
#define M_PI
Definition: mconf.h:117
correctedAcc
Vector3 correctedAcc(const PreintegratedImuMeasurements &pim, const Vector3 &measuredAcc, const Vector3 &measuredOmega)
Definition: testImuFactor.cpp:474
screwPose2::expectedT
Point2 expectedT(-0.0446635, 0.29552)
common::state1
static const NavState state1(x1, v1)
gtsam::ManifoldPreintegration::biasCorrectedDelta
Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
Definition: ManifoldPreintegration.cpp:112
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:86
align_3::t
Point2 t(10, 10)
testing::Params
static std::shared_ptr< PreintegratedCombinedMeasurements::Params > Params(const Matrix3 &biasAccCovariance=Matrix3::Zero(), const Matrix3 &biasOmegaCovariance=Matrix3::Zero(), const Matrix6 &biasAccOmegaInt=Matrix6::Zero())
Definition: testCombinedImuFactor.cpp:39
gtsam::PreintegrationBase::deltaTij
double deltaTij() const
Definition: PreintegrationBase.h:105
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
TEST
TEST(ImuFactor, PreintegratedMeasurementsConstruction)
Definition: testImuFactor.cpp:62
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::Scenario
Simple trajectory simulator.
Definition: Scenario.h:25
Pose3.h
3D Pose
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
ImuFactor.h
common
Definition: testImuFactor.cpp:172
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:14