【笔记】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数据里明确的给出了每一张相片的内外参数矩阵，可直接将其参数拿来使用
矫正前：

矫正后：

结束