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

Some minor refactoring/renaming

release/4.3a0
dellaert 2015-07-27 16:00:06 +02:00
parent a02a167da4
commit 1ac790da1c
1 changed files with 5 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/PreintegrationBase.cpp
View File

@ -64,25 +64,22 @@ void PreintegrationBase::updatePreintegratedMeasurements(
OptionalJacobian<9, 9> F) {
const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Vector3 j_acc = dRij * correctedAcc; // acceleration in current frame
const Vector3 i_acc = dRij * correctedAcc; // acceleration in i frame
double dt22 = 0.5 * deltaT * deltaT;
deltaPij_ += deltaVij_ * deltaT + dt22 * j_acc;
deltaVij_ += deltaT * j_acc;
deltaPij_ += deltaVij_ * deltaT + dt22 * i_acc;
deltaVij_ += deltaT * i_acc;
Matrix3 R_i, F_angles_angles;
if (F)
R_i = dRij; // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
if (F) {
const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
const Matrix3 F_vel_angles = -dRij * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles;
F_pos_angles = 0.5 * F_vel_angles * deltaT;
// pos vel angle
*F << //
I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
I_3x3, I_3x3 * deltaT, 0.5 * F_vel_angles * deltaT, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles; // angle
}