最近在做機器人導航專案，於是就順手在OpenCV裡面將以前看過但是不太理解的從Homography求Rotation和Translation實現了一下

程式碼如下：

1. int calcRTfromHomo(CvMat* H, double t[3], double rodrot[3]){  
2.     double r[9];  
3.     CvMat _r = cvMat(3, 3, CV_64F, r);          //rotation matrix
4.     double intrinsic[9]={1, 0, 0, 0, 1, 0, 0, 0, 1};  
5.     CvMat _M = cvMat(3, 3, CV_64F, intrinsic); //intrinsic matrix, of no use in this implementation, reserved for future use
6.     double ones[]={1,1,1};  
7.     CvMat _ones = cvMat(3, 1, CV_64F, ones);      
8. //  double rodrot[3];
9.     CvMat _rodrot = cvMat(3, 1, CV_64F, rodrot);  
10.     //for SVD
11.     CvMat* U = cvCreateMat(3, 3, CV_64F);  
12.     CvMat* W = cvCreateMat(3, 3, CV_64F);  
13.     CvMat* V = cvCreateMat(3, 3, CV_64F);  
14.     CvMat* invM = cvCreateMat(3, 3, CV_64F);  
15.     // three columns of Homography matrix
16.     CvMat* h1 = cvCreateMat(3, 1, CV_64F);  
17.     CvMat* h2 = cvCreateMat(3, 1, CV_64F);  
18.     CvMat* h3 = cvCreateMat(3, 1, CV_64F);  
19.     // three columns of rotation matrix
20.     CvMat* r1 = cvCreateMat(3, 1, CV_64F);  
21.     CvMat* r2 = cvCreateMat(3, 1, CV_64F);  
22.     CvMat* r3 = cvCreateMat(3, 1, CV_64F);  
23.     // translation vector
24.     CvMat _t =  cvMat(3, 1, CV_64F, t);   
25.     cvGetCol(H,h1,0);  
26.     cvGetCol(H,h2,1);  
27.     cvGetCol(H,h3,2);  
28.     cvGetCol(&_r,r1,0);  
29.     cvGetCol(&_r,r2,1);  
30.     cvGetCol(&_r,r3,2);  
31.     cvInvert(&_M, invM);  
32.     cvMatMul(invM,h1,r1);  
33.     cvMatMul(invM,h2,r2);  
34.     cvMatMul(invM,h3,&_t);  
35.     cvNormalize(r1, r1);  
36.     cvMul(r2,&_ones,r2,1/cvNorm(r1));  
37.     cvMul(&_t,&_ones, &_t,1/cvNorm(r1) );  
38.     cvCrossProduct(r1, r2, r3);  
39.     cvSVD(&_r, W, U, V, CV_SVD_V_T);  
40.     cvMatMul(U,V,&_r);  
41.     cvRodrigues2(&_r, &_rodrot, NULL);  
42.     return 1;  
43. }  

程式碼原理的話可以參考[1]，實現的時候還碰到了一個小問題，即Rotation矩陣三列歸一化的時候都是除以第一列矩陣的Norm，當時自己實現的時候沒有發現，誤用了三列各自的Norm，結果一直沒有找到錯誤，後來參考了[2]的程式碼才得以解決。

函式的輸出一個是Translation 向量，另一個是Rodrigues變換之後的向量，該向量的方向代表旋轉軸，模代表旋轉的角度，具體數學表示見參考[3].

函式中的輸入Homography矩陣，可以用cvFindHomography求出。至於如何從特徵點求得Homography可以參考opencv2.0裡find_obj例程的下面這段程式碼：

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

下面是上面這個函式的一個應用，也是正在進行中的一個工作：(sigh, csdn部落格竟然不支援嵌入式視訊功能。。。太不給力了，真的是要找下家了。。)

牆內的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