摄像机标定---2
1.2.2 步骤二 求解外参初始值
上一步已经得到内参的初值，这里考虑无畸变模型：
Z c ⋅ [ x c y c 1 ] = Z c ⋅ K − 1 ⋅ [ u v 1 ] = [ r 1 , r 2 , t ] ⏟ H 3 × 3 ⋅ [ X W Y W 1 ] Z_c \cdot \begin{bmatrix} x_c \\ y_c \\ 1 \\ \end{bmatrix} = Z_c \cdot \mathbf{K}^{-1} \cdot \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \underbrace{ \begin{bmatrix} \mathbf{r}_1, \mathbf{r}_2,\mathbf{t} \end{bmatrix} }_{\displaystyle \mathbf{H}_{3 \times 3}} \cdot \begin{bmatrix} X_W \\ Y_W \\ 1 \\ \end{bmatrix} Zc⋅⎣ ⎡xcyc1⎦ ⎤=Zc⋅K−1⋅⎣ ⎡uv1⎦ ⎤=H3×3 [r1,r2,t]⋅⎣ ⎡XWYW1⎦ ⎤
1.2.2.1 求解单应性矩阵
H \mathbf{H} H的求解参照步骤一的求解方法。
1.2.2.2 求解外参的初值
H = [ h 1 , h 2 , h 3 ] R = [ r 1 , r 2 , r 3 ] \mathbf{H} = [\mathbf{h}_1,\mathbf{h}_2,\mathbf{h}_3] \\ \mathbf{R} = [\mathbf{r}_1,\mathbf{r}_2,\mathbf{r}_3] \\ H=[h1,h2,h3]R=[r1,r2,r3]
于是
r 1 = h 1 ∣ ∣ h 1 ∣ ∣ r 2 = h 2 ∣ ∣ h 2 ∣ ∣ r 3 = r 1 × r 2 t = h 3 ( ∣ ∣ h 1 ∣ ∣ + ∣ ∣ h 2 ∣ ∣ ) / 2 \mathbf{r}_1 = \frac{\mathbf{h}_1}{||\mathbf{h}_1||} \\ \mathbf{r}_2 = \frac{\mathbf{h}_2}{||\mathbf{h}_2||} \\ \mathbf{r}_3 = \mathbf{r}_1 \times \mathbf{r}_2 \\ \mathbf{t} = \frac{\mathbf{h}_3}{(||\mathbf{h}_1||+||\mathbf{h}_2||)/2} r1=∣∣h1∣∣h1r2=∣∣h2∣∣h2r3=r1×r2t=(∣∣h1∣∣+∣∣h2∣∣)/2h3
由上一步得到的 R \mathbf{R} R,可利用Rodrigues公式，先转换到旋转向量，再转换回旋转矩阵的方式去规范化 R \mathbf{R} R的初值。
1.2.2.3 代码实现时的规范化操作
在代码实现时，会对物点坐标做数值上的规范化
[ X ^ W Y ^ W 0 ] = [ X W Y W 0 ] − [ X W ‾ Y W ‾ 0 ] \begin{bmatrix} \hat{X}_W \\ \hat{Y}_W \\ 0 \\ \end{bmatrix} = \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} - \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix} ⎣ ⎡X^WY^W0⎦ ⎤=⎣ ⎡XWYW0⎦ ⎤−⎣ ⎡XWYW0⎦ ⎤
相当于对标定物的世界坐标系做了平移，平移向量为 [ X W ‾ , Y W ‾ , 0 ] T [\overline{X_W},\overline{Y_W},0]^T [XW,YW,0]T，这样
Z c ⋅ [ x c y c 1 ] = Z c ⋅ K − 1 ⋅ [ u v 1 ] = [ r ^ 1 , r ^ 2 , t ^ ] ⏟ H ^ 3 × 3 ⋅ [ X ^ W Y ^ W 1 ] Z_c \cdot \begin{bmatrix} x_c \\ y_c \\ 1 \\ \end{bmatrix} = Z_c \cdot \mathbf{K}^{-1} \cdot \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \underbrace{ \begin{bmatrix} \hat{\mathbf{r}}_1, \hat{\mathbf{r}}_2,\hat{\mathbf{t}} \end{bmatrix} }_{\displaystyle \hat{\mathbf{H}}_{3 \times 3}} \cdot \begin{bmatrix} \hat{X}_W \\ \hat{Y}_W \\ 1 \\ \end{bmatrix} Zc⋅⎣ ⎡xcyc1⎦ ⎤=Zc⋅K−1⋅⎣ ⎡uv1⎦ ⎤=H^3×3 [r^1,r^2,t^]⋅⎣ ⎡X^WY^W1⎦ ⎤
按照之前的解法，解得 H ^ , R ^ , t ^ \hat{\mathbf{H}},\hat{\mathbf{R}},\hat{\mathbf{t}} H^,R^,t^。由物点在摄像机坐标系下的坐标不变，可得
[ X c Y c Z c ] = R ⋅ [ X W Y W 0 ] + t = R ^ ⋅ [ X ^ W Y ^ W 0 ] + t ^ = R ^ ⋅ ( [ X W Y W 0 ] − [ X W ‾ Y W ‾ 0 ] ) + t ^ = R ^ ⋅ [ X W Y W 0 ] + ( − R ^ ⋅ [ X W ‾ Y W ‾ 0 ] + t ^ ) \begin{align} \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ \end{bmatrix} &= \mathbf{R} \cdot \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} + \mathbf{t} \\ &= \hat{\mathbf{R}} \cdot \begin{bmatrix} \hat{X}_W \\ \hat{Y}_W \\ 0 \\ \end{bmatrix} + \hat{\mathbf{t}} \\ &= \hat{\mathbf{R}} \cdot \left( \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} - \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix} \right) + \hat{\mathbf{t}} \\ & = \hat{\mathbf{R}} \cdot \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} + \left( - \hat{\mathbf{R}} \cdot \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix} + \hat{\mathbf{t}} \right) \end{align} ⎣ ⎡XcYcZc⎦ ⎤=R⋅⎣ ⎡XWYW0⎦ ⎤+t=R^⋅⎣ ⎡X^WY^W0⎦ ⎤+t^=R^⋅⎝ ⎛⎣ ⎡XWYW0⎦ ⎤−⎣ ⎡XWYW0⎦ ⎤⎠ ⎞+t^=R^⋅⎣ ⎡XWYW0⎦ ⎤+⎝ ⎛−R^⋅⎣ ⎡XWYW0⎦ ⎤+t^⎠ ⎞
于是
R = R ^ t = − R ^ ⋅ [ X W ‾ Y W ‾ 0 ] + t ^ \mathbf{R} = \hat{\mathbf{R}} \\ \mathbf{t} = - \hat{\mathbf{R}} \cdot \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix} + \hat{\mathbf{t}} R=R^t=−R^⋅⎣ ⎡XWYW0⎦ ⎤+t^
1.2.3 步骤三 LM算法求解内参、外参、畸变参数的最优解
前两步已经得到了所有的待解参数的初始值，这一步基于重投影误差最小化准则，使用LM算法去迭代求解所有待解参数的最优解。至此，标定结束。