compute the marginal covariance for only a vertex

[JzHuai0108]

After optimizing a NonlinearFactorGraph, the marginal covariance of a vertex is desired. This can be done with the Marginals class, its syntax following gtsam examples looks like the following.

(1) gtsam::Marginals marginals(graph, result);

(2) Eigen::MatrixXd covKey = marginals.marginalCovariance(key);

By running a timing test, line (1) takes much longer time than (2), for example, 0.03 s vs 0.000005 s.

So I suspect that the Marginals constructor in line (1) does preparation for computing marginals of all vertices/keys.

Is there a way to compute the marginal covariance for a specific vertex/key without doing so much work as in the constructor of Marginals?

P.S. The g2o library computes the marginals for a vertex with the following pseudo code which may take 0.001 s.

g2o::SparseBlockMatrix<MatrixXd> spiv; optimizer.computeMarginals(spiv, vertex);

[Frank Dellaert]

No time to do this now, but we should look at this later