极线校正原理

设左相机坐标系为世界坐标系，右相机外参为R,T，则左相机坐标系中的点P1，在右相机坐标系中为：P2=RP1+T           (1)

极线校正的第一步，是将两个相机各旋转R的一半，令两相机的成像平面平行

首先对R进行罗德里格斯变换，得到旋转向量θn，其中θ为旋转角，n为旋转轴单位向量

对旋转向量θ/2*n做罗德里格斯逆变换，得到旋转矩阵r，r具有性质：r*r=R。

则令左相机旋转r，右相机旋转,可令左右相机图像平面平行，

旋转后点的坐标为：,

代入（1） 可得：  （2） 令 为旋转后的平移向量

下一步需要让两相机图像的极线水平，即x轴与t重合，则：

首先计算t与x轴的旋转轴：对t与单位向量（1,0,0）做叉乘得到旋转轴w，其模为t的模与需要旋转角α的正弦：|t|*sinα

下一步计算旋转角：t在x轴上的分量，即为其x轴坐标c，则旋转角α=acos（c/|t|）

则因此对w的长度进行调整：让其等于α，需乘acos（c/|t|）/ |w|

对缩放后的w进行罗德格里斯逆变换得到旋转矩阵Rw

对左右相机都旋转Rw，', ',代入（2）可得：

最终得到左右相机的旋转矩阵：

此时两相机图像平面平行，且极线与基线平行

接下来调整相机内参：

由于两相机焦距不同，要实现行对齐，需要令fy相等，因此令fx=fy=(fy1+fy2)/2=f

即对两平面前后移动至对齐。

接下来计算主点的位置：

设图像的尺寸为（w×h）

首先将左右图像的四个角的坐标（0,0），（w ，0）(0，h)  (w ，h) 根据内参矩阵和畸变系数，计算出归一化坐标，即将其坐标从图像坐标系转换到相机坐标系中，再投影到z=1的平面上，再将其投影到矫正后的z=f,（cx,cy）=(0,0)的图像平面上

，取图像上四点坐标的平均（x，y）为图像的中心（h/2,w/2），原点仍在左上角,则主点坐标为（w/2-u,h/2-v）。该步骤对左右相机均适用。

即（x,y)->((nx - 1) / 2,(ny - 1) / 2),(0,0)->(cx,cy),cx=(nx - 1) / 2-x,cy=(ny - 1) / 2-y

由此可以求得调整后的相机内参，fx，fy，cx，cy