Print whole matrix when debugging Kimera