change Matrix from Mat to Matx34d