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;
// const Matrix3 R_i = deltaRij();
// OptionalJacobian<3,3> F_angles_angles;
// updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? F_angles_angles : boost::none);
// if(F){
// 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){
const Matrix3 R_i = deltaRij();
Matrix3 R_i;
if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
Matrix3 F_angles_angles;
updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0);
if(F){
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
F->resize(9,9);
// 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
}else{
updateIntegratedRotationAndDeltaT(incrR,deltaT);
}
}