diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index fdf522f73..6b9a48339 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -18,6 +18,13 @@ // pull in the 2D PoseSLAM domain with all typedefs and helper functions defined #include +// include this for marginals +#include +#include + +#include +#include + 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; }