testManifoldPreintegration.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 
25 
26 #include "imuFactorTesting.h"
27 
28 using namespace std::placeholders;
29 
30 namespace testing {
31 // Create default parameters with Z-down and above noise parameters
32 static std::shared_ptr<PreintegrationParams> Params() {
33  auto p = PreintegrationParams::MakeSharedD(kGravity);
34  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
35  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
36  p->integrationCovariance = 0.0001 * I_3x3;
37  return p;
38 }
39 }
40 
41 /* ************************************************************************* */
42 TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
44 
45  std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
46  [=](const Vector3& a, const Vector3& w) {
49  return pim.deltaRij();
50  };
51 
52  std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
53  [=](const Vector3& a, const Vector3& w) {
56  return pim.deltaPij();
57  };
58 
59  std::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
60  [=](const Vector3& a, const Vector3& w) {
63  return pim.deltaVij();
64  };
65 
66  // Actual pre-integrated values
69 
70  EXPECT(
72  Matrix3(Z_3x3)));
73  EXPECT(
75  pim.delRdelBiasOmega(), 1e-3));
76 
77  EXPECT(
79  pim.delPdelBiasAcc()));
80  EXPECT(
82  pim.delPdelBiasOmega(), 1e-3));
83 
84  EXPECT(
86  pim.delVdelBiasAcc()));
87  EXPECT(
89  pim.delVdelBiasOmega(), 1e-3));
90 }
91 
92 /* ************************************************************************* */
93 TEST(ManifoldPreintegration, computeError) {
95  NavState x1, x2;
97  Matrix9 aH1, aH2;
98  Matrix96 aH3;
99  pim.computeError(x1, x2, bias, aH1, aH2, aH3);
100  std::function<Vector9(const NavState&, const NavState&,
101  const imuBias::ConstantBias&)>
102  f = std::bind(&ManifoldPreintegration::computeError, pim,
103  std::placeholders::_1, std::placeholders::_2,
104  std::placeholders::_3, nullptr, nullptr,
105  nullptr);
106  // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
110 }
111 
112 /* ************************************************************************* */
113 TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
114  // Create a PreintegratedRotation object
115  auto p = std::make_shared<PreintegratedRotationParams>();
117 
118  // Integrate a single measurement
119  const double omega = 0.1;
120  const Vector3 trueOmega(omega, 0, 0);
121  const Vector3 biasOmega(1, 2, 3);
122  const Vector3 measuredOmega = trueOmega + biasOmega;
123  const double deltaT = 0.5;
124 
125  // Check the accumulated rotation.
126  Rot3 expected = Rot3::Roll(omega * deltaT);
127  const Vector biasOmegaHat = biasOmega;
128  pim.integrateGyroMeasurement(measuredOmega, biasOmegaHat, deltaT);
129  EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
130 
131  // Now do the same for a ManifoldPreintegration object
132  imuBias::ConstantBias biasHat {Z_3x1, biasOmega};
133  ManifoldPreintegration manifoldPim(testing::Params(), biasHat);
135  EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9));
136 
137  // Check their internal Jacobians are the same:
138  EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega()));
139 
140  // Let's check the derivatives a delta away from the bias hat
141  const double delta = 0.05;
142  const Vector3 biasOmegaIncr(delta, 0, 0);
143  imuBias::ConstantBias bias_i {Z_3x1, biasOmegaHat + biasOmegaIncr};
144 
145  // Check PreintegratedRotation::biascorrectedDeltaRij.
146  // Note that biascorrectedDeltaRij expects the biasHat to be subtracted already
147  Matrix3 H;
148  Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
149  EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
150  const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT);
151  EXPECT(assert_equal(expected2, corrected, 1e-9));
152 
153  // Check ManifoldPreintegration::biasCorrectedDelta.
154  // Note that, confusingly, biasCorrectedDelta will subtract biasHat inside
155  Matrix96 H2;
156  Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2);
157  Vector9 expected3;
158  expected3 << Rot3::Logmap(expected2), Z_3x1, Z_3x1;
159  EXPECT(assert_equal(expected3, biasCorrected, 1e-9));
160 
161  // Check that this one is sane:
162  auto g = [&](const Vector3& b) {
163  return manifoldPim.biasCorrectedDelta({Z_3x1, b}, {});
164  };
165  EXPECT(assert_equal<Matrix>(numericalDerivative11<Vector9, Vector3>(g, bias_i.gyroscope()),
166  H2.rightCols<3>()));
167 }
168 
169 /* ************************************************************************* */
170 int main() {
171  TestResult tr;
172  return TestRegistry::runAllTests(tr);
173 }
174 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam::PreintegrationBase::integrateMeasurement
virtual void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt)
Version without derivatives.
Definition: PreintegrationBase.cpp:107
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
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
testing
Definition: benchmark.h:20
kGyroSigma
static const double kGyroSigma
Definition: testScenarioRunner.cpp:32
gtsam::PreintegratedRotation::delRdelBiasOmega
const Matrix3 & delRdelBiasOmega() const
Definition: PreintegratedRotation.h:166
ManifoldPreintegration.h
kAccelSigma
static const double kAccelSigma
Definition: testScenarioRunner.cpp:33
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:47
TestHarness.h
b
Scalar * b
Definition: benchVecAdd.cpp:17
main
int main()
Definition: testManifoldPreintegration.cpp:170
gtsam::PreintegratedRotation::integrateGyroMeasurement
void integrateGyroMeasurement(const Vector3 &measuredOmega, const Vector3 &biasHat, double deltaT, OptionalJacobian< 3, 3 > F={})
Calculate an incremental rotation given the gyro measurement and a time interval, and update both del...
Definition: PreintegratedRotation.cpp:97
gtsam::PreintegratedRotation::deltaRij
const Rot3 & deltaRij() const
Definition: PreintegratedRotation.h:165
gtsam::ManifoldPreintegration::delVdelBiasAcc
Matrix3 delVdelBiasAcc() const
Definition: ManifoldPreintegration.h:84
gtsam::ManifoldPreintegration::deltaRij
Rot3 deltaRij() const override
Definition: ManifoldPreintegration.h:77
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
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::NavState
Definition: NavState.h:38
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
testing::integrateMeasurements
void integrateMeasurements(const vector< ImuMeasurement > &measurements, PIM *pim)
Definition: imuFactorTesting.h:59
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
EQUALITY
#define EQUALITY(expected, actual)
Definition: Test.h:127
testing::Params
static std::shared_ptr< PreintegrationParams > Params()
Definition: testManifoldPreintegration.cpp:32
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
biased_x_rotation::trueOmega
const Vector3 trueOmega(omega, 0, 0)
common::deltaT
static const double deltaT
Definition: testImuFactor.cpp:183
numericalDerivative.h
Some functions to compute numerical derivatives.
expressions.h
Common expressions, both linear and non-linear.
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
x1
Pose3 x1
Definition: testPose3.cpp:663
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::ManifoldPreintegration::delPdelBiasOmega
Matrix3 delPdelBiasOmega() const
Definition: ManifoldPreintegration.h:83
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
kGravity
const double kGravity
Definition: ImuFactorsExample2.cpp:36
gtsam::ManifoldPreintegration::delRdelBiasOmega
Matrix3 delRdelBiasOmega() const
Definition: ManifoldPreintegration.h:81
gtsam::ManifoldPreintegration::delVdelBiasOmega
Matrix3 delVdelBiasOmega() const
Definition: ManifoldPreintegration.h:85
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::ManifoldPreintegration::delPdelBiasAcc
Matrix3 delPdelBiasAcc() const
Definition: ManifoldPreintegration.h:82
TestResult
Definition: TestResult.h:26
expressionTesting.h
Test harness methods for expressions.
imuFactorTesting.h
Common testing infrastructure.
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
TEST
TEST(ManifoldPreintegration, BiasCorrectionJacobians)
Definition: testManifoldPreintegration.cpp:42
gtsam::ManifoldPreintegration
Definition: ManifoldPreintegration.h:33
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
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:143
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
Bias
imuBias::ConstantBias Bias
Definition: testImuBias.cpp:27
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
gtsam::PreintegratedRotation
Definition: PreintegratedRotation.h:117
p
float * p
Definition: Tutorial_Map_using.cpp:9
kZero
static const Vector kZero
Definition: testLPSolver.cpp:37
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
ExpressionFactor.h
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix< double, 9, 9 >
gtsam::ManifoldPreintegration::deltaPij
Vector3 deltaPij() const override
Definition: ManifoldPreintegration.h:78
gtsam::ManifoldPreintegration::biasCorrectedDelta
Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
Definition: ManifoldPreintegration.cpp:112
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
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
testing::SomeMeasurements
Definition: imuFactorTesting.h:65
gtsam::PreintegratedRotation::biascorrectedDeltaRij
Rot3 biascorrectedDeltaRij(const Vector3 &biasOmegaIncr, OptionalJacobian< 3, 3 > H={}) const
Return a bias corrected version of the integrated rotation.
Definition: PreintegratedRotation.cpp:130


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:16:46