Add test for guess matrix in GICP

This commit adds a test unit to prove that #887 fixes #754.