Fix pose vector test, clean up unused code

Fix pose vector test, clean up unused code #541