visualization improvements

marginalization
markers now have correct shape and size and position wrt center of mass in rviz