diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 92d4b9520..cc4fcee16 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -268,41 +268,41 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const dfVdPi += temp * dt; } (*H1) << + D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dRi + Z_3x3, // dfR/dPi skewSymmetric(fp + deltaPij_biascorrected), // dfP/dRi dfPdPi, // dfP/dPi skewSymmetric(fv + deltaVij_biascorrected), // dfV/dRi - dfVdPi, // dfV/dPi - D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dRi - Z_3x3; // dfR/dPi + dfVdPi; // dfV/dPi } if (H2) { (*H2) << + Z_3x3, // dfR/dVi -Ri.transpose() * dt + Ritranspose_omegaCoriolisHat * dt2, // dfP/dVi - -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt, // dfV/dVi - Z_3x3; // dfR/dVi + -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt; // dfV/dVi } if (H3) { (*H3) << + D_fR_fRrot, Z_3x3, // dfR/dPosej Z_3x3, Ri.transpose() * rot_j.matrix(), // dfP/dPosej - Matrix::Zero(3, 6), // dfV/dPosej - D_fR_fRrot, Z_3x3; // dfR/dPosej + Matrix::Zero(3, 6); // dfV/dPosej } if (H4) { (*H4) << + Z_3x3, // dfR/dVj Z_3x3, // dfP/dVj - Ri.transpose(), // dfV/dVj - Z_3x3; // dfR/dVj + Ri.transpose(); // dfV/dVj } if (H5) { // H5 by this point already contains 3*3 biascorrectedThetaRij derivative const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; (*H5) << + Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega), // dfR/dBias -delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias - -delVdelBiasAcc(), -delVdelBiasOmega(), // dfV/dBias - Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega); // dfR/dBias + -delVdelBiasAcc(), -delVdelBiasOmega(); // dfV/dBias } Vector9 r; - r << fp, fv, fR; + r << fR, fp, fv; return r; } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 6b66be342..c6aa48b30 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -47,7 +47,7 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias) { return Rot3::Expmap( - factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3)); + factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); } // Auxiliary functions to test Jacobians F and G used for @@ -247,16 +247,16 @@ TEST(ImuFactor, ErrorAndJacobians) { // Jacobians are around zero, so the rotation part is the same as: Matrix H1Rot3 = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - EXPECT(assert_equal(H1Rot3, H1a.bottomRows(3))); + EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); Matrix H3Rot3 = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - EXPECT(assert_equal(H3Rot3, H3a.bottomRows(3))); + EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); // Evaluate error with wrong values Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); values.update(V(2), v2_wrong); - errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; + errorExpected << 0, 0, 0, 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901; EXPECT( assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6));