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

Merge pull request #1797 from borglab/fix/ahrs_delR 

Fix and add tests for AHRS
release/4.3a0
Frank Dellaert 2024-08-26 16:18:50 -07:00 committed by GitHub
parent 69a64ffaef 475b37f7d6
commit cf8a04d198
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 186 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/geometry/tests/testPose3.cpp
View File

@ -1207,6 +1207,31 @@ TEST(Pose3, Print) {
EXPECT(assert_print_equal(expected, pose));
}
/* ************************************************************************* */
TEST(Pose3, ExpmapChainRule) {
// Muliply with an arbitrary matrix and exponentiate
Matrix6 M;
M << 1, 2, 3, 4, 5, 6, //
7, 8, 9, 1, 2, 3, //
4, 5, 6, 7, 8, 9, //
1, 2, 3, 4, 5, 6, //
7, 8, 9, 1, 2, 3, //
4, 5, 6, 7, 8, 9;
auto g = [&](const Vector6& omega) {
return Pose3::Expmap(M*omega);
};
// Test the derivatives at zero
const Matrix6 expected = numericalDerivative11<Pose3, Vector6>(g, Z_6x1);
EXPECT(assert_equal<Matrix6>(expected, M, 1e-5)); // Pose3::ExpmapDerivative(Z_6x1) is identity
// Test the derivatives at another value
const Vector6 delta{0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
const Matrix6 expected2 = numericalDerivative11<Pose3, Vector6>(g, delta);
const Matrix6 analytic = Pose3::ExpmapDerivative(M*delta) * M;
EXPECT(assert_equal<Matrix6>(expected2, analytic, 1e-5)); // note tolerance
}
/* ************************************************************************* */
int main() {
TestResult tr;

40
gtsam/geometry/tests/testRot3.cpp
View File

@ -956,6 +956,46 @@ TEST(Rot3, determinant) {
}
}
/* ************************************************************************* */
TEST(Rot3, ExpmapChainRule) {
// Multiply with an arbitrary matrix and exponentiate
Matrix3 M;
M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
auto g = [&](const Vector3& omega) {
return Rot3::Expmap(M*omega);
};
// Test the derivatives at zero
const Matrix3 expected = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal<Matrix3>(expected, M, 1e-5)); // SO3::ExpmapDerivative(Z_3x1) is identity
// Test the derivatives at another value
const Vector3 delta{0.1,0.2,0.3};
const Matrix3 expected2 = numericalDerivative11<Rot3, Vector3>(g, delta);
EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5));
}
/* ************************************************************************* */
TEST(Rot3, expmapChainRule) {
// Multiply an arbitrary rotation with exp(M*x)
// Perhaps counter-intuitively, this has the same derivatives as above
Matrix3 M;
M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
const Rot3 R = Rot3::Expmap({1, 2, 3});
auto g = [&](const Vector3& omega) {
return R.expmap(M*omega);
};
// Test the derivatives at zero
const Matrix3 expected = numericalDerivative11<Rot3, Vector3>(g, Z_3x1);
EXPECT(assert_equal<Matrix3>(expected, M, 1e-5));
// Test the derivatives at another value
const Vector3 delta{0.1,0.2,0.3};
const Matrix3 expected2 = numericalDerivative11<Rot3, Vector3>(g, delta);
EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5));
}
/* ************************************************************************* */
int main() {
TestResult tr;

10
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -75,13 +75,11 @@ Rot3 IncrementalRotation::operator()(
Vector3 correctedOmega = measuredOmega - bias;
// Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame. If Jacobian is
// requested, the rotation matrix is obtained as `rotate` Jacobian.
Matrix3 body_R_sensor;
// (originally in the IMU frame) into the body frame.
// Note that the rotate Jacobian is just body_P_sensor->rotation().matrix().
if (body_P_sensor) {
// rotation rate vector in the body frame
correctedOmega = body_P_sensor->rotation().rotate(
correctedOmega, {}, H_bias ? &body_R_sensor : nullptr);
correctedOmega = body_P_sensor->rotation() * correctedOmega;
}
// rotation vector describing rotation increment computed from the
@ -90,7 +88,7 @@ Rot3 IncrementalRotation::operator()(
Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !!
if (H_bias) {
*H_bias *= -deltaT; // Correct so accurately reflects bias derivative
if (body_P_sensor) *H_bias *= body_R_sensor;
if (body_P_sensor) *H_bias *= body_P_sensor->rotation().matrix();
}
return incrR;
}

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

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

138
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,56 +71,127 @@ 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) {
// return pim.biascorrectedDeltaRij(increment, {});
// };
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
// Check the derivative matches the numerical one
auto g = [&](const Vector3& increment) {
return pim.biascorrectedDeltaRij(increment, {});
};
Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
EXPECT(assert_equal(expectedH, H));
// Let's integrate a second IMU measurement and check the Jacobian update:
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
EXPECT(assert_equal(expectedH, H));
}
//******************************************************************************
// 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) {
// return pim.biascorrectedDeltaRij(increment, {});
// };
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
// Check the derivative matches the *expectedH* one
auto g = [&](const Vector3& increment) {
return pim.biascorrectedDeltaRij(increment, {});
};
Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
EXPECT(assert_equal(expectedH, H));
// Let's integrate a second IMU measurement and check the Jacobian update:
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
EXPECT(assert_equal(expectedH, H));
}
// 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);
// Check the derivative matches the numerical one
auto g = [&](const Vector3& increment) {
return pim.biascorrectedDeltaRij(increment, {});
};
Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
EXPECT(assert_equal(expectedH, H));
// Let's integrate a second IMU measurement and check the Jacobian update:
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
EXPECT(assert_equal(expectedH, H));
}
//******************************************************************************