4 cout <<
"x1 covariance:\n" <<
marginals.marginalCovariance(1) << endl;
5 cout <<
"x2 covariance:\n" <<
marginals.marginalCovariance(2) << endl;
6 cout <<
"x3 covariance:\n" <<
marginals.marginalCovariance(3) << endl;
NonlinearFactorGraph graph
Marginals marginals(graph, result)