Output a pose covariance estimate

[felix-h-kong]

Is your feature request related to a problem? Please describe. I would like to know the confidence of the pose estimate, and when it's reliable or not. For situations similar to the "flat wall experiment" (https://koide3.github.io/glim/demo.html#flat-wall-experiment), the pose covariance would be very useful.

Describe the solution you'd like If i understand iSAM and factor-graph SLAM methods correctly, I suspect covariances are being used in the in the graph and the optimization problem already, they're just not being published. I would like to have covariance be published with the pose message. That is, the message type would be PoseWithCovarianceStamped as opposed to PoseStamped.

[koide3]

It should be easy to implement it with isam2->marginalCovariance(). I'll add a topic publishing PoseWithCovarianceStamped later.