[ArucoThread] add computeCorMat(origM) for computing added correction corM matrix from origM matrix. Correction matrix will be use for normalizing matrix from aruco, like camera was in horizontal direction. Add also debug methods for printing matrix and vector, and 1 test, for creating Unit test later.