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

Punctuation/scope

release/4.3a0
AndreMichelin 2024-08-12 19:03:40 -07:00
parent c6bd3f8e32
commit c22b76506c
1 changed files with 28 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,40 +36,33 @@ const Vector3 measuredOmega = trueOmega + bias;
const double deltaT = 0.5;
} // namespace biased_x_rotation
// Create params where x and y axes are exchanged.
static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
auto p = std::make_shared<PreintegratedRotationParams>();
p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}});
return p;
}
//******************************************************************************
TEST(PreintegratedRotation, integrateGyroMeasurement) {
// Example where IMU is identical to body frame, then omega is roll
using namespace biased_x_rotation;
auto p = std::make_shared<PreintegratedRotationParams>();
PreintegratedRotation pim(p);
// Check the value.
Matrix3 H_bias;
internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
const Rot3 incrR = f(bias, H_bias);
Rot3 expected = Rot3::Roll(omega * deltaT);
EXPECT(assert_equal(expected, incrR, 1e-9));
const Rot3 expected = Rot3::Roll(omega * deltaT);
EXPECT(assert_equal(expected, incrR, 1e-9))
// Check the derivative:
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias))
// Check value of deltaRij() after integration.
Matrix3 F;
PreintegratedRotation pim(p);
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9))
// Check that system matrix F is the first derivative of compose:
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
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()));
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()))
// Check if we make a correction to the bias, the value and Jacobian are
// correct. Note that the bias is subtracted from the measurement, and the
@ -78,8 +71,8 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) {
const double delta = 0.05;
const Vector3 biasOmegaIncr(delta, 0, 0);
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9));
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) {
@ -89,39 +82,47 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) {
}
//******************************************************************************
// Create params where x and y axes are exchanged.
static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
auto p = std::make_shared<PreintegratedRotationParams>();
p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}});
return p;
}
TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
// Example where IMU is rotated, so measured omega indicates pitch.
using namespace biased_x_rotation;
auto p = paramsWithTransform();
PreintegratedRotation pim(p);
// Check the value.
Matrix3 H_bias;
internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
Rot3 expected = Rot3::Pitch(omega * deltaT);
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
const Rot3 expected = Rot3::Pitch(omega * deltaT); // Pitch, because of sensor-IMU rotation!
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9))
// Check the derivative:
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias))
// Check value of deltaRij() after integration.
Matrix3 F;
PreintegratedRotation pim(p);
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9))
// Check that system matrix F is the first derivative of compose:
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
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()));
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);
EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected));
EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9));
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) {