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

Clean up jacobians a bit

release/4.3a0
dellaert 2015-07-17 01:14:44 -07:00
parent d45d2e17ed
commit 66a9c34840
1 changed files with 21 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
gtsam/navigation/PreintegrationBase.cpp
View File

@ -246,10 +246,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
// Calculate Jacobians, matrices below needed only for some Jacobians... // Calculate Jacobians, matrices below needed only for some Jacobians...
Vector3 fR; Vector3 fR;
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat; Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot;
// This is done to save computation: we ask for the jacobians only when they are needed // This is done to save computation: we ask for the jacobians only when they are needed
const double dt = deltaTij(), dt2 = dt*dt;
Rot3 fRrot; Rot3 fRrot;
const Rot3 RiBetweenRj = rot_i.between(rot_j); const Rot3 RiBetweenRj = rot_i.between(rot_j);
if (H1 || H2 || H3 || H4 || H5) { if (H1 || H2 || H3 || H4 || H5) {
@ -265,6 +264,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
fR = Rot3::Logmap(fRrot); fR = Rot3::Logmap(fRrot);
} }
const double dt = deltaTij(), dt2 = dt*dt;
Matrix3 Ritranspose_omegaCoriolisHat;
if (H1 || H2) if (H1 || H2)
Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
@ -278,56 +279,38 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
dfVdPi += temp * dt; dfVdPi += temp * dt;
} }
(*H1) << (*H1) <<
// dfP/dRi skewSymmetric(fp + deltaPij_biascorrected), // dfP/dRi
skewSymmetric(fp + deltaPij_biascorrected), dfPdPi, // dfP/dPi
// dfP/dPi skewSymmetric(fv + deltaVij_biascorrected), // dfV/dRi
dfPdPi, dfVdPi, // dfV/dPi
// dfV/dRi D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dRi
skewSymmetric(fv + deltaVij_biascorrected), Z_3x3; // dfR/dPi
// dfV/dPi
dfVdPi,
// dfR/dRi
D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis),
// dfR/dPi
Z_3x3;
} }
if (H2) { if (H2) {
(*H2) << (*H2) <<
// dfP/dVi -Ri.transpose() * dt + Ritranspose_omegaCoriolisHat * dt2, // dfP/dVi
-Ri.transpose() * dt + Ritranspose_omegaCoriolisHat * dt2, // Coriolis term - we got rid of the 2 wrt ins paper -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt, // dfV/dVi
// dfV/dVi Z_3x3; // dfR/dVi
-Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt, // Coriolis term
// dfR/dVi
Z_3x3;
} }
if (H3) { if (H3) {
(*H3) << (*H3) <<
// dfP/dPosej Z_3x3, Ri.transpose() * rot_j.matrix(), // dfP/dPosej
Z_3x3, Ri.transpose() * rot_j.matrix(), Matrix::Zero(3, 6), // dfV/dPosej
// dfV/dPosej D_fR_fRrot, Z_3x3; // dfR/dPosej
Matrix::Zero(3, 6),
// dfR/dPosej
D_fR_fRrot, Z_3x3;
} }
if (H4) { if (H4) {
(*H4) << (*H4) <<
// dfP/dVj Z_3x3, // dfP/dVj
Z_3x3, Ri.transpose(), // dfV/dVj
// dfV/dVj Z_3x3; // dfR/dVj
Ri.transpose(),
// dfR/dVj
Z_3x3;
} }
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) <<
// dfP/dBias -delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias
-delPdelBiasAcc(), -delPdelBiasOmega(), -delVdelBiasAcc(), -delVdelBiasOmega(), // dfV/dBias
// dfV/dBias Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega); // dfR/dBias
-delVdelBiasAcc(), -delVdelBiasOmega(),
// dfR/dBias
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega);
} }
Vector9 r; Vector9 r;
r << fp, fv, fR; r << fp, fv, fR;