Add test for guess matrix in GICP