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

marginals

release/4.3a0
Frank Dellaert 2012-05-20 22:02:11 +00:00
parent 00c56b0d0a
commit 7a28e6d5cb
1 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
examples/LocalizationExample.cpp
View File

@ -18,6 +18,13 @@
// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
// include this for marginals
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <iomanip>
#include <iostream>
using namespace std;
using namespace gtsam;
@ -58,6 +65,14 @@ int main(int argc, char** argv) {
pose2SLAM::Values result = graph.optimize(initialEstimate);
result.print("\nFinal result:\n ");
// use an explicit Optimizer object so we can query the marginals
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Marginals marginals(graph, result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(3)) << endl;
return 0;
}