H矩阵求取旋转和平移分量

最近在做机器人导航项目，于是就顺手在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);
     // three columns of rotation matrix
     CvMat* r1 = cvCreateMat(3, 1, CV_64F);
     CvMat* r2 = cvCreateMat(3, 1, CV_64F);
     CvMat* r3 = cvCreateMat(3, 1, CV_64F);
     // translation vector
     CvMat _t =  cvMat(3, 1, CV_64F, t);
     cvGetCol(H,h1,0);
     cvGetCol(H,h2,1);
     cvGetCol(H,h3,2);
     cvGetCol(&_r,r1,0);
     cvGetCol(&_r,r2,1);
     cvGetCol(&_r,r3,2);

     cvInvert(&_M, invM);
     cvMatMul(invM,h1,r1);
     cvMatMul(invM,h2,r2);
     cvMatMul(invM,h3,&_t);

     cvNormalize(r1, r1);
     cvMul(r2,&_ones,r2,1/cvNorm(r1));
     cvMul(&_t,&_ones, &_t,1/cvNorm(r1) );
     cvCrossProduct(r1, r2, r3);
     cvSVD(&_r, W, U, V, CV_SVD_V_T);
     cvMatMul(U,V,&_r);
     cvRodrigues2(&_r, &_rodrot, NULL);
     return 1;
 }

 

代码原理的话可以参考[1]，实现的时候还碰到了一个小问题，即Rotation矩阵三列归一化的时候都是除以第一列矩阵的Norm，当时自己实现的时候没有发现，误用了三列各自的Norm，结果一直没有找到错误，后来参考了[2]的代码才得以解决。

 

函数的输出一个是Translation 向量，另一个是Rodrigues变换之后的向量，该向量的方向代表旋转轴，模代表旋转的角度，具体数学表示见参考[3].

 

函数中的输入Homography矩阵，可以用cvFindHomography求出。至于如何从特征点求得Homography可以参考opencv2.0里find_obj例程的下面这段代码：

 

 if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
     return 0;

 

下面是上面这个函数的一个应用，也是正在进行中的一个工作：(sigh, csdn博客竟然不支持嵌入式视频功能。。。太不给力了，真的是要找下家了。。)

墙外的可以点击：http://www.youtube.com/watch?v=YjrTFO0ftDQ

墙内的youku视频正在上传。。。这里上传好慢。。。

 

 

视频是一个移动机器人在遥控器控制下用BumbleBee2立体摄像头拍的一段视频处理后的结果。

下面是iRobot机器人平台，低角度拍摄很是霸气哈哈。上面有很多传感器，我只用了BumbleBee2：

 

 

 

目前的话，还有两个问题：

 

1. 一个由于SURF点是每个frame都要重新计算的，因此计算出来Homography矩阵不一定准确，表现在视频上的话会产生映射的抖动(见视频mapping的框)，而且在特征点比较少的时候，这种抖动尤为明显(可以见当机器人从室内转到走廊这段，由于走廊的SURF点明显减少，映射框剧烈抖动)。这种抖动用optical flow应该可以解决。不过在目前应用中应该也不是特别大的问题，而且个人认为这种连续性需要到scene understanding这个更高的层次解决，而不是死抠这里一些只关注细节的方法。

 

 

2. 另外一个问题是关于Rotation向量的，从这段视频中可以提取出50个连续的场景(目前程序里不同场景的定义两个场景之间匹配的SURF点小于整个场景SURF点的1/8，这个参数是可调的)，不同场景之间的旋转和平移量见下图：

 

可以看出场景之间的切换基本都是Translation(T向量的z都是1，所以我省略了), Rotation的量很小(最大也只不过是0.2弧度左右)，这与机器人在场景里面的运动相符

但是可以看Rotation向量，计算出来的值似乎是显示所有的旋转都是绕着第三个量也就是Z轴进行的，而在实际过程中，旋转似乎是主要绕着Y轴进行的。

当然根据这个结果我们可以大致绘制出机器人的行进路线图，背后的原理似乎还有点那么不通，感觉这是由于我对于Affine Space和Othodogical Space没有完全理解导致的。求高人解答。

 

*******************************************************************

 

刚刚去问了老师。。。整个彻底错了。。。立体的场景不是一个平面，中间的点是不能用Homography来映射的，这个地方是需要用Fundamental矩阵的。这也是为什么Homography抖动以及求出Rotation Translation有误的原因。

 

基础不牢，地动山摇，本文权当反面教材吧。

 

********************************************************************

再补充20110525：

经验证 利用宽松限制(比如允许误差在10像素以内)的RANSAC求Homography是可以粗略筛选outlier匹配对的，效果比用RANSAC做FindFundmental好。因为Homography在两幅视图里是点对应，而Fundamental在两幅视图里是点和极线对应

 

_________________________________________________________________

 

Reference:

[1]. Chapter 11, Learning OpenCV: P384-P391

[2]. https://gist.github.com/740979

[3]. http://opencv.willowgarage.com/documentation/camera_calibration_and_3d_reconstruction.html#rodrigues2