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