Visualize Pose Covariance Ellipsoid

[mcm001]

Is your feature request related to a problem? Please describe.

I'm playing with gtsam to generate tag maps and can estimate the marginal covariance matrix for each of my states (robot poses, tag poses, etc).

Describe the solution you'd like

It would be great to visualize this covariance matrix as a rotated ellipsoid for position uncertainty. I'm using the following wpistruct for PoseWithVariance: Pose3d pose; double rx; double ry; double rz; double tx; double ty; double tz. I'm right now just sending the variance instead of the full 6x6 pose covariance matrix to save data on the wire but if we can do cool stuff with the matrix let's do it.

Describe alternatives you've considered

Rviz is able to do this - see some screenshots in ros-visualization/rviz#1540 - and I can always just graph the uncertainty components. but this is harder to visualize

This is what the current visualization of camera and tag poses looks like: