code clean up and brief testing
another util function: creates the 3D transformation matrix (including Jacobian)
second possibility implemented to achieve relative motion from visual odometry messages. and further tests: comp- and inverseoperator work with translation, but rotation makes problems.