diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 367a62360..3b1cea06f 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -183,6 +183,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + // Note that derivative of constructors below is not identity for velocity, but + // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() NavState state_i(pose_i, vel_i); NavState state_j(pose_j, vel_j); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c95c1d667..79497f63c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -872,8 +872,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { values.insert(B(1), bias); // Make sure linearization is correct - double diffDelta = 1e-5; - // This fails, except if tol = 1e-1: probably wrong! + double diffDelta = 1e-7; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); }