12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix and comment another test

release/4.3a0
AndreMichelin 2024-08-14 17:53:18 -07:00
parent ac96b59469
commit 6412517d3f
1 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -118,17 +118,18 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
// Integrate a single measurement // Integrate a single measurement
const double omega = 0.1; const double omega = 0.1;
const Vector3 trueOmega(omega, 0, 0); const Vector3 trueOmega(omega, 0, 0);
const Vector3 bias(1, 2, 3); const Vector3 biasOmega(1, 2, 3);
const Vector3 measuredOmega = trueOmega + bias; const Vector3 measuredOmega = trueOmega + biasOmega;
const double deltaT = 0.5; const double deltaT = 0.5;
// Check the accumulated rotation. // Check the accumulated rotation.
Rot3 expected = Rot3::Roll(omega * deltaT); Rot3 expected = Rot3::Roll(omega * deltaT);
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); const Vector biasOmegaHat = biasOmega;
pim.integrateGyroMeasurement(measuredOmega, biasOmegaHat, deltaT);
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
// Now do the same for a ManifoldPreintegration object // Now do the same for a ManifoldPreintegration object
imuBias::ConstantBias biasHat {Z_3x1, bias}; imuBias::ConstantBias biasHat {Z_3x1, biasOmega};
ManifoldPreintegration manifoldPim(testing::Params(), biasHat); ManifoldPreintegration manifoldPim(testing::Params(), biasHat);
manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT); manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT);
EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9)); EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9));
@ -136,17 +137,21 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
// Check their internal Jacobians are the same: // Check their internal Jacobians are the same:
EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega())); EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega()));
// Check PreintegratedRotation::biascorrectedDeltaRij. // Let's check the derivatives a delta away from the bias hat
Matrix3 H;
const double delta = 0.05; const double delta = 0.05;
const Vector3 biasOmegaIncr(delta, 0, 0); const Vector3 biasOmegaIncr(delta, 0, 0);
imuBias::ConstantBias bias_i {Z_3x1, biasOmegaHat + biasOmegaIncr};
// Check PreintegratedRotation::biascorrectedDeltaRij.
// Note that biascorrectedDeltaRij expects the biasHat to be subtracted already
Matrix3 H;
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT); const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT);
EXPECT(assert_equal(expected2, corrected, 1e-9)); EXPECT(assert_equal(expected2, corrected, 1e-9));
// Check ManifoldPreintegration::biasCorrectedDelta. // Check ManifoldPreintegration::biasCorrectedDelta.
imuBias::ConstantBias bias_i {Z_3x1, bias + biasOmegaIncr}; // Note that, confusingly, biasCorrectedDelta will subtract biasHat inside
Matrix96 H2; Matrix96 H2;
Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2); Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2);
Vector9 expected3; Vector9 expected3;
@ -154,12 +159,11 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
EXPECT(assert_equal(expected3, biasCorrected, 1e-9)); EXPECT(assert_equal(expected3, biasCorrected, 1e-9));
// Check that this one is sane: // Check that this one is sane:
auto g = [&](const Vector3& increment) { auto g = [&](const Vector3& b) {
return manifoldPim.biasCorrectedDelta({Z_3x1, bias + increment}, {}); return manifoldPim.biasCorrectedDelta({Z_3x1, b}, {});
}; };
EXPECT(assert_equal<Matrix>(numericalDerivative11<Vector9, Vector3>(g, Z_3x1), EXPECT(assert_equal<Matrix>(numericalDerivative11<Vector9, Vector3>(g, bias_i.gyroscope()),
H2.rightCols<3>(), H2.rightCols<3>()));
1e-4)); // NOTE(frank): reduced tolerance
} }
/* ************************************************************************* */ /* ************************************************************************* */