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

Avoid duplicate calculation of D_acc_R

release/4.3a0
dellaert 2015-07-29 21:21:03 -07:00
parent e5ace26d5f
commit 7149b8ee42
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/navigation/PreintegrationBase.cpp
View File

@ -89,8 +89,8 @@ void PreintegrationBase::updatePreintegratedMeasurements(
// Calculate acceleration in *current* i frame, i.e., before rotation update below // Calculate acceleration in *current* i frame, i.e., before rotation update below
Matrix3 D_acc_R; Matrix3 D_acc_R;
const Matrix3 dRij = deltaRij_.matrix(); // expensive const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F ? &D_acc_R : 0); const Vector3 i_acc = deltaRij_.rotate(correctedAcc, D_acc_R);
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * delRdelBiasOmega_; const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
Matrix3 D_Rij_incrR; Matrix3 D_Rij_incrR;
PreintegratedRotation::integrateMeasurement(measuredOmega, PreintegratedRotation::integrateMeasurement(measuredOmega,
@ -106,9 +106,9 @@ void PreintegrationBase::updatePreintegratedMeasurements(
deltaT * D_acc_R, Z_3x3, I_3x3; // vel deltaT * D_acc_R, Z_3x3, I_3x3; // vel
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij;
delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * temp; delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp * deltaT; delVdelBiasOmega_ += D_acc_biasOmega * deltaT;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------