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

Block-wise multiplication

release/4.3a0
Frank Dellaert 2016-01-02 15:47:41 -08:00
parent a313fb92b9
commit f11369ff4d
1 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/navigation/AggregateImuReadings.cpp
View File

@ -232,23 +232,29 @@ SharedGaussian AggregateImuReadings::noiseModel() const {
Vector d; Vector d;
boost::tie(RS, d) = posterior_k_->matrix(); boost::tie(RS, d) = posterior_k_->matrix();
// NOTEfrank): R'*R = inv(zetaCov) // NOTEfrank): R'*R = inv(zetaCov)
const Matrix9 R = RS.block<9, 9>(0, 0); Matrix9 R = RS.block<9, 9>(0, 0);
// Correct for application of retract, by calculating the retract derivative H // Correct for application of retract, by calculating the retract derivative H
// TODO(frank): yet another application of expmap and expmap derivative // We have inv(Rp'Rp) = H inv(Rz'Rz) H' => Rp = Rz * inv(H)
Vector3 theta = values.at<Vector3>(T(k_)); // From NavState::retract:
// H << D_R_theta, Z_3x3, Z_3x3,
// Z_3x3, iRj.transpose(), Z_3x3,
// Z_3x3, Z_3x3, iRj.transpose();
Matrix3 D_R_theta; Matrix3 D_R_theta;
const Rot3 iRb = Rot3::Expmap(theta, D_R_theta); Vector3 theta = values.at<Vector3>(T(k_));
Matrix9 H; // TODO(frank): yet another application of expmap and expmap derivative
H << D_R_theta, Z_3x3, Z_3x3, // const Matrix3 iRj = Rot3::Expmap(theta, D_R_theta).matrix();
Z_3x3, iRb.transpose(), Z_3x3, //
Z_3x3, Z_3x3, iRb.transpose();
// inv(Rp'Rp) = H inv(Rz'Rz) H' => Rp = Rz * inv(H) // Rp = R * H.inverse(), implemented blockwise in-place below
Matrix9 Rp = R * H.inverse(); // NOTE(frank): makes sense: a change in the j-frame has to be converted to a
// change in the i-frame, byy rotating with iRj. Similarly, a change in
// rotation nRj is mapped to a change in theta via the inverse dexp.
R.block<9, 3>(0, 0) *= D_R_theta.inverse();
R.block<9, 3>(0, 3) *= iRj;
R.block<9, 3>(0, 6) *= iRj;
// TODO(frank): think of a faster way - implement in noiseModel // TODO(frank): think of a faster way - implement in noiseModel
return noiseModel::Gaussian::SqrtInformation(Rp, false); return noiseModel::Gaussian::SqrtInformation(R, false);
} }
Matrix9 AggregateImuReadings::preintMeasCov() const { Matrix9 AggregateImuReadings::preintMeasCov() const {