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; dfVdPi += temp * dt;
} }
(*H1) << (*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 skewSymmetric(fp + deltaPij_biascorrected), // dfP/dRi
dfPdPi, // dfP/dPi dfPdPi, // dfP/dPi
skewSymmetric(fv + deltaVij_biascorrected), // dfV/dRi skewSymmetric(fv + deltaVij_biascorrected), // dfV/dRi
dfVdPi, // dfV/dPi dfVdPi; // dfV/dPi
D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dRi
Z_3x3; // dfR/dPi
} }
if (H2) { if (H2) {
(*H2) << (*H2) <<
Z_3x3, // dfR/dVi
-Ri.transpose() * dt + Ritranspose_omegaCoriolisHat * dt2, // dfP/dVi -Ri.transpose() * dt + Ritranspose_omegaCoriolisHat * dt2, // dfP/dVi
-Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt, // dfV/dVi -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt; // dfV/dVi
Z_3x3; // dfR/dVi
} }
if (H3) { if (H3) {
(*H3) << (*H3) <<
D_fR_fRrot, Z_3x3, // dfR/dPosej
Z_3x3, Ri.transpose() * rot_j.matrix(), // dfP/dPosej Z_3x3, Ri.transpose() * rot_j.matrix(), // dfP/dPosej
Matrix::Zero(3, 6), // dfV/dPosej Matrix::Zero(3, 6); // dfV/dPosej
D_fR_fRrot, Z_3x3; // dfR/dPosej
} }
if (H4) { if (H4) {
(*H4) << (*H4) <<
Z_3x3, // dfR/dVj
Z_3x3, // dfP/dVj Z_3x3, // dfP/dVj
Ri.transpose(), // dfV/dVj Ri.transpose(); // dfV/dVj
Z_3x3; // dfR/dVj
} }
if (H5) { if (H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
(*H5) << (*H5) <<
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega), // dfR/dBias
-delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias -delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias
-delVdelBiasAcc(), -delVdelBiasOmega(), // dfV/dBias -delVdelBiasAcc(), -delVdelBiasOmega(); // dfV/dBias
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega); // dfR/dBias
} }
Vector9 r; Vector9 r;
r << fp, fv, fR; r << fR, fp, fv;
return r; 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 Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) { const imuBias::ConstantBias& bias) {
return Rot3::Expmap( 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 // 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: // Jacobians are around zero, so the rotation part is the same as:
Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>( Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); 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>( Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); 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 // Evaluate error with wrong values
Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
values.update(V(2), v2_wrong); 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( EXPECT(
assert_equal(errorExpected, assert_equal(errorExpected,
factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6)); factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6));