相机标定方法机理总结与公式推导（Camera calibration）

关于位姿测量的一些研究成果：

Camera calibration

研究现有相机标定的方法的步骤和机理，探讨主动视觉的标定新方法；

张正友法

1）标定模型的建立
λp=K[r1r2t][XwYw1]\lambda p=K\left[\mathbf{r}_{1} \quad \mathbf{r}_{2} \quad t\right]\left[\begin{array}{c}X^{w} \\ Y^{w} \\ 1\end{array}\right] λp=K[r1r2t]⎣⎡XwYw1⎦⎤

λp=HP^\lambda p=H \hat{P} λp=HP^

H=[h1h2h3h4h5h6h7h8h9]H=\left[\begin{array}{lll}h_{1} & h_{2} & h_{3} \\ h_{4} & h_{5} & h_{6} \\ h_{7} & h_{8} & h_{9}\end{array}\right] H=⎣⎡h1h4h7h2h5h8h3h6h9⎦⎤

2）单映性矩阵H的求解

将H矩阵变换成向量的形式：
h=[h1,h2,h3,h4,h5,h6,h7,h8,h9]Th=\left[h_{1}, h_{2}, h_{3}, h_{4}, h_{5}, h_{6}, h_{7}, h_{8}, h_{9}\right]^{T} h=[h1,h2,h3,h4,h5,h6,h7,h8,h9]T
带入成像模型得到下式子，含有 8 个未知数，因此只需要获取到 4 个及以上的图像特的空间参考点世界坐标，便可以利用最小二乘法求解得到 h ，进而恢复单映性矩阵 H；
[XwYw1000−Xwu−Ywu−u000XwYw1−Xwv−Ywv−v]h=[00]\left[\begin{array}{ccccccccc}X^{w} & Y^{w} & 1 & 0 & 0 & 0 & -X^{w} u & -Y^{w} u & -u \\ 0 & 0 & 0 & X^{w} & Y^{w} & 1 & -X^{w} v & -Y^{w} v & -v\end{array}\right] h=\left[\begin{array}{l}0 \\ 0\end{array}\right] [Xw0Yw0100Xw0Yw01−Xwu−Xwv−Ywu−Ywv−u−v]h=[00]
3）内参数的求解
[h1h2h3]=sK[r1r2t]\left[\begin{array}{lll}\mathbf{h}_{1} & \mathbf{h}_{2} & \mathbf{h}_{3}\end{array}\right]=s K\left[\begin{array}{lll}\mathbf{r}_{1} & \mathbf{r}_{2} & t\end{array}\right] [h1h2h3]=sK[r1r2t]
因为R为正交矩阵：
r1Tr2=0∥r1∥=∥r2∥=1\mathbf{r}_{1}^{T} \mathbf{r}_{2}=0 \quad\left\|\mathbf{r}_{1}\right\|=\left\|\mathbf{r}_{2}\right\|=1 r1Tr2=0∥r1∥=∥r2∥=1
可得到下列关系：
{h1TK−TK−1h2=0h1TK−TK−1h1=h2TK−TK−1h2\left\{\begin{array}{c}\mathbf{h}_{1}^{T} K^{-T} K^{-1} \mathbf{h}_{2}=0 \\ \mathbf{h}_{1}^{T} K^{-T} K^{-1} \mathbf{h}_{1}=\mathbf{h}_{2}^{T} K^{-T} K^{-1} \mathbf{h}_{2}\end{array}\right. {h1TK−TK−1h2=0h1TK−TK−1h1=h2TK−TK−1h2

B=K−TK−1B = K^{-T} K^{-1} B=K−TK−1

B 为一个对称矩阵:
B=[B11B12B13B12B22B23B13B23B33]B=\left[\begin{array}{lll}B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33}\end{array}\right] B=⎣⎡B11B12B13B12B22B23B13B23B33⎦⎤
将B矩阵变成向量形式，然后可得到如下关系：
[V12T(V11−V22)T]b=0\left[\begin{array}{c}V_{12}^{T} \\ \left(V_{11}-V_{22}\right)^{T}\end{array}\right] b=0 [V12T(V11−V22)T]b=0
4）内外参数的优化求解

以上步骤得到的相机参数是从数值意义上得到的（即为数值解），并不具有物理意义。因此，Zhang 的方法接下来利用最大似然估计的方法，从物理意义上进一步优化求解以上参数。对于拍摄的n幅图像（每幅图像上选择 m 个特征点），建立以空间特征点实际投影和理想投影误差最小为目标的代价函数：
K,R,t=arg⁡min⁡(∑i=1n∑j=1m∥pij−p^(K,R,t,Pij)∣2)K, R, t=\arg \min \left(\sum_{i=1}^{n} \sum_{j=1}^{m} \| p_{i j}-\left.\hat{p}\left(K, R, t, P_{i j}\right)\right|^{2}\right) K,R,t=argmin(i=1∑nj=1∑m∥pij−p^(K,R,t,Pij)∣2)
通过将步骤 3）中计算得到的结果作为初值，带上式，利用 Levenberg-Marquardt 迭代算法优化求解得到最终的相机内外参数。

通常情况下，相机的镜头是包含畸变的。因此，在综合考虑畸变和噪声的影响后：
K,R,t,k1,k2,p1,p2=arg⁡min⁡(∑i=1n∑j=1m∥pij−p^(K,R,t,k1,k2,p1,p2,Pij)∣2)K, R, t, k_{1}, k_{2}, p_{1}, p_{2}=\arg \min \left(\sum_{i=1}^{n} \sum_{j=1}^{m} \| p_{i j}-\left.\hat{p}\left(K, R, t, k_{1}, k_{2}, p_{1}, p_{2}, P_{i j}\right)\right|^{2}\right) K,R,t,k1,k2,p1,p2=argmin(i=1∑nj=1∑m∥pij−p^(K,R,t,k1,k2,p1,p2,Pij)∣2)

加入相机二维纯旋转运动（俯仰运动和偏航运动）约束后：

HHH为相机内参数,[R1,T1][R_{1},T_{1}][R1,T1]为初始相机外参数，sss为尺度因子，RTiR_{Ti}RTi为第iii幅图像与初始图像的旋转矩阵，可以通过经纬仪得到；
[sipisi1]=[H001]×[RctTct01]−1×[RTi001]×[RctTct01]×[R1T101]×P\begin{aligned}\left[\begin{array}{c}s_{i} \boldsymbol{p}_{i} \\ s_{i} \\ 1\end{array}\right]=\left[\begin{array}{cc}\boldsymbol{H} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{1}\end{array}\right] \times\left[\begin{array}{cc}\boldsymbol{R}_{\mathrm{ct}} & \boldsymbol{T}_{\mathrm{ct}} \\ \boldsymbol{0} & \boldsymbol{1}\end{array}\right]^{-1} \times\left[\begin{array}{cc}\boldsymbol{R}_{T i} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{1}\end{array}\right] \times \left[\begin{array}{cc}\boldsymbol{R}_{\mathrm{ct}} & \boldsymbol{T}_{\mathrm{ct}} \\ \mathbf{0} & \mathbf{1}\end{array}\right] \times\left[\begin{array}{cc}\boldsymbol{R}_{1} & \boldsymbol{T}_{1} \\ \mathbf{0} & \mathbf{1}\end{array}\right] \times \boldsymbol{P} \end{aligned} ⎣⎡sipisi1⎦⎤=[H001]×[Rct0Tct1]−1×[RTi001]×[Rct0Tct1]×[R10T11]×P

RTi=[cos⁡φi0−sin⁡φi010sin⁡φi0cos⁡φi][1000cos⁡ψisin⁡ψi0−sin⁡ψicos⁡ψi]\boldsymbol{R}_{Ti}=\left[\begin{array}{ccc}\cos \varphi_{i} & 0 & -\sin \varphi_{i} \\ 0 & 1 & 0 \\ \sin \varphi_{i} & 0 & \cos \varphi_{i}\end{array}\right]\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \psi_{i} & \sin \psi_{i} \\ 0 & -\sin \psi_{i} & \cos \psi_{i}\end{array}\right] RTi=⎣⎡cosφi0sinφi010−sinφi0cosφi⎦⎤⎣⎡1000cosψi−sinψi0sinψicosψi⎦⎤

加入相机纯平移运动约束后：

极几何约束：

相机不同位置拍摄同一个点时，这两幅图存在如下约束：
x1T(t×d1x1)=x1Tt×Rd0x0x_{1}^{T}\left(t \times d_{1} x_{1}\right)=x_{1}^{T} t \times R d_{0} x_{0} x1T(t×d1x1)=x1Tt×Rd0x0

同一特征点点变焦距成像模型：

该模型包括 5 个坐标系：世界坐标系OwXwYwZwO_wX_wY_wZ_wOwXwYwZw 、变焦相机基坐标系Oc0Xc0Yc0Zc0{O}_{c0}X_{c0}Y_{c0}Z_{c0}Oc0Xc0Yc0Zc0 、变焦相机动坐标系OcnXcnYcnZcn{O}_{cn}X_{cn}Y_{cn}Z_{cn}OcnXcnYcnZcn、图像物理坐标O1xyO_{1xy}O1xy、图像像素坐标系O0uvO_{0uv}O0uv；

当变焦相机的焦距位于任意位置fn时，物体的三维空间点P通过变焦相机的动坐标系光心OcnO_{cn}Ocn，并映射到变焦相机的像平面上点pcnp_{cn}pcn位置。当变焦相机的焦距位于f0f_0f0时，点P通过变焦相机的基坐标系光心Oc0O_{c0}Oc0，并映射到变焦相机的像平面上点pc0p_{c0}pc0位置。

变焦相机动坐标系OcnXcnYcnZcn{O}_{cn}X_{cn}Y_{cn}Z_{cn}OcnXcnYcnZcn的ZcZ_cZc轴与相机的光轴 Zc0Z_{c0}Zc0重合，它垂直于图像平面并通过图像坐标系的中心，XcnX_{cn}Xcn轴和YcnY_{cn}Ycn轴分别平行于图像坐标系的u轴和v轴，光心到图像平面的距离 OcnO1O_{cn}O_1OcnO1称为变焦相机的实时焦距fnf_nfn，设变焦相机坐标系OcnXcnYcnZcn{O}_{cn}X_{cn}Y_{cn}Z_{cn}OcnXcnYcnZcn中的物体空间点 P 在图像物理坐标系O1xyO_{1xy}O1xy下的成像像点为pcn=[x,y]Tp_{cn}=[x,y]Tpcn=[x,y]T ，则可以得到：
{u=u0+fndxXcnZcnv=v0+fndyYcnZcn\left\{\begin{array}{l}u=u_{0}+\frac{f_{n}}{d x} \frac{X_{c n}}{Z_{c n}} \\ v=v_{0}+\frac{f_{n}}{d y} \frac{Y_{c n}}{Z_{c n}}\end{array}\right. {u=u0+dxfnZcnXcnv=v0+dyfnZcnYcn
根据相机坐标系下空间点和图像点之间的转换关系，相机的内参数矩阵由式中矩阵 M1 表示。当空间变焦相机的焦距位于基准焦距 f0 时，可以得到：
[uv1]=[f0dx0u00f0dyv0001][Xc0Zc0YC0Zc01]=M1[Xc0Zc0Yc0Zc01]\left[\begin{array}{l}u \\ v \\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{f_{0}}{d x} & 0 & u_{0} \\ 0 & \frac{f_{0}}{d y} & v_{0} \\ 0 & 0 & 1\end{array}\right]\left[\begin{array}{c}\frac{X_{c 0}}{Z_{c 0}} \\ \frac{Y_{C 0}}{Z_{c 0}} \\ 1\end{array}\right]=\mathbf{M}_{1}\left[\begin{array}{c}\frac{X_{c 0}}{Z_{c 0}} \\ \frac{Y_{c 0}}{Z_{c 0}} \\ 1\end{array}\right] ⎣⎡uv1⎦⎤=⎣⎡dxf0000dyf00u0v01⎦⎤⎣⎡Zc0Xc0Zc0YC01⎦⎤=M1⎣⎡Zc0Xc0Zc0Yc01⎦⎤
当相机的焦距发生改变，αx=fcndx\alpha_{x}=\frac{f_{c n}}{d x}αx=dxfcn为u轴上的放大系数,αy=fcndy\alpha_{y}=\frac{f_{c n}}{d y}αy=dyfcn为v轴上的放大系数，相机的外参数矩阵用M2M_2M2表示，TwenT_{wen}Twen为3×1平移矩阵，RwenR_{wen}Rwen为3×3为正交旋转矩阵,则三维空间点与二维图像点之间的映射关系可以通过下式进行描述：
Zcn[uv1]=[αx0u00αyv000010][Xw01][RwenTwenYwZw1]=M1M2[XwYwZw1]=M[XwYwZw1]Z_{c n}\left[\begin{array}{l}u \\ v \\ 1\end{array}\right]=\left[\begin{array}{cccc}\alpha_{x} & 0 & u_{0} & \\ 0 & \alpha_{y} & v_{0} & 0 \\ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{cc}X_{w} & \\ 0 & 1\end{array}\right]\left[\begin{array}{c}\mathbf{R}_{\mathrm{wen}} \mathbf{T}_{\mathrm{wen}} \\ Y_{w} \\ Z_{w} \\ 1\end{array}\right]=\mathbf{M}_{1} \mathbf{M}_{2}\left[\begin{array}{c}X_{w} \\ Y_{w} \\ Z_{w} \\ 1\end{array}\right]=\mathbf{M}\left[\begin{array}{c}X_{w} \\ Y_{w} \\ Z_{w} \\ 1\end{array}\right] Zcn⎣⎡uv1⎦⎤=⎣⎡αx000αy0u0v0100⎦⎤[Xw01]⎣⎢⎢⎡RwenTwenYwZw1⎦⎥⎥⎤=M1M2⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤=M⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤