diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c3c73e9d8..ed4ed583e 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -144,8 +144,9 @@ Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body, velocity + a_nav* dt; // velocity if (A) { -#define USE_NUMERICAL_DERIVATIVE #ifdef USE_NUMERICAL_DERIVATIVE + // The use of this yields much more accurate derivatives, but it's slow! + // TODO(frank): find a cheap closed form solution (look at Iserles) auto f = [w_body](const Vector3& theta) { return Rot3::ExpmapDerivative(theta).inverse() * w_body; }; @@ -153,7 +154,6 @@ Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body, numericalDerivative11(f, theta); #else // First order (small angle) approximation of derivative of invH*w: - // TODO(frank): find a cheap closed form solution (look at Iserles) // NOTE(frank): Rot3::ExpmapDerivative(w_body) is a less accurate approximation const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body); #endif diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 06ceef994..036afcdb9 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -139,7 +139,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1), pim.zeta_H_biasAcc())); EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1), - pim.zeta_H_biasOmega(), 1e-7)); + pim.zeta_H_biasOmega(), 1e-3)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c02b49fbf..78e9c5ddd 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -453,7 +453,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1), pim.zeta_H_biasAcc())); EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1), - pim.zeta_H_biasOmega(), 1e-7)); + pim.zeta_H_biasOmega(), 1e-3)); } /* ************************************************************************* */