立体视觉入门指南（4）：相机标定之DLT直接线性变换【建议收藏】

亲爱的同学们，我们的世界是3D世界，我们的双眼能够观测三维信息，帮助我们感知距离，导航避障，从而翱翔于天地之间。而当今世界是智能化的世界，我们的科学家们探索各种机器智能技术，让机器能够拥有人类的三维感知能力，并希望在速度和精度上超越人类，比如自动驾驶导航中的定位导航，无人机的自动避障，测量仪中的三维扫描等，都是高智机器智能技术在3D视觉上的具体实现。

立体视觉是三维重建领域的重要方向，它模拟人眼结构用双相机模拟双目，以透视投影、三角测量为基础，通过逻辑复杂的同名点搜索算法，恢复场景中的三维信息。它的应用十分之广泛，自动驾驶、导航避障、文物重建、人脸识别等诸多高科技应用都有它关键的身影。

本课程将带大家由浅入深的了解立体视觉的理论与实践知识。我们会从坐标系讲到相机标定，从被动式立体讲到主动式立体，甚至可能从深度恢复讲到网格构建与处理，感兴趣的同学们，来和我一起探索立体视觉的魅力吧！

本课程是电子资源，所以行文并不会有太多条条框框的约束，但会以逻辑清晰、浅显易懂为目标，水平有限，若有不足之处，还请不吝赐教！
个人微信：EthanYs6，加我申请进技术交流群 StereoV3D，一起技术畅聊。
CSDN搜索：Ethan Li 李迎松，查看网页版课程。
随课代码，将择日上传至github上，地址：StereoV3DCode：https://github.com/ethan-li-coding/StereoV3DCode

同学们好，在上一篇立体视觉入门指南（3）：相机标定之张式标定法中，我们接触了迄今为止可能是最为出名应用最为广泛的相机标定方法：Zhang式标定法。但实际上，并不止其一种标定方法被广泛使用，还有另外一种使用也非常广泛的标定方法，称为：直接线性变换法（Direct Linear Transform，DLT）。

让我们首先回顾下，通过投影矩阵MMM建立的世界坐标系PwP_wPw与图像坐标系ppp之间的关系式：
λp=K[Rt][Pw1]=M[Pw1],M=K[Rt]\lambda p=K\left[\begin{matrix}R&t\end{matrix}\right]\left[\begin{matrix}P_w\\1\end{matrix}\right]=M\left[\begin{matrix}P_w\\1\end{matrix}\right],M=K\left[\begin{matrix}R&t\end{matrix}\right]λp=K[Rt][Pw1]=M[Pw1],M=K[Rt]

相机标定的目的是确定相机的内参矩阵KKK以及外参矩阵R,tR,tR,t（可选）。而投影矩阵刚好由内外参唯一确定M=K[Rt]M=K\left[\begin{matrix}R&t\end{matrix}\right]M=K[Rt]，所以能否先计算投影矩阵，再通过投影矩阵计算内外参呢？确实可以，这就是我们今天要介绍的直接线性变换法的思路：

通过大量已知量(p,Pw)(p,P_w)(p,Pw)，代入关系式λp=M[Pw1]\lambda p=M\left[\begin{matrix}P_w\\1\end{matrix}\right]λp=M[Pw1]计算投影矩阵MMM。
分解矩阵MMM得到K,R,tK,R,tK,R,t。

第一步我们可以通过线性方程组的求解来计算MMM，而第二步，则采用矩阵的QR分解来实现。

本篇第二节开始我们将聊一聊具体的实现细节，第一节首先来看下DLT直接线性变换法到底在什么场景下有其用武之地。

文章目录

DLT直接线性变换法的应用场景
DLT实现细节1：解线性方程组求解MMM
DLT实现细节2：QR分解得到K,R,TK,R,TK,R,T
DLT实现细节3：非线性迭代优化
DLT实施
作业

DLT直接线性变换法的应用场景

有同学想必有疑问，既然有了非常方便操作的张式标定法，为什么我们还需要直接线性变换法呢？

张式标定法虽然方便，但是有一个明显的缺点就是必须要求标定板是平面，精度要求越高的应用，就对标定板的平面度有更高的要求。当测量距离很小时，比如在30cm左右，相机的FOV也在数十厘米的范围，我们可以制作一块小型标定板，比如20cm*20cm，并在工艺上提出较高要求，比如板子的平面度在微米级。但是当测量距离很大时，比如在30米，且FOV在有在数十米的范围，这时候你再要求做一块高精度的超大平面，就非常不现实了。

所以，我们不会再采用张式标定法，而是使用大型的三维标定场，就像下图这样：

标定场中布设有大量已知世界坐标Pw(Xw,Yw,Zw)P_w(X_w,Y_w,Z_w)Pw(Xw,Yw,Zw)的控制点，用需要标定的相机拍摄控制场并提取控制点的图像坐标p(u,v)p(u,v)p(u,v)，得到大量观测值(pi,Pwi)(p_i,P_{wi})(pi,Pwi)，即可通过DLT直接线性变换法求出投影矩阵MMM，进而分解得到内外参K,R,tK,R,tK,R,t。

DLT实现细节1：解线性方程组求解MMM

投影关系式 λp=M[Pw1]\lambda p=M\left[\begin{matrix}P_w\\1\end{matrix}\right]λp=M[Pw1]建立了世界坐标系和图像坐标系之间的关系，它是一个齐次坐标表达式，我们设 x=λp=λ[uv1],X=[Pw1]=(X,Y,Z,1)T\text x=\lambda p=\lambda \left[\begin{matrix}u\\v\\1\end{matrix}\right],\text X=\left[\begin{matrix}P_w\\1\end{matrix}\right]=(X,Y,Z,1)^Tx=λp=λ⎣⎡uv1⎦⎤,X=[Pw1]=(X,Y,Z,1)T，得到
x=MX(1)\text x=M\text X \tag 1x=MX(1)

投影矩阵MMM是一个3×43\times43×4矩阵，不妨设
M=[m11m12m13m14m21m22m23m24m31m32m33m34]=[ATBTCT]M= \left[\begin{matrix}m_{11}&m_{12}&m_{13}&m_{14}\\m_{21}&m_{22}&m_{23}&m_{24}\\m_{31}&m_{32}&m_{33}&m_{34}\end{matrix}\right]=\left[\begin{matrix}A^T\\B^T\\C^T\end{matrix}\right]M=⎣⎡m11m21m31m12m22m32m13m23m33m14m24m34⎦⎤=⎣⎡ATBTCT⎦⎤

其中，A=[m11m12m13m14],B=[m21m22m23m24],C=[m31m32m33m34]A=\left[\begin{matrix}m_{11}&m_{12}&m_{13}&m_{14}\end{matrix}\right],B=\left[\begin{matrix}m_{21}&m_{22}&m_{23}&m_{24}\end{matrix}\right],C=\left[\begin{matrix}m_{31}&m_{32}&m_{33}&m_{34}\end{matrix}\right]A=[m11m12m13m14],B=[m21m22m23m24],C=[m31m32m33m34]。我们重写式（1）可得，

[λuλvλ]=x=MX=[ATBTCT]X=[ATXBTXCTX](2)\left[\begin{matrix}\lambda u\\\lambda v\\\lambda\end{matrix}\right] = \text x=M \text X = \left[\begin{matrix}A^T\\B^T\\C^T\end{matrix}\right]\text X = \left[\begin{matrix}A^T\text X\\B^T\text X\\C^T\text X\end{matrix}\right]\tag 2⎣⎡λuλvλ⎦⎤=x=MX=⎣⎡ATBTCT⎦⎤X=⎣⎡ATXBTXCTX⎦⎤(2)

上式左右均除以第3行，得
u=ATXCTXv=BTXCTX\begin{aligned} u&=\frac {A^T\text X}{C^T\text X}\\ v&=\frac {B^T\text X}{C^T\text X} \end{aligned}uv=CTXATX=CTXBTX

我们写成另外一种形式：
uCTX−ATX=0vCTX−BTX=0\begin{aligned} uC^T\text X-A^T\text X&=0\\ vC^T\text X-B^T\text X&=0 \end{aligned}uCTX−ATXvCTX−BTX=0=0
这是线性方程组，未知量为A,B,CA,B,CA,B,C，一组观测值(u,v,X)(u,v,\text X)(u,v,X)可以列2个线性方程。我们换一种写法，写成关于未知数A,B,CA,B,CA,B,C的线性方程组形式：
−XTA+uXTC=0−XTB+vXTC=0\begin{aligned} -\text X^TA\quad\quad\quad+u\text X^TC&=0\\ -\text X^TB+v\text X^TC&=0 \end{aligned}−XTA+uXTC−XTB+vXTC=0=0

设m=[m1,m2,...m12]T=[ABC]\text m = [m_1,m_2,...m_{12}]^T=\left[\begin{matrix}A\\B\\C\end{matrix}\right]m=[m1,m2,...m12]T=⎣⎡ABC⎦⎤，则上式可以写成
[−XTuXT−XTvXT]m=0\left[\begin{matrix}-\text X^T&\quad&u\text X^T\\\quad&-\text X^T&v\text X^T\end{matrix}\right]\text m=0[−XT−XTuXTvXT]m=0

展开得到，
[−X−Y−Z−10000uXuYuZu0000−X−Y−Z−1vXvYvZv][m11m12m13m14m21m22m23m24m31m32m33m34]=0(3)\left[\begin{matrix}-X&-Y&-Z&-1&0&0&0&0&uX&uY&uZ&u\\0&0&0&0&-X&-Y&-Z&-1&vX&vY&vZ&v\end{matrix}\right]\left[\begin{matrix}m_{11}\\m_{12}\\m_{13}\\m_{14}\\m_{21}\\m_{22}\\m_{23}\\m_{24}\\m_{31}\\m_{32}\\m_{33}\\m_{34}\end{matrix}\right]=0 \tag 3[−X0−Y0−Z0−100−X0−Y0−Z0−1uXvXuYvYuZvZuv]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡m11m12m13m14m21m22m23m24m31m32m33m34⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0(3)

这是一组观测值所列出的方程，如果有多组观测值，我们设为(ui,vi,Xi),i=0,...,N(N≥6)(u_i,v_i,\text X_i),i=0,...,N(N\geq6)(ui,vi,Xi),i=0,...,N(N≥6)，则最终的线性方程组为：
[−X1−Y1−Z1−10000u1X1u1Yiu1Z1u10000−X1−Y1−Z1−1v1X1v1Y1v1Z1v1−X2−Y2−Z2−10000u2X2u2Y2u2Z2u20000−X2−Y2−Z2−1v2X2v2Y2v2Z2v2................................................−Xi−Yi−Zi−10000uiXiuiYiuiZiui0000−Xi−Yi−Zi−1viXiviYiviZivi][m11m12m13m14m21m22m23m24m31m32m33m34]=0\left[\begin{matrix}-X_1&-Y_1&-Z_1&-1&0&0&0&0&u_1X_1&u_1Y_i&u_1Z_1&u_1\\0&0&0&0&-X_1&-Y_1&-Z_1&-1&v_1X_1&v_1Y_1&v_1Z_1&v_1\\ -X_2&-Y_2&-Z_2&-1&0&0&0&0&u_2X_2&u_2Y_2&u_2Z_2&u_2\\0&0&0&0&-X_2&-Y_2&-Z_2&-1&v_2X_2&v_2Y_2&v_2Z_2&v_2\\ .&.&.&.&.&.&.&.&.&.&.&.\\.&.&.&.&.&.&.&.&.&.&.&.\\ .&.&.&.&.&.&.&.&.&.&.&.\\.&.&.&.&.&.&.&.&.&.&.&.\\ -X_i&-Y_i&-Z_i&-1&0&0&0&0&u_iX_i&u_iY_i&u_iZ_i&u_i\\0&0&0&0&-X_i&-Y_i&-Z_i&-1&v_iX_i&v_iY_i&v_iZ_i&v_i \end{matrix}\right]\left[\begin{matrix}m_{11}\\m_{12}\\m_{13}\\m_{14}\\m_{21}\\m_{22}\\m_{23}\\m_{24}\\m_{31}\\m_{32}\\m_{33}\\m_{34}\end{matrix}\right]=0⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−X10−X20....−Xi0−Y10−Y20....−Yi0−Z10−Z20....−Zi0−10−10....−100−X10−X2....0−Xi0−Y10−Y2....0−Yi0−Z10−Z2....0−Zi0−10−1....0−1u1X1v1X1u2X2v2X2....uiXiviXiu1Yiv1Y1u2Y2v2Y2....uiYiviYiu1Z1v1Z1u2Z2v2Z2....uiZiviZiu1v1u2v2....uivi⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡m11m12m13m14m21m22m23m24m31m32m33m34⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0

显然其是一个Ax=0Ax=0Ax=0的线性方程问题，解法请参考【代数之美】线性方程组Ax=0的求解方法。

不得不提的是，若要通过上式求得m\text mm的最小二乘解，有一些约束条件要注意。

观测值的数量要不小于6。
观测值没有严重粗差。
如果所有输入点在一个平面上，则无法解出理想的m\text mm。

对于第3点来说，如果输入点在一个平面上，我们举一个特例，即世界坐标的ZZZ分量为0（其他情况其实可以通过旋转等价于该特例）。则式（3）变成：
[−X−Y0−10000uXuY0u0000−X−Y0−1vXvY0v][m11m12m13m14m21m22m23m24m31m32m33m34]=0(3)\left[\begin{matrix}-X&-Y&\fbox0&-1&0&0&0&0&uX&uY&\fbox0&u\\0&0&0&0&-X&-Y&\fbox0&-1&vX&vY&\fbox0&v\end{matrix}\right]\left[\begin{matrix}m_{11}\\m_{12}\\m_{13}\\m_{14}\\m_{21}\\m_{22}\\m_{23}\\m_{24}\\m_{31}\\m_{32}\\m_{33}\\m_{34}\end{matrix}\right]=0 \tag 3[−X0−Y000−100−X0−Y000−1uXvXuYvY00uv]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡m11m12m13m14m21m22m23m24m31m32m33m34⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0(3)

显然，系数矩阵的第3、7、11列全是0，则对应的m13,m23,m33m_{13},m_{23},m_{33}m13,m23,m33可以任意取值，将存在无数解。

DLT实现细节2：QR分解得到K,R,TK,R,TK,R,T

上一节计算得到投影矩阵M∈R3×4M\in R^{3\times4}M∈R3×4，我们知道投影矩阵和内外参矩阵之间满足关系式：
M=K[R∣t]M=K[R|t]M=K[R∣t]

那么如何进一步得到K,R,tK,R,tK,R,t呢？

首先，我们将等式右边的内参矩阵KKK写到括号里面去：
M=[KR∣Kt]M=[KR|Kt]M=[KR∣Kt]

也就是说，投影矩阵MMM的前3列等于KRKRKR，第4列等于KtKtKt，我们另
M=[H∣h],H∈R3×3,h∈R3×1M=[H|h],H\in R^{3\times3},h\in R^{3\times1}M=[H∣h],H∈R3×3,h∈R3×1

则有
H=KR,h=KtH=KR,h=KtH=KR,h=Kt

所以如果能够通过HHH得到KKK和RRR，再通过hhh和KKK得到ttt，就计算出了内外参矩阵。怎么做呢？

首先看看H=KRH=KRH=KR，是否存在一个矩阵分解，可以直接把HHH分解为KKK和RRR呢？那就要观察下KKK和RRR的矩阵性质了。

想必有的同学已经发现了：KKK是一个上三角矩阵，RRR是一个正交矩阵。

那么有没有一种分解可以分解为一个上三角矩阵和一个正交矩阵的乘积？

好像没有！

但是有一种分解可以分解为一个正交矩阵和上三角矩阵的乘积。

那就是QR分解！

于是我们可以换一种思路，既然H=KRH=KRH=KR，则有
H−1=(KR)−1=R−1K−1=RTK−1H^{-1}=(KR)^{-1}=R^{-1}K^{-1}=R^{T}K^{-1}H−1=(KR)−1=R−1K−1=RTK−1

等式右边正是一个正交矩阵乘以一个上三角矩阵，所以我们可以通过对H−1H^{-1}H−1进行QR分解得到KKK和RRR。

由于KRKRKR是齐次的，计算得到的KKK需要将最后一个元素K33K_{33}K33变成1，即QR分解得到的KKK需要做如下处理：
K←1K33KK\gets \frac {1}{K_{33}}KK←K331K

除此之外，还需要面对的一个关键情形是，分解得到的KKK矩阵对应的内参元素(fx,fy,s)(f_x,f_y,s)(fx,fy,s)有可能是负值，这时需要在求得的结果上施加一个绕z轴旋转180度的旋转矩阵：
K←KR(z,π),R←R(z,π)RK\gets KR(z,\pi),R\gets R(z,\pi)RK←KR(z,π),R←R(z,π)R

其中，
R(z,π)=[−1000−10001]R(z,\pi)=\left[\begin{matrix}-1&0&0\\0&-1&0\\0&0&1\end{matrix}\right]R(z,π)=⎣⎡−1000−10001⎦⎤

这并不会破坏HHH关于K,RK,RK,R的关系式：H=KR=KR(z,π)R(z,π)RH=KR=KR(z,\pi)R(z,\pi)RH=KR=KR(z,π)R(z,π)R。

得到K,RK,RK,R后，即可计算t=K−1ht=K^{-1}ht=K−1h。

DLT实现细节3：非线性迭代优化

实际上前两节已经完整的描述了DLT直接线性变换法进行相机标定的理论基础，但是实际上我们并不会直接采用前两节的结果作为最终结果。有一定相机标定经验的同学都了解，镜头畸变、观测噪声导致直接通过理论公式计算出来的结果是不精确的，同张式标定法一样，我们同样需要通过非线性迭代优化的方式最小化重投影误差以求得更精确的内外参，而DLT法计算的内外参则做为迭代优化的初值。

关于非线性迭代优化部分的描述，请参看上篇博客：立体视觉入门指南（3）：相机标定之张式标定法【超详细值得收藏】的最大似然估计部分。本篇不再赘述。

DLT实施

所以，DLT直接线性变换法的实施步骤可以总结为：

多个角度拍摄已知世界坐标的控制场
（1）如果图片中点数很多，且分布合理，其实一个角度也可以
（2）控制场也可能是三维标定板
提取图像中控制点的像素坐标。（提取精度当然是越高越好）
通过求解线性方程组计算投影矩阵MMM
对投影矩阵MMM进行QR分解得到内外参矩阵K,R,tK,R,tK,R,t
将镜头畸变考虑进来，列出重投影误差函数，非线性迭代优化法计算精确内外参矩阵（初值为3,4的结果）和畸变参数（初值为0）

作业

这里为大家准备了一些练习题，可以通过实践加深理解：

1 给定一组模拟观测值，通过DLT直接线性变换法恢复相机的内外参数。
2 更高阶的是，使用非线性迭代优化得到更精确的内外参数以及镜头畸变。

❤❤❤博主时间有限，代码更新速度比较慢，还望谅解。感兴趣可以关注下本系列的源码https://github.com/ethan-li-coding/StereoV3DCode ，有时间我会进行更新。❤❤❤