位姿估計_3

這裡是位姿估計的第三講，利用滅點（vanishing point）來估計相機位姿。

滅點 vanishing point

滅點的介紹，wiki百科，簡單的說，就是在真是實體世界中互相平行的兩條直線，在相機的2d投影中，會彙聚相交到一點，該點就是滅點或者消失點（vanishing point），抽象描述實體世界中的無窮遠處。

如圖所示

圖中的灰色部分表示一個十字路口，其中在Z方向的無窮遠處有一點Z_inf，其在真實世界中的坐标為Z_inf = [0, 0, 1, 0]。我的了解是，在Z方向的無窮遠處，X和Y是遠小于Z的，可以忽略為零，而且在該坐标的表達方式上，w=0，也表示該點位于Z的無窮遠處。

在十字路口在相機的2D投影中，可以看到無窮遠處的路兩邊是相交到一點了，這就是滅點，其中Zv對應Z_inf點

這裡旋轉矩陣R=[r1,r2,r3]，内參矩陣為K。

根據相機成像原理有

z*Vz = K[r1, r2, r3 | t]Z_inf, 即 z*Vz = K[r1, r2, r3 | t][0, 0, 1, 0]T，如下

化簡可得

z*Vz = Kr3, ==> r3 = (k逆）Vz / ||(k逆）Vz||，如下

利用沿着z方向的滅點可以求出旋轉矩陣的r3，同理，利用沿着x方向的滅點可以求出旋轉矩陣的r1，沿着y方向的滅點可以求出旋轉矩陣的r2，但是在求旋轉矩陣的時候隻需要知道兩個方向的r即可，第三個方向上的r可以通過已知兩個方向上的r叉乘得到

如下

利用滅點求解位姿步驟

1. 準備一張比較好的圖檔
2. 分别找出兩組平行線上的兩個點，計算得出滅點
3. 結合相機内參矩陣，計算分别得到兩個方向上的r，并歸一化
4. 兩個互相垂直的r叉乘得到另外一個次元上的r

至此就得到了旋轉矩陣R，但是這裡依然得不到平移向量t，因為僅靠旋轉是沒有辦法得出的

結果

1. 教程上的求解滅點展示

   

2. 自己求解滅點展示

疑惑

使用滅點求解的旋轉矩陣R怎麼和使用arcuo庫函數得到的結果不一樣呐，了解的朋友幫忙指點一下吧，謝謝！！！

代碼

//這裡的cornerReturn是vector<Point2f>類型的，包含四個元素，分為兩組，是兩條垂直直線上的點

    Point2f vanish_y;
    float ky1=, ky2=;
    ky1 = (cornerReturn[].y - cornerReturn[].y)/(cornerReturn[].x - cornerReturn[].x);
    ky2 = (cornerReturn[].y - cornerReturn[].y)/(cornerReturn[].x - cornerReturn[].x);
    cout<<"ky  is "<<ky1<<" "<<ky2<<endl;
    vanish_y.x = (ky1*cornerReturn[].x - cornerReturn[].y - ky2*cornerReturn[].x +cornerReturn[].y)/(ky1 - ky2);
    vanish_y.y = ky1*(vanish_y.x - cornerReturn[].x) + cornerReturn[].y;
    line(frame, cornerReturn[], vanish_y, Scalar(, , ), );
    line(frame, cornerReturn[], vanish_y, Scalar(, , ), );
    cout<<"y vanishing point is "<<vanish_y<<endl;

    //x vanishing point
    Point2f vanish_x;
    float kx1=, kx2=;
    kx1 = (cornerReturn[].y - cornerReturn[].y)/(cornerReturn[].x - cornerReturn[].x);
    kx2 = (cornerReturn[].y - cornerReturn[].y)/(cornerReturn[].x - cornerReturn[].x);
    cout<<"kx  is "<<kx1<<" "<<kx2<<endl;
    vanish_x.x = (kx1*cornerReturn[].x - cornerReturn[].y - kx2*cornerReturn[].x +cornerReturn[].y)/(kx1 - kx2);
    vanish_x.y = kx1*(vanish_x.x - cornerReturn[].x) + cornerReturn[].y;
    line(frame, cornerReturn[], vanish_x, Scalar(, , ), );
    line(frame, cornerReturn[], vanish_x, Scalar(, , ), );
    cout<<"x vanishing point is "<<vanish_x<<endl;

    Mat Vx = (Mat_<float>(, ) << vanish_x.x, vanish_x.y, f);
    Mat Vy = (Mat_<float>(, ) << vanish_y.x, vanish_y.y, f);
    Mat r1 = Mat(,,CV_32F);
    Mat r2 = Mat(,,CV_32F);
    Mat kInv = intrinsic_matrixCommon.inv();
    cout<<"kInv  is "<<kInv<<endl;

    float normX;
    Mat rTemp1 = kInv*Vx;
    normX = abs(rTemp1.at<float>(,)) + abs(rTemp1.at<float>(,)) + abs(rTemp1.at<float>(,));
    cout<<"rTemp1  is "<<rTemp1<<endl<<"normX  is "<<normX<<endl;
    //第一個方向上的r，并歸一化
    r1 = rTemp1/normX;
    cout<<"r1  is "<<r1<<endl;
    cout<<"******************* "<<endl;

    float normY;
    Mat rTemp2 = kInv*Vy;
    normY = abs(rTemp2.at<float>(,)) + abs(rTemp2.at<float>(,)) + abs(rTemp2.at<float>(,));
    cout<<"rTemp2  is "<<rTemp2<<endl<<"normY  is "<<normY<<endl;
    //第二個方向上的r，并歸一化
    r2 = rTemp2/normY;
    cout<<"r2  is "<<r2<<endl;

    Mat r3 = Mat(,,CV_32F);
    //叉乘得到第三個方向上的r
    r3 = r1.cross(r2);      
    cout<<"r3  is "<<r3<<endl;
    cout<<"******************* "<<endl;

    //這是得到的旋轉矩陣
    Mat rotateMatrix = Mat(,,CV_32F);
    rotateMatrix.at<float>(,) = r1.at<float>(,);//x
    rotateMatrix.at<float>(,) = r1.at<float>(,);
    rotateMatrix.at<float>(,) = r1.at<float>(,);
    rotateMatrix.at<float>(,) = r2.at<float>(,);//y
    rotateMatrix.at<float>(,) = r2.at<float>(,);
    rotateMatrix.at<float>(,) = r2.at<float>(,);
    rotateMatrix.at<float>(,) = r3.at<float>(,);//z
    rotateMatrix.at<float>(,) = r3.at<float>(,);
    rotateMatrix.at<float>(,) = r3.at<float>(,);
    cout<<"rotateMatrix  is "<<endl<<rotateMatrix<<endl;