从Homography矩阵得到旋转和平移量(From Homography to Rotation and Translation)

最近在做机器人导航项目，于是就顺手在OpenCV里面将以前看过但是不太理解的从Homography求Rotation和Translation实现了一下

代码如下：

int calcRTfromHomo(CvMat* H, double t[3], double rodrot[3]){ double r[9]; CvMat _r = cvMat(3, 3, CV_64F, r); //rotation matrix double intrinsic[9]={1, 0, 0, 0, 1, 0, 0, 0, 1}; CvMat _M = cvMat(3, 3, CV_64F, intrinsic); //intrinsic matrix, of no use in this implementation, reserved for future use double ones[]={1,1,1}; CvMat _ones = cvMat(3, 1, CV_64F, ones); // double rodrot[3]; CvMat _rodrot = cvMat(3, 1, CV_64F, rodrot); //for SVD CvMat* U = cvCreateMat(3, 3, CV_64F); CvMat* W = cvCreateMat(3, 3, CV_64F); CvMat* V = cvCreateMat(3, 3, CV_64F); CvMat* invM = cvCreateMat(3, 3, CV_64F); // three columns of Homography matrix CvMat* h1 = cvCreateMat(3, 1, CV_64F); CvMat* h2 = cvCreateMat(3, 1, CV_64F); CvMat* h3 = cvCreateMat(3, 1, CV_64F); /