Add test for guess matrix in GICP

This commit adds a test unit to prove that #887 fixes #754.
This can also be check with this [test program](https://gist.github.com/VictorLamoine/20ef23dcb7cf6ef93cdd).

@saarnold is it ok for you now?