Derivative accuracy

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);
 

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);
 }