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

Removed (passing) tests of deprecated

release/4.3a0
Frank Dellaert 2024-06-09 10:10:01 -07:00
parent b531df7004
commit 1ac286f97a
1 changed files with 2 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,7 +44,7 @@ static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
} }
//****************************************************************************** //******************************************************************************
TEST(PreintegratedRotation, integrateMeasurement) { TEST(PreintegratedRotation, integrateGyroMeasurement) {
// Example where IMU is identical to body frame, then omega is roll // Example where IMU is identical to body frame, then omega is roll
using namespace biased_x_rotation; using namespace biased_x_rotation;
auto p = std::make_shared<PreintegratedRotationParams>(); auto p = std::make_shared<PreintegratedRotationParams>();
@ -74,48 +74,7 @@ TEST(PreintegratedRotation, integrateMeasurement) {
} }
//****************************************************************************** //******************************************************************************
TEST(PreintegratedRotation, Deprecated) { TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
// 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;
PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
p->getBodyPSensor()};
Rot3 expected = Rot3::Roll(omega * deltaT);
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
// Ephemeral test for deprecated Jacobian:
Matrix3 D_incrR_integratedOmega;
(void)pim.incrementalRotation(measuredOmega, bias, deltaT,
D_incrR_integratedOmega);
auto g = [&](const Vector3& x, const Vector3& y) {
return pim.incrementalRotation(x, y, deltaT, {});
};
const Matrix3 oldJacobian =
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias);
EXPECT(assert_equal<Matrix3>(oldJacobian, -deltaT * D_incrR_integratedOmega));
// Check deprecated version.
Matrix3 D_incrR_integratedOmega2, F;
pim.integrateMeasurement(measuredOmega, bias, deltaT,
D_incrR_integratedOmega2, F);
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));
// Check that deprecated Jacobian is correct.
EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9));
// Make sure delRdelBiasOmega is H_bias after integration.
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
}
//******************************************************************************
TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
// Example where IMU is rotated, so measured omega indicates pitch. // Example where IMU is rotated, so measured omega indicates pitch.
using namespace biased_x_rotation; using namespace biased_x_rotation;
auto p = paramsWithTransform(); auto p = paramsWithTransform();
@ -143,47 +102,6 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega())); EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
} }
//******************************************************************************
TEST(PreintegratedRotation, DeprecatedWithTransform) {
// 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;
PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
p->getBodyPSensor()};
Rot3 expected = Rot3::Pitch(omega * deltaT);
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
// Ephemeral test for deprecated Jacobian:
Matrix3 D_incrR_integratedOmega;
(void)pim.incrementalRotation(measuredOmega, bias, deltaT,
D_incrR_integratedOmega);
auto g = [&](const Vector3& x, const Vector3& y) {
return pim.incrementalRotation(x, y, deltaT, {});
};
const Matrix3 oldJacobian =
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias);
EXPECT(assert_equal<Matrix3>(oldJacobian, -deltaT * D_incrR_integratedOmega));
// Check deprecated version.
Matrix3 D_incrR_integratedOmega2, F;
pim.integrateMeasurement(measuredOmega, bias, deltaT,
D_incrR_integratedOmega2, F);
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));
// Check that deprecated Jacobian is correct.
EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9));
// Make sure delRdelBiasOmega is H_bias after integration.
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
}
//****************************************************************************** //******************************************************************************
int main() { int main() {
TestResult tr; TestResult tr;