Reproject error cost function. Plus several bugfixes for templated methods

Change-Id: Ia17f160f9a4c60445303afa7ebe7b74676cbb28d