## 单目标定
### 1. 总体思路
1. 多个角度拍摄标定板，得到多角度图像，利用图像点和标定板点的对应关系可得单应矩阵H；
2. 利用正交阵的列向量正交和模相等得到H和K的关系；
3. 利用多个单应H求出K;
4. 根据H,K，r1,r2,t的关系求出位姿R,t；
5. 最小化重投影误差求K，R，t；
6. 处理失真：添加失真因素，最小化重投影误差求K，R，t，k1,k2,k3,p1,p2(失真系数初值设为0)；
### 2. 求单应矩阵H
相机模型：
$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & \gamma & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12} & r_{13} &t_1 \\ r_{21} & r_{22} & r_{23} & t_2 \\ r_{31} & r_{32} & r_{33} & t_3 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} $$
$$ sm = K \begin{bmatrix} r_1 & r_2 & r_3 & t \end{bmatrix} M $$
设标定板平面与世界坐标系$ Z = 0 $ 平面重合，则
$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} r_1 & r_2 && t \end{bmatrix} \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix} $$
对每一张标定板图像，可以根据对应点计算出相应的单应矩阵H，
$$ H = K \begin{bmatrix} r_1 & r_2 & r_3 & t \end{bmatrix} $$
由于单应矩阵有8个未知数，所以需要4个对应点坐标才能求出单应矩阵，所以，棋盘格最小是3X3(只计算内点)
### 3. 根据单应矩阵求K
根据上一步得到的单应矩阵$ H = \begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix} $ ,有
$$ r_1 = \lambda K^{-1} h_1 $$
$$ r_2 = \lambda K^{-1} h_2 $$
$$ t = \lambda K^{-1} h_3 $$
由于$ \begin{bmatrix} r_1 & r_2 & r_3 \end{bmatrix} $ 为正交阵，所以r1 r2 正交且模长相等：
$$ \begin{cases} h^T_1 K^{-T}K^{-1} h_1 = h^T_2 K^{-T}K^{-1} h_2 \\ h^T_1 K^{-T}K^{-1} h_2 = 0 \end{cases} $$
令 $$ B = K^{-T}K^{-1} $$（B为绝对二次曲线的像）
设 $ B = \begin{bmatrix} B_{11} & B_{12} & B_{13} \\ B_{21} & B_{22} & B_{23} \\ B_{31} & B_{32} & B_{33} \end{bmatrix} $
由于B对称，所以B有6个未知数b。上式线性化可以得出2个关于b的等式。由$ B = K^{-T}K^{-1} $，所以B只有5个自由度。如果K中$\gamma$为0,则由两个单应矩阵即可求出K。（通过B求K：可以直接带入fx，fy，cx，cy，或用过LTL分解）
补充：
由上式得出 $ Vb = 0$ (V:2nx6,b:6x1) ,b 为$V^TV$的最小特征值对应的特征向量；
### 4. 根据H,K，r1,r2,t的关系求出位姿R,t
$ r_3 = r_1 × r_2 $
计算的R不满足正交阵情况下，可使用$R=UDV^T$分解，取$R=UV^T$
补充：
min ||Q-R|| 取矩阵F范数，即
$$ min ||Q-R||_F = min trace[(Q-R)^T(Q-R)] $$
### 5.最小化重投影误差
$$ min ||m - \hat m(K,R,t,M)|| $$
优化量：K,R,t
### 6. 处理失真
$ x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6) $
$y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6)$
$x_{distorted} = x+2p_1xy+p_2(r^2+2x^2)$
$y_{distorted} = y+p_1(r^2+2y^2)+2p_2xy$
其中，$(x,y)^T$ 是由三维空间点坐标投影到归一化坐标的无失真坐标，$(x_{distorted},y_{distorted})^T$ 是畸变之后的归一化坐标，如果内参矩阵乘$(x_{distorted},y_{distorted})^T$得到畸变图像的坐标
图像坐标系关系：
$u_{distorted} = u + (u-u_0)(k_1(x^2+y^2)+k_2(x^2+y^2)^2)$
$v_{distorted} = v+(v-v_0)(k_1(x^2+y^2)+k_2(x^2+y^2)^2)$
(忽略高次，根据张正友标定论文得)
变形为：
$$ \begin{bmatrix} (u-u_0)(x^2+y^2) & (u-u_0)(x^2+y^2)^2 \\ (v-v_0)(x^2+y^2) & (v-v_0)(x^2+y^2)^2 \end{bmatrix} \begin{bmatrix} k_1 \\ k_2 \end{bmatrix} = \begin{bmatrix} u_{distorted} - u \\ v_{distorted} - v \end{bmatrix} $$
即 $ DK = d , k=[k_1,k_2]^T $ ，超定方程求解，线性最小二乘得：
$ K = (D^TD)^{-1}Dd $
然后，最小重投影误差，不包括误差系数。再重复上面的步骤，直到收敛。
以上为方法一。
方法二：
直接通过加上失真系数的重投影误差优化，初始值为0,或由上面得出。
$$ min ||m - \hat m(K,k_1,k_2,R,t,M)|| $$
## 双目标定
Stereo calibration is the process of computing the geometrical relationship between the two cameras in space. In contrast,stereo rectification is the process of “correcting” the individual images so that they appear as if they had been taken by two cameras with row-aligned image planes.
### 1. Stereo Calibration
计算左右相机的相对位姿关系
$ P_r = RP_l + T $
计算方法：
$P_l = R_lP + T_l $
$P_r = R_rP + T_r $
得出：
$ P_r = RP_l + T $
其中，
$ R = R_rR^T_l$
$ T = T_r - RT_l $
显然，左右相机利用单目标定函数cv::calibrateCamera()分别算出$R_r,R_l$,然后计算得出相对旋转。对于由此计算的很多R,T，opencv选择中间值。
然后计算重投影误差，即最小化空间点到左相机和右相机的投影点误差。右相机的位姿由$R_r = RR_l,T_r = T + RT_l $ 计算
The cv::stereoCalibrate() routine then takes the median values for the R and T parameters as the initial approximation of the true solution and runs a robust Levenberg-Marquardt iterative algorithm to find the (local) minimum of the reprojection error of the calibration points or both camera views, and the final solution for R and T is returned.
### 2.Stereo Rectification
opencv 函数： cv::stereoRectify
在得到相对旋转矩阵R(右相机到左相机的旋转)和T之后，需要矫正两相机的图像平面（通过旋转图像平面），以达到共面且行对齐。这里面涉及的变换参数为左右相机的失真系数，旋转矩阵$R_rect$（针对图像平面），矫正和未矫正的相机内参矩阵（$K_rect,K$)
首先处理共面问题，对使两图像平面共面的旋转有很多，这里选择的是，通过对左右相机坐标系各旋转一半：
$r_l = r^T_r$
$R = r_rr^T_l$
这样得到结果是，两相机坐标系朝向相同，原点相差T^'。分析如下：
设对左右相机坐标系的旋转分别为r_l,r_r,有
$r_r = r_l^T$
$R = r_r*r_r$
此时，由于左右相机坐标系已经变了，如果要计算左右相机坐标系的转换关系，或坐标转换关系，需要计算原来的T（即基线向量，方向为右相机中心到左相机中心）在新右相机坐标系下的坐标表示$T'$。
$T = r_r*T'$
$T' = r_r^T*T$
由$P_r = R P_l + T$两边左乘$r^T_r$得，
$r^T_r*P_r = r^T_r * R * P_l +r_r^T*T $
由旋转矩阵坐标转换关系得
$P_r = r_rP'_r$
$P_l = r_lP'_l$
$P'_r = P'_l + T'$
由上式结果可以看出，新的左右相机坐标系的朝向是一样的。
此时的左右相机主轴平行，但是不一定与基线垂直，图像平面不一定与基线平行，即极点不在无穷远处。
因为左右相机坐标系朝向相同，所以针对某一相机坐标系的旋转对另一坐标系仍然能达到相同的效果。现在以右相机坐标系说明：
由于矫正之后相机坐标系基线为X轴，主轴为Z轴。现在右相机坐标系基线为T‘，所以只需要旋转右相机坐标系，使得基线为X轴即可。只需要将右相机坐标系的X轴旋转到基线，如此得到的旋转矩阵R‘即是。
设R’的轴角表示为（vect,angular）
$vect = ((1,0,0)×T‘).norm()$
angular即为（1,0,0）和T’之间的夹角。
注意，根据T‘指向不同，如果T’[0] < 0,则取（-1,0,0）。
由于左右相机坐标系朝向一样，所以满足行对齐。后面做的主轴与基线垂直，目的在于便于根据视差求距离。这个待验证。
### 3.Rectification map
通过cv::initUndistortRectifyMap()计算当前图像到行对齐图像的映射关系。
remap()得到行对齐图像。