From 9d1111a76702d327c1ee0ebec61da516b6169768 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jul 2015 11:53:24 -0700 Subject: [PATCH] Derivative accuracy --- gtsam/navigation/PreintegrationBase.cpp | 2 ++ gtsam/navigation/tests/testImuFactor.cpp | 3 +-- 2 files changed, 3 insertions(+), 2 deletions(-) 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); }