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

Small savings of matrix-vector mult

release/4.3a0
dellaert 2014-11-23 13:57:27 +01:00
parent 490520d6d0
commit 36bc1d3e3f
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/AHRSFactor.cpp
View File

@ -139,14 +139,15 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
Vector3 AHRSFactor::PreintegratedMeasurements::predict( Vector3 AHRSFactor::PreintegratedMeasurements::predict(
const imuBias::ConstantBias& bias, boost::optional<Matrix&> H) const { const imuBias::ConstantBias& bias, boost::optional<Matrix&> H) const {
const Vector3 biasOmegaIncr = bias.gyroscope() - biasHat_.gyroscope(); const Vector3 biasOmegaIncr = bias.gyroscope() - biasHat_.gyroscope();
Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
const Rot3 deltaRij_biascorrected = deltaRij_.retract( const Rot3 deltaRij_biascorrected = deltaRij_.retract(
delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) { if (H) {
const Matrix3 Jrinv_theta_bc = // const Matrix3 Jrinv_theta_bc = //
Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = // const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
} }
return theta_biascorrected; return theta_biascorrected;