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

ok, fixed updatePreintegratedMeasurements with optionalJacobian

release/4.3a0
Luca 2014-12-09 18:32:38 -05:00
parent d809a952df
commit b96a463b10
1 changed files with 4 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/navigation/PreintegrationBase.h
View File

@ -127,28 +127,16 @@ public:
} }
deltaVij_ += temp; deltaVij_ += temp;
// const Matrix3 R_i = deltaRij(); Matrix3 R_i;
// OptionalJacobian<3,3> F_angles_angles; if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
// updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? F_angles_angles : boost::none); Matrix3 F_angles_angles;
// if(F){ updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0);
// Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
// // pos vel angle
// *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
// Z_3x3, I_3x3, F_vel_angles, // vel
// Z_3x3, Z_3x3, F_angles_angles;// angle
// }
if(F){ if(F){
const Matrix3 R_i = deltaRij();
Matrix3 F_angles_angles;
updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
F->resize(9,9);
// pos vel angle // pos vel angle
*F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles;// angle Z_3x3, Z_3x3, F_angles_angles;// angle
}else{
updateIntegratedRotationAndDeltaT(incrR,deltaT);
} }
} }