Improved documentation in Pose, added helper vec/mat templates. Also renamed 'compose' to 'composeBody' for clarity. Added code to convert from GTSAM to krpose.