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

Small refactor, save on transpose and copy

release/4.3a0
dellaert 2014-11-23 11:24:43 +01:00
parent 7683917558
commit 5ec6039988
1 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/navigation/AHRSFactor.cpp
View File

@ -100,34 +100,34 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3 theta_incr = correctedOmega * deltaT; const Vector3 theta_incr = correctedOmega * deltaT;
// rotation increment computed from the current rotation rate measurement // rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); const Rot3 incrR = Rot3::Expmap(theta_incr);
const Matrix3 incrRt = incrR.transpose();
// Right jacobian computed at theta_incr // Right Jacobian computed at theta_incr
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
// Update Jacobians // Update Jacobians
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
delRdelBiasOmega_ = Rincr.transpose() * delRdelBiasOmega_ delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
- Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance // Update preintegrated measurements covariance
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
Rot3 Rot_j = deltaRij_ * Rincr; Rot3 Rot_j = deltaRij_ * incrR;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // Update preintegrated measurements covariance: as in [2] we consider a first
// can be seen as a prediction phase in an EKF framework // order propagation that can be seen as a prediction phase in an EKF framework
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.transpose() * Jr_theta_i; Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>
// (boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrpt preintegrated measurements (df/dx)
Matrix3 F; const Matrix3& F = H_angles_angles;
F << H_angles_angles;
// first order uncertainty propagation // first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise // the deltaT allows to pass from continuous time noise to discrete time noise
@ -136,7 +136,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// Update preintegrated measurements // Update preintegrated measurements
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
deltaRij_ = deltaRij_ * Rincr; deltaRij_ = deltaRij_ * incrR;
deltaTij_ += deltaT; deltaTij_ += deltaT;
} }