Separated pnp, rodrigues and related stuff into its own object

This shortens and simplifies Chiltiags3D a little bit and also will allow a cleaner next step which is noise filtering on the 3D pose and then the integration of the IMU if present. 

This is a purely refactoring PR, API/performance is not changed.