相机标定方法总结
内参外参同时标定
相机模型为
[ u v 1 ] = K R [ I 3 ∣ − W X C ] [ x y z 1 ] = [ H ∣ h ] [ x y z 1 ] \left[\begin{matrix}u\\v\\1\end{matrix}\right] = \mathbf{K}\mathbf{R}\left[\begin{matrix}{\mathbf{I}_3|-_W\mathbf{X}_C}\end{matrix}\right]\left[\begin{matrix}x\\y\\z\\1\end{matrix}\right] = \left[\begin{matrix}{\mathbf{H}|\mathbf{h}}\end{matrix}\right]\left[\begin{matrix}x\\y\\z\\1\end{matrix}\right] ⎣⎡uv1⎦⎤=KR[I3∣−WXC]⎣⎢⎢⎡xyz1⎦⎥⎥⎤=[H∣h]⎣⎢⎢⎡xyz1⎦⎥⎥⎤
其中 W X C _W\mathbf{X}_C WXC为相机在世界系内的坐标。解出矩阵 [ H ∣ h ] \left[\begin{matrix}{\mathbf{H}|\mathbf{h}}\end{matrix}\right] [H∣h]后，由 H − 1 = R T K − 1 \mathbf{H}^{-1} = \mathbf{R}^T\mathbf{K}^{-1} H−1=RTK−1对 H − 1 \mathbf{H}^{-1} H−1进行QR分解，即可解出 R \mathbf{R} R和 K \mathbf{K} K。 W X C = − H − 1 h _W\mathbf{X}_C = -\mathbf{H}^{-1}\mathbf{h} WXC=−H−1h。
矩阵 [ H ∣ h ] \left[\begin{matrix}{\mathbf{H}|\mathbf{h}}\end{matrix}\right] [H∣h]共有11个自由度，至少需要6个点，且不在同一平面上。
相机内参标定
取同一平面上的点，假设 z = 0 z = 0 z=0，则有
[ u v 1 ] = [ c s x H 0 c ( 1 + m ) y H 0 0 1 ] [ r 11 r 12 t 1 r 21 r 22 t 2 r 31 r 32 t 3 ] [ x y 1 ] = H [ x y 1 ] \left[\begin{matrix}u\\v\\1\end{matrix}\right] = \left[\begin{matrix}c & s & x_H\\0 & c(1+m) & y_H\\0&0&1\end{matrix}\right]\left[\begin{matrix}r_{11} & r_{12} & t_1\\r_{21} & r_{22} & t_2\\r_{31}&r_{32}&t_3\end{matrix}\right]\left[\begin{matrix}x\\y\\1\end{matrix}\right] = \mathbf{H}\left[\begin{matrix}x\\y\\1\end{matrix}\right] ⎣⎡uv1⎦⎤=⎣⎡c00sc(1+m)0xHyH1⎦⎤⎣⎡r11r21r31r12r22r32t1t2t3⎦⎤⎣⎡xy1⎦⎤=H⎣⎡xy1⎦⎤
由于 H \mathbf{H} H有8个自由度，需用4个点解出 H \mathbf{H} H。
记 r 1 = [ r 11 r 21 r 31 ] \mathbf{r}_1 = \left[\begin{matrix}r_{11}\\r_{21}\\r_{31}\end{matrix}\right] r1=⎣⎡r11r21r31⎦⎤， h 1 = [ h 11 h 21 h 31 ] \mathbf{h}_1 = \left[\begin{matrix}h_{11}\\h_{21}\\h_{31}\end{matrix}\right] h1=⎣⎡h11h21h31⎦⎤。有 r 1 = K − 1 h 1 \mathbf{r}_1= \mathbf{K}^{-1}\mathbf{h_1} r1=K−1h1， r 2 = K − 1 h 2 \mathbf{r}_2= \mathbf{K}^{-1}\mathbf{h_2} r2=K−1h2
r 1 T r 2 = h 1 T K − T K − 1 h 2 = 0 \mathbf{r}_1^T\mathbf{r}_2 = \mathbf{h_1}^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h_2}=0 r1Tr2=h1TK−TK−1h2=0
r 1 T r 1 = h 1 T K − T K − 1 h 1 = h 2 T K − T K − 1 h 2 = r 2 T r 2 \mathbf{r}_1^T\mathbf{r}_1 = \mathbf{h_1}^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h_1}= \mathbf{h_2}^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h_2}=\mathbf{r}_2^T\mathbf{r}_2 r1Tr1=h1TK−TK−1h1=h2TK−TK−1h2=r2Tr2
令 B : = K − T K − 1 \mathbf{B}:=\mathbf{K}^{-T}\mathbf{K}^{-1} B:=K−TK−1，在解出矩阵 B \mathbf{B} B后可由Cholesky分解解出矩阵 K \mathbf{K} K。记 b = ( b 11 b 12 b 13 b 21 b 22 b 23 ) \mathbf{b} = \left(\begin{matrix}b_{11}&b_{12}&b_{13}&b_{21}&b_{22}&b_{23}\end{matrix}\right) b=(b11b12b13b21b22b23)， h i B h j = v i j T b \mathbf{h}_i\mathbf{B}\mathbf{h}_j = \mathbf{v}_{ij}^T\mathbf{b} hiBhj=vijTb。
对于每一个平面，有 v 12 T b = 0 \mathbf{v}_{12}^T\mathbf{b} = 0 v12Tb=0和 ( v 11 T − v 22 T ) b = 0 (\mathbf{v}_{11}^T - \mathbf{v}_{22}^T)\mathbf{b} = 0 (v11T−v22T)b=0两个方程，由3个平面即可解出 b \mathbf{b} b。
Fundamental Matrix
x ′ F x ′ ′ = 0 \mathbf{x}'\mathbf{F}\mathbf{x}'' = 0 x′Fx′′=0，其中 x ′ , x ′ ′ \mathbf{x}', \mathbf{x}'' x′,x′′为像素坐标。
将 x ′ F x ′ ′ = 0 \mathbf{x}'\mathbf{F}\mathbf{x}'' = 0 x′Fx′′=0转化为 A f = 0 \mathbf{A}\mathbf{f} = 0 Af=0。由8个点即可求解。 f ^ \mathbf{\hat{f}} f^为矩阵 A \mathbf{A} A特征值最小的特征向量。
F ^ = reshape ( f ^ , ( 3 , 3 ) ) \mathbf{\hat{F}} = \text{reshape}(\mathbf{\hat{f}}, (3,3)) F^=reshape(f^,(3,3))
由于 F ^ \mathbf{\hat{F}} F^需保证秩为2，故对 F ^ \mathbf{\hat{F}} F^在作SVD分解， F ^ = U D V T \mathbf{\hat{F}} = \mathbf{U}\mathbf{D}\mathbf{V}^T F^=UDVT，
F = U diag ( D 11 , D 22 , 0 ) V T \mathbf{F} = \mathbf{U}\text{diag}(D_{11}, D_{22},0)\mathbf{V}^T F=Udiag(D11,D22,0)VT
Essential Matrix
k x ′ E k x ′ ′ = 0 ^k\mathbf{x}'\mathbf{E}^k\mathbf{x}'' = 0 kx′Ekx′′=0，其中 k x ′ , k x ′ ′ ^k\mathbf{x}', ^k\mathbf{x}'' kx′,kx′′为关键点在相机系下的坐标。
方法一
与解Fundamental Matrix同理， E = U diag ( 1 , 1 , 0 ) V T \mathbf{E} = \mathbf{U}\text{diag}(1, 1,0)\mathbf{V}^T E=Udiag(1,1,0)VT
方法二（五点法）
E = S b R T \mathbf{E} = \mathbf{S}_b\mathbf{R}^T E=SbRT，共有5个自由度（位姿2，角度3）。
求解时用到 det E = 0 \det{\mathbf{E}} = 0 detE=0以及 E E T E − 1 2 trace ( E E T ) E = 0 \mathbf{E}\mathbf{E}^T\mathbf{E} - \frac{1}{2}\text{trace}(\mathbf{E}\mathbf{E}^T)\mathbf{E} = 0 EETE−21trace(EET)E=0两条性质。
IMU和相机相对位置标定
工具：Kalibr
总体思路：最小化加速度/角速度/关键点投影的估计值和实际测量值的差值