testTangentPreintegration.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 
18 #include <gtsam/navigation/TangentPreintegration.h>
19 #include <gtsam/base/numericalDerivative.h>
20 #include <gtsam/nonlinear/expressions.h>
21 #include <gtsam/nonlinear/ExpressionFactor.h>
22 #include <gtsam/nonlinear/expressionTesting.h>
23 
24 #include <CppUnitLite/TestHarness.h>
25 
26 #include "imuFactorTesting.h"
27 
28 using namespace std::placeholders;
29 
30 static const double kDt = 0.1;
31 
32 Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
33  return TangentPreintegration::UpdatePreintegrated(a, w, kDt, zeta);
34 }
35 
36 namespace testing {
37 // Create default parameters with Z-down and above noise parameters
38 static std::shared_ptr<PreintegrationParams> Params() {
39  auto p = PreintegrationParams::MakeSharedD(kGravity);
40  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
41  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
42  p->integrationCovariance = 0.0001 * I_3x3;
43  return p;
44 }
45 }
46 
47 /* ************************************************************************* */
48 TEST(TangentPreintegration, UpdateEstimate1) {
49  TangentPreintegration pim(testing::Params());
50  const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
51  Vector9 zeta;
52  zeta.setZero();
53  Matrix9 aH1;
54  Matrix93 aH2, aH3;
55  pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
56  EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9));
57  EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9));
58  EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
59 }
60 
61 /* ************************************************************************* */
62 TEST(TangentPreintegration, UpdateEstimate2) {
63  TangentPreintegration pim(testing::Params());
64  const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
65  Vector9 zeta;
66  zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
67  Matrix9 aH1;
68  Matrix93 aH2, aH3;
69  pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
70  // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
71  EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
72  EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));
73  EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
74 }
75 
76 /* ************************************************************************* */
77 TEST(ImuFactor, BiasCorrectionJacobians) {
78  testing::SomeMeasurements measurements;
79 
80  std::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
81  [=](const Vector3& a, const Vector3& w) {
82  TangentPreintegration pim(testing::Params(), Bias(a, w));
83  testing::integrateMeasurements(measurements, &pim);
84  return pim.preintegrated();
85  };
86 
87  // Actual pre-integrated values
88  TangentPreintegration pim(testing::Params());
89  testing::integrateMeasurements(measurements, &pim);
90 
91  EXPECT(
92  assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
93  pim.preintegrated_H_biasAcc()));
94  EXPECT(
95  assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
96  pim.preintegrated_H_biasOmega(), 1e-3));
97 }
98 
99 /* ************************************************************************* */
100 TEST(TangentPreintegration, computeError) {
101  TangentPreintegration pim(testing::Params());
102  NavState x1, x2;
103  imuBias::ConstantBias bias;
104  Matrix9 aH1, aH2;
105  Matrix96 aH3;
106  pim.computeError(x1, x2, bias, aH1, aH2, aH3);
107  std::function<Vector9(const NavState&, const NavState&,
108  const imuBias::ConstantBias&)>
109  f = std::bind(&TangentPreintegration::computeError, pim,
110  std::placeholders::_1, std::placeholders::_2,
111  std::placeholders::_3, nullptr, nullptr,
112  nullptr);
113  // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
114  EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
115  EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
116  EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
117 }
118 
119 /* ************************************************************************* */
120 TEST(TangentPreintegration, Compose) {
121  testing::SomeMeasurements measurements;
122  TangentPreintegration pim(testing::Params());
123  testing::integrateMeasurements(measurements, &pim);
124 
125  std::function<Vector9(const Vector9&, const Vector9&)> f =
126  [pim](const Vector9& zeta01, const Vector9& zeta12) {
127  return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
128  };
129 
130  // Expected merge result
131  TangentPreintegration expected_pim02(testing::Params());
132  testing::integrateMeasurements(measurements, &expected_pim02);
133  testing::integrateMeasurements(measurements, &expected_pim02);
134 
135  // Actual result
136  Matrix9 H1, H2;
137  TangentPreintegration actual_pim02 = pim;
138  actual_pim02.mergeWith(pim, &H1, &H2);
139 
140  const Vector9 zeta = pim.preintegrated();
141  const Vector9 actual_zeta =
142  TangentPreintegration::Compose(zeta, zeta, pim.deltaTij());
143  EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7));
144  EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7));
145  EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7));
146 }
147 
148 /* ************************************************************************* */
149 TEST(TangentPreintegration, MergedBiasDerivatives) {
150  testing::SomeMeasurements measurements;
151 
152  auto f = [=](const Vector3& a, const Vector3& w) {
153  TangentPreintegration pim02(testing::Params(), Bias(a, w));
154  testing::integrateMeasurements(measurements, &pim02);
155  testing::integrateMeasurements(measurements, &pim02);
156  return pim02.preintegrated();
157  };
158 
159  // Expected merge result
160  TangentPreintegration expected_pim02(testing::Params());
161  testing::integrateMeasurements(measurements, &expected_pim02);
162  testing::integrateMeasurements(measurements, &expected_pim02);
163 
164  EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1),
165  expected_pim02.preintegrated_H_biasAcc()));
166  EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1),
167  expected_pim02.preintegrated_H_biasOmega(), 1e-7));
168 }
169 
170 /* ************************************************************************* */
171 int main() {
172  TestResult tr;
173  return TestRegistry::runAllTests(tr);
174 }
175 /* ************************************************************************* */
kGravity
const double kGravity
Definition: ImuFactorsExample2.cpp:36
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
testing::Params
static std::shared_ptr< PreintegrationParams > Params()
Definition: testTangentPreintegration.cpp:38
TEST
TEST(TangentPreintegration, UpdateEstimate1)
Definition: testTangentPreintegration.cpp:48
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
testing::integrateMeasurements
void integrateMeasurements(const vector< ImuMeasurement > &measurements, PIM *pim)
Definition: imuFactorTesting.h:59
numericalDerivative.h
Some functions to compute numerical derivatives.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
imuFactorTesting.h
Common testing infrastructure.
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
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
zeta
const CwiseBinaryOp< internal::scalar_zeta_op< Scalar >, const Derived, const DerivedQ > zeta(const EIGEN_CURRENT_STORAGE_BASE_CLASS< DerivedQ > &q) const
Definition: ArrayCwiseBinaryOps.h:355
kAccelSigma
static const double kAccelSigma
Definition: testScenarioRunner.cpp:33
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
expressionTesting.h
Test harness methods for expressions.
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
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
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
Eigen::internal::omega
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
kZero
static const Vector kZero
Definition: testLPSolver.cpp:37
gtsam::TangentPreintegration::mergeWith
void mergeWith(const TangentPreintegration &pim, Matrix9 *H1, Matrix9 *H2)
Definition: TangentPreintegration.cpp:216
Bias
imuBias::ConstantBias Bias
Definition: testImuBias.cpp:27
gtsam::TangentPreintegration::UpdatePreintegrated
static Vector9 UpdatePreintegrated(const Vector3 &a_body, const Vector3 &w_body, const double dt, const Vector9 &preintegrated, OptionalJacobian< 9, 9 > A={}, OptionalJacobian< 9, 3 > B={}, OptionalJacobian< 9, 3 > C={})
Definition: TangentPreintegration.cpp:54
x1
Pose3 x1
Definition: testPose3.cpp:663
p
float * p
Definition: Tutorial_Map_using.cpp:9
kDt
static const double kDt
Definition: testTangentPreintegration.cpp:30
gtsam::TangentPreintegration::preintegrated_H_biasOmega
const Matrix93 & preintegrated_H_biasOmega() const
Definition: TangentPreintegration.h:79
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
gtsam::TangentPreintegration::preintegrated_H_biasAcc
const Matrix93 & preintegrated_H_biasAcc() const
Definition: TangentPreintegration.h:78
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
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
f
Vector9 f(const Vector9 &zeta, const Vector3 &a, const Vector3 &w)
Definition: testTangentPreintegration.cpp:32
kGyroSigma
static const double kGyroSigma
Definition: testScenarioRunner.cpp:32
gtsam::TangentPreintegration::preintegrated
const Vector9 & preintegrated() const
Definition: TangentPreintegration.h:76

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:44