【筆記】C++版Andrea Fusiello立體視覺極線矯正算法

之前在做立體匹配的時候，一直使用opencv3的stereoRectify函數，不過後來出了點問題，也一直沒法找到原因，最後決定閱讀經典論文Fusiello A, Trucco E, Verri A. A compact algorithm for rectification of stereo pairs[J]. Machine Vision & Applications, 2000, 12(1):16-22.
使用該論文裏所介紹的極線矯正算法，該論文還提供了相應的Matlab代碼，由於編程需要使用C++，所以在此記錄下其C++代碼

變量說明

根據相機針孔投影模型

如圖所示，C$C$ 爲相機中心 ， W$W$ 是世界座標系下的空間點，M$M$ 是W$W$ 在圖像平面上經過針孔投影后所得到的點（像點）
現有W$W$ 的齊次座標爲w=[xyz1]T$w={\left[\begin{array}{cccc}x& y& z& 1\end{array}\right]}^T$ 像點M$M$ 的齊次座標爲m=[uv1]T$m={\left[\begin{array}{ccc}u& v& 1\end{array}\right]}^T$ ， 則可以定義P$P$ 爲投影矩陣P=A[R|t]$P=A[R|t]$ , A$A$ 是相機內參數矩陣，[R|t]$[R|t]$ 是相機外參數矩陣，則投影變換關係如下：
λm=Pw$\lambda m=Pw$
其中A$A$ 是相機內參數矩陣

A=⎡⎣⎢⎢⎢fx00γfy0u0v01⎤⎦⎥⎥⎥$$A=\left[\begin{array}{ccc}{f}_x& \gamma & u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]$$

矩陣P=A[R|t]=[AR|At]=[Q|q]$P=A[R|t]=[AR|At]=[Q|q]$
若定義相機中心爲C$C$ ，C$C$ 的齊次座標爲c$c$ ，則有c=−Q−1q=−R−1t$c=-Q^{-1}q=-R^{-1}t$
若定義λ$\lambda$ 爲深度值，則有世界座標與像點座標的關係爲w=c+λQ−1m$w=c+\lambda Q^{-1}m$ ，該式子可以理解爲從相機中心出發，沿着相機中心至m$m$ 像點的射線上距離λ$\lambda$ 處的某一點即爲物方空間點W$W$

極線矯正

首先，在進行極限矯正之前，先確定我們的已知條件：①相機已經過標定，相機內參數矩陣A1$A_1$ 與A2$A_2$ 是已知的。②原相機的外方位元素是已知的，即我們已知相機的投影矩陣Po1$P_{o1}$ 與Po2$P_{o2}$ 。已知以上 條件後，我們即將下圖進行極線矯正

如上圖可以知道，經過極線矯正後，相機中心座標並未發生改變，即①矯正後的新的相機中心座標c1,c2$c_1,c_2$ 未發生改變。②矯正後新的左右相機內參數矩陣A$A$ 是相同的（此時的內參數矩陣可任意選取，只要相同就行）。 ③矯正後的左右相機的旋轉方向都是相同的。

因此可以將新的左右相機看作只沿着相機基線X軸平移了某個長度而已，他們的旋轉朝向都是相同的，都假設爲R$R$

因此可以根據以上矯正條件，寫出新的矯正後的新的左右投影矩陣Pn1$P_{n1}$ 與Pn2$P_{n2}$ ,有如下：
Pn1=A[R|−Rc1]$P_{n1}=A[R|-R{c}_1]$
Pn2=A[R|−Rc2]$P_{n2}=A[R|-R{c}_2]$
其中，矯正後的相機內參數矩陣A$A$ 可任意選取，c1,c2$c_1,c_2$ 與矯正前的c1,c2$c_1,c_2$ 相同，因此，需要求解的僅僅只有一個參數R$R$ 而已， 那麼根據以上推論， 可作以下假設：
首先將新投影矩陣中的外方位元素的旋轉矩陣R$R$ 寫爲列向量形式：

R=⎡⎣⎢⎢⎢rT1rT2rT3⎤⎦⎥⎥⎥$$R=\left[\begin{array}{c}{r}_1^T\\ r_2^T\\ r_3^T\end{array}\right]$$

三個分量rT1$r_1^T$ ， rT2$r_2^T$ ， rT3$r_3^T$ 分別對應相機參考座標系的X$X$ , Y$Y$ , Z$Z$ 軸
根據以上推論得到以下三個條件：
①新的X$X$ 軸應該與基線向量baseline平行：即r1=c1−c2||c1−c2||$r_1=\frac{c1-c2}{||c1-c2||}$
②新的Y$Y$ 軸應該與X$X$ 軸垂直，且同時垂直於k$k$ 軸，該k$k$ 軸論文原文裏選擇了矯正前座標系的Z$Z$ 軸的單位向量，因此有：r2=k×r1$r_2=k\times r_1$
③ 新的Z$Z$ 軸 應該同時垂直於新的X$X$ 軸與 Y$Y$ 軸， 因此有r3=r1×r2$r_3=r_1\times r_2$

完成，由此便可得到新的投影矩陣Pn1$P_{n1}$ 與Pn2$P_{n2}$ 了。

下一步，就該計算由矯正前至矯正後的映射矩陣了

由前文知w=c+λQ−1m$w=c+\lambda Q^{-1}m$ ，則有：
w=c1+λoQ−1o1mo1$w=c_1+{\lambda }_o{Q}_{o1}^{-1}{m}_{o1}$
w=c1+λnQ−1n1mn1$w=c_1+{\lambda }_n{Q}_{n1}^{-1}{m}_{n1}$
因此得到矯正後的像點座標與矯正前的像點座標之間的映射關係爲
mn1=λoλnQn1Q−1o1mo1$m_{n1}=\frac{{\lambda }_o}{{\lambda }_n}{Q}_{n1}{Q}_{o1}^{-1}{m}_{o1}$
因此有映射矩陣T1=Qn1Q−1o1$T_1=Q_{n1}{Q}_{o1}^{-1}$
對於右相機同理：
mn2=λoλnQn2Q−1o2mo2$m_{n2}=\frac{{\lambda }_o}{{\lambda }_n}{Q}_{n2}{Q}_{o2}^{-1}{m}_{o2}$
右相機映射矩陣T2=Qn2Q−1o2$T_2=Q_{n2}{Q}_{o2}^{-1}$

C++代碼

原文給出了Matlab代碼，若需要查看原文代碼的可以直接下載原文查看，這裏就是直接將MATLAB的代碼轉化爲了C++的而已，矩陣等運算都是基於OpenCV3的

#include <iostream>
#include <vector>
#include <string>
#include <opencv2/opencv.hpp>

using namespace std;


void art(const cv::Mat &P , cv::Mat &A , cv::Mat &R , cv::Mat &t){

    cv::Mat t_temp;
    cv::decomposeProjectionMatrix(P  ,A , R , t_temp); // 得到的t 是一個四維向量 , 是指相機所在的位置的3D齊次座標， 需要將其化爲非齊次座標並乘以一個-R轉化爲平移向量
    t = cv::Mat::zeros(3 , 1 , CV_64F);

    t.row(0) = t_temp.row(0) / t_temp.row(3);
    t.row(1) = t_temp.row(1) / t_temp.row(3);
    t.row(2) = t_temp.row(2) / t_temp.row(3);
    t = -R * t ;

}

void my_stereoRectify(const cv::Mat &P_o1,
                      const cv::Mat &P_o2,
                      cv::Mat &T1,
                      cv::Mat &T2,
                      cv::Mat &P_n1,
                      cv::Mat &P_n2)
{
    cv::Mat A1 , R1 , t1;
    cv::Mat A2 , R2 , t2;
    art(P_o1 , A1 , R1 , t1);
    art(P_o2 , A2 , R2 , t2);

    // 相機中心c1 , c2
    cv::Mat c1 = cv::Mat::zeros(3 , 1 , CV_64F);
    cv::Mat c2 = cv::Mat::zeros(3 , 1 , CV_64F);
    c1 = - R1.inv() * t1;
    c2 = - R2.inv() * t2;


    cv::Mat v1 = cv::Mat::zeros(3 , 1 , CV_64F);
    cv::Mat v2 = cv::Mat::zeros(3 , 1 , CV_64F);
    cv::Mat v3 = cv::Mat::zeros(3 , 1 , CV_64F);

   // 新的x軸new x axis (baseline , from c1 to c2)
    v1 = c2 - c1 ;
    v2 = R1.row(2).t().cross(v1);
    v3 = v1.cross(v2);

    //新的外方位元素中的R
    cv::Mat R = cv::Mat::eye(3 , 3 , CV_64F);
    R.row(0) = v1.t() / cv::norm(v1);
    R.row(1) = v2.t() / cv::norm(v2);
    R.row(2) = v3.t() / cv::norm(v3);

    // 新的內方位元素
    cv::Mat A_n1 = cv::Mat::eye(3 , 3 , CV_64F);
    cv::Mat A_n2 = cv::Mat::eye(3 , 3 , CV_64F);
    A_n1 = A2;
    A_n1.at<double>(0 , 1) = 0;
    A_n2 = A2;
    A_n2.at<double>(0 , 1) = 0;

    A_n1.at<double>(0 , 2) = A_n1.at<double>(0 , 2);
    A_n1.at<double>(1 , 2) = A_n1.at<double>(1 , 2);
    A_n2.at<double>(0 , 2) = A_n2.at<double>(0 , 2);
    A_n2.at<double>(1 , 2) = A_n2.at<double>(1 , 2);


    // 新的投影矩陣
    cv::Mat t1_new = cv::Mat::zeros(3 , 1 , CV_64F);
    cv::Mat t2_new = cv::Mat::zeros(3 , 1 , CV_64F);
    t1_new = -R * c1;
    t2_new = -R * c2;

    P_n1.create(3 , 4 , CV_64F);
    P_n2.create(3 , 4 , CV_64F);
    R.convertTo(P_n1.colRange(0 , 3) , CV_64F);
    R.convertTo(P_n2.colRange(0 , 3) , CV_64F);


    t1_new.convertTo(P_n1.col(3) , CV_64F);
    t2_new.convertTo(P_n2.col(3) , CV_64F);

    //cout << "A_n1 = " <<  A_n1 << "\nA_n2 = " << A_n2 << endl;

    cout << "P_n1 = " <<  P_n1 << "\nP_n2 = " << P_n2 << endl;
    P_n1 = A_n1 * P_n1;
    P_n2 = A_n2 * P_n2;

    // 立體矯正的變換矩陣
    T1.create(3 , 3 , CV_64F);
    T2.create(3 , 3 , CV_64F);

    T1 = P_n1.colRange(0 , 3) * P_o1.colRange(0 , 3).inv();
    T2 = P_n2.colRange(0 , 3) * P_o2.colRange(0 , 3).inv();
}

測試

本人使用了Middleburry網站的temple數據，temple數據裏明確的給出了每一張相片的內外參數矩陣，可直接將其參數拿來使用
矯正前：

矯正後：

結束