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

Pick out correct blocks

release/4.3a0
Frank Dellaert 2015-12-22 19:09:05 -08:00
parent 380d0dc989
commit 9b559b3620
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/ScenarioRunner.h
View File

@ -74,10 +74,10 @@ class ScenarioRunner {
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
Matrix6 poseCovariance( Matrix6 poseCovariance(
const ImuFactor::PreintegratedMeasurements& pim) const { const ImuFactor::PreintegratedMeasurements& pim) const {
Matrix9 cov = pim.preintMeasCov(); // _ position rotation Matrix9 cov = pim.preintMeasCov();
Matrix6 poseCov; Matrix6 poseCov;
poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), // poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), //
cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3); cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3);
return poseCov; return poseCov;
} }