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

Switched to Eigen Fixed-Size Block expression.

release/4.3a0
Alex Hagiopol 2016-04-11 19:01:58 -04:00
parent 72a21d63d0
commit 3c195f5bc6
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/dataset.cpp
View File

@ -564,10 +564,12 @@ GraphAndValues load3D(const string& filename) {
}
}
Matrix mgtsam = I_6x6;
mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal
mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation
mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation
mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);