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

Adding more tests

release/4.3a0
AndreMichelin 2024-08-12 19:55:20 -07:00
parent c2145bdffc
commit 37696b274e
1 changed files with 74 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
gtsam/navigation/tests/testPreintegratedRotation.cpp
View File

@ -74,11 +74,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) {
EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected))
EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9))
// TODO(frank): again the derivative is not the *sane* one!
// auto g = [&](const Vector3& increment) {
// return pim.biascorrectedDeltaRij(increment, {});
// };
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
auto g = [&](const Vector3& increment) {
return pim.biascorrectedDeltaRij(increment, {});
};
const Matrix3 sane = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal(sane, pim.delRdelBiasOmega()));
// TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one!
// EXPECT(assert_equal(sane, H));
// Let's integrate a second IMU measurement and check the Jacobian update:
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
const Matrix3 sane2 = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal(sane2, pim.delRdelBiasOmega()));
}
//******************************************************************************
@ -124,11 +131,68 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected))
EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9))
// TODO(frank): again the derivative is not the *sane* one!
// auto g = [&](const Vector3& increment) {
// return pim.biascorrectedDeltaRij(increment, {});
// };
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
auto g = [&](const Vector3& increment) {
return pim.biascorrectedDeltaRij(increment, {});
};
const Matrix3 sane = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal(sane, pim.delRdelBiasOmega()));
// TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one!
// EXPECT(assert_equal(sane, H));
// Let's integrate a second IMU measurement and check the Jacobian update:
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
const Matrix3 sane2 = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal(sane2, pim.delRdelBiasOmega()));
}
// Create params we have a non-axis-aligned rotation and even an offset.
static std::shared_ptr<PreintegratedRotationParams> paramsWithArbitraryTransform() {
auto p = std::make_shared<PreintegratedRotationParams>();
p->setBodyPSensor({Rot3::Expmap({1,2,3}), {4,5,6}});
return p;
}
TEST(PreintegratedRotation, integrateGyroMeasurementWithArbitraryTransform) {
// Example with a non-axis-aligned transform and some position.
using namespace biased_x_rotation;
auto p = paramsWithArbitraryTransform();
// Check the derivative:
Matrix3 H_bias;
const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
f(bias, H_bias);
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias))
// Check derivative of deltaRij() after integration.
Matrix3 F;
PreintegratedRotation pim(p);
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
// Check that system matrix F is the first derivative of compose:
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F))
// Make sure delRdelBiasOmega is H_bias after integration.
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()))
// Check the bias correction in same way, but will now yield pitch change.
Matrix3 H;
const double delta = 0.05;
const Vector3 biasOmegaIncr(delta, 0, 0);
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
auto g = [&](const Vector3& increment) {
return pim.biascorrectedDeltaRij(increment, {});
};
const Matrix3 sane = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal(sane, pim.delRdelBiasOmega()));
// TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one!
// EXPECT(assert_equal(sane, H));
// Let's integrate a second IMU measurement and check the Jacobian update:
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
const Matrix3 sane2 = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal(sane2, pim.delRdelBiasOmega()));
}
//******************************************************************************