Add get_diagonal_vector() method to SGMatrix class