12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed order of components in error to match RTV order

release/4.3a0
dellaert 2015-07-17 15:34:58 -07:00
parent 61c58c6fa6
commit 55269d642c
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/navigation/PreintegrationBase.cpp
View File

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

8
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -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<Rot3, Pose3>(
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<Rot3, Pose3>(
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));