Visualize Pose Covariance Ellipsoid #285
Description
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:
