（超详细）张正友标定法原理及公式推导
（超详细）张正友标定法原理及公式推导
文章目录
- （超详细）张正友标定法原理及公式推导
-
- 0. 前言
- 1. 带着问题阅读
- 2. 基本公式与符号定义
-
- 2.1 符号表示
- 2.2 模型平面（棋盘格）及其图像之间得单应性矩阵
- 3. 单应性矩阵估计
-
- 3.1 简单介绍
- 3.2 性质
- 3.3 求解
- 3.4 实际优化估计
-
- 3.4.1 求解优化初值
- 3.4.2 极大似然估计
- 4. 相机内参估计
-
- 4.1 相机内参的约束条件
- 4.2 相机内参的封闭解
-
- 4.2.1 B = A − T A − 1 B=A^{-T}A^{-1} B=A−TA−1的具体表达式
- 4.2.2 向量 b b b的求解
- 4.2.3 内参矩阵A的求解
- 5. 外参估计
-
- 5.1 什么是单目相机的外参
- 5.2 单目外参的求解
- 6. 极大似然优化（1）
- 7. 畸变参数估计
-
- 7.1 畸变简介
- 7.2 畸变估计
-
- 7.2.1 假设
- 7.2.2 径向畸变模型
- 7.2.3 k 1 、 k 2 k_{1}、k_{2} k1、k2求解
- 8. 极大似然优化（2）
- 9. 标准旋转矩阵估计
- 10. 总结
-
- 10.1 标定流程
- 10.2 展望
0. 前言
本文记录张正友标定法的原理以及详细推导过程，涉及很多公式推导的细节，适合想要深入理解张正友标定法的同学阅读，相信只要注意细节，一步步耐心推导公式，一定可以理解张正友标定法。
许多博客都介绍了张正友标定法，但是大多都是泛泛而谈，也许作者已经明白甚至编码实现了标定方法，但是真正一步一步推导的文章很少，如果文中的细节刚好解答了你的疑惑，我想这值得一个点赞！
这里我想强调一点：张正友标定法最大的贡献是提出一种计算相机参数优化初值的方法，提高了相机标定参数优化的鲁棒性，因此阅读重点应放在初值的求解上。
1. 带着问题阅读
- 问题一：什么是单应性矩阵？(3.1节)
- 问题二：求解单应性矩阵为什么至少需要四个角点对？(3.3节)
- 问题三：相机内参数怎么求解？（4.2节）
- 问题四：求解相机内参数为什么至少需要3幅棋盘格图像？（4.2节）
- 问题五：每幅图像对应的旋转矩阵和平移矩阵怎么求？（第5节）
- 问题六：畸变参数怎么求？（第7节）
- 问题七：如何获得一个标准得旋转矩阵（单位正交矩阵）？（第9节）
2. 基本公式与符号定义
2.1 符号表示
-
2D像素坐标点 m = [ u v ] T m=\begin{bmatrix} u & v \end{bmatrix}^{T} m=[uv]T、3D世界坐标点 M = [ X Y Z ] T M=\begin{bmatrix} X & Y & Z \end{bmatrix}^{T} M=[XYZ]T
-
齐次坐标： m ~ = [ u v 1 ] T \tilde{m}=\begin{bmatrix} u & v & 1\end{bmatrix}^{T} m~=[uv1]T、 M ~ = [ X Y Z 1 ] T \tilde{M}=\begin{bmatrix} X & Y & Z & 1 \end{bmatrix}^{T} M~=[XYZ1]T
-
根据小孔成像模型（关于小孔成像模型可以参考这篇博文），真实世界三维坐标点M与其在相机图片上的投影点m的关系式为：
- s m ~ = A [ R t ] M ~ = A [ r 1 r 2 r 3 t ] M ~ ( 1 ) s\tilde{m}=A\begin{bmatrix}R& t\end{bmatrix}\tilde{M}=A\begin{bmatrix}r_{1}&r_{2}&r_{3}& t\end{bmatrix}\tilde{M}\qquad \qquad \qquad \qquad \qquad \qquad(1) sm~=A[Rt]M~=A[r1r2r3t]M~(1)
- s是比例因子
- A = [ α γ u 0 0 β v 0 0 0 1 ] A=\begin{bmatrix} \alpha & \gamma & u_{0} \\ 0 & \beta & v_{0} \\ 0 & 0 & 1 \end{bmatrix} A=⎣⎡α00γβ0u0v01⎦⎤，为相机内参数，各参数的意义见上面的推荐博文
- R R R、 t t t分别为旋转矩阵和平移矩阵，统称为相机的外参。
- r i r_{i} ri表示列向量
- s m ~ = A [ R t ] M ~ = A [ r 1 r 2 r 3 t ] M ~ ( 1 ) s\tilde{m}=A\begin{bmatrix}R& t\end{bmatrix}\tilde{M}=A\begin{bmatrix}r_{1}&r_{2}&r_{3}& t\end{bmatrix}\tilde{M}\qquad \qquad \qquad \qquad \qquad \qquad(1) sm~=A[Rt]M~=A[r1r2r3t]M~(1)
这里我强调一点：旋转矩阵R是标准的旋转矩阵，即R为单位正交矩阵，可以用罗德里格斯公式表示，更进一步得讲，有 r i r_{i} ri之间相互正交且模为1。后面会用到这个关键的性质。
- 特殊符号： A − T A^{-T} A−T表示 ( A − 1 ) T (A^{-1})^{T} (A−1)T或者 ( A T ) − 1 (A^{T})^{-1} (AT)−1
这里我想解释一下（1）式，熟悉小孔成像模型的同学知道，齐次坐标系下像素坐标和世界坐标的的完整等式是：
[ u v 1 ] = [ α γ u 0 0 0 β v 0 0 0 0 1 0 ] 1 Z c [ R t 0 T 1 ] [ X w Y w Z w 1 ] \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=\begin{bmatrix} \alpha & \gamma &u_{0} & 0\\ 0 & \beta & v_{0} &0 \\ 0 & 0& 1 & 0\end{bmatrix}\frac{1}{Z_{c}}\begin{bmatrix} R & t \\ 0^{T}& 1 \end{bmatrix}\begin{bmatrix} X_{w} \\Y_{w} \\Z_{w} \\ 1 \end{bmatrix} ⎣⎡uv1⎦⎤=⎣⎡α00γβ0u0v01000⎦⎤Zc1[R0Tt1]⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤如果把 1 Z c \frac{1}{Z_{c}} Zc1当作比例因子s，并且展开得
s [ u v 1 ] = [ α γ u 0 0 0 β v 0 0 0 0 1 0 ] [ r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 ] [ X w Y w Z w 1 ] s\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=\begin{bmatrix} \alpha & \gamma &u_{0} & 0\\ 0 & \beta & v_{0} &0 \\ 0 & 0& 1 & 0\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} & r_{13} &t_{x}\\ r_{21} & r_{22} & r_{23} &t_{y}\\ r_{31} & r_{32} & r_{33} &t_{z}\\ 0&0&0& 1 \end{bmatrix}\begin{bmatrix} X_{w} \\Y_{w} \\Z_{w} \\ 1 \end{bmatrix} s⎣⎡uv1⎦⎤=⎣⎡α00γβ0u0v01000⎦⎤⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330txtytz1⎦⎥⎥⎤⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤，根据矩阵运算规则，你会发现：
[ α γ u 0 0 0 β v 0 0 0 0 1 0 ] [ r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 ] = A [ r 1 r 2 r 3 t ] \begin{bmatrix} \alpha & \gamma &u_{0} & 0\\ 0 & \beta & v_{0} &0 \\ 0 & 0& 1 & 0\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} & r_{13} &t_{x}\\ r_{21} & r_{22} & r_{23} &t_{y}\\ r_{31} & r_{32} & r_{33} &t_{z}\\ 0&0&0& 1 \end{bmatrix}=A\begin{bmatrix}r_{1}&r_{2}&r_{3}& t\end{bmatrix} ⎣⎡α00γβ0u0v01000⎦⎤⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330txtytz1⎦⎥⎥⎤=A[r1r2r3t]，请不熟悉矩阵得同学动手推一下体会体会。
2.2 模型平面（棋盘格）及其图像之间得单应性矩阵
-
假定世界坐标系在棋盘格平面，且棋盘格平面处于Z=0得平面上，如下图所示：
注：这里得模型平面不一定是棋盘格，还可以是原点图和一些其他图案，本文只是以棋盘格平面作为例子。
-
根据以上假定，我们可以得到：
注：后面仍然使用 M ~ \tilde{M} M~表示 [ X Y 1 ] T \begin{bmatrix} X & Y & 1 \end{bmatrix}^{T} [XY1]T进而可以得到：
-
-
单应性矩阵 H H H是一个非奇异3×3矩阵，并且含有比例因子。
- H = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] H=\begin{bmatrix} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{bmatrix} H=⎣⎡h11h21h31h12h22h32h13h23h33⎦⎤
3. 单应性矩阵估计
3.1 简单介绍
- 单应性变换又叫投影变换：应用在平面坐标变换中。
- 单应性矩阵：描述两个平面上的对应点之间的变换关系
- 应用：图像校正、图像拼接、相机位姿估计、视觉SLAM等。
- 定义： H = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] H=\begin{bmatrix} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{bmatrix} H=⎣⎡h11h21h31h12h22h32h13h23h33⎦⎤
- 详细介绍请参考单应性（Homography）变换与单应性矩阵的求解
3.2 性质
- 这里使用的是齐次坐标系，也就是说可以进行任意尺度的缩放（s为尺度因子），也就是说把H乘以任意一个非零常数k并不改变上式结果，无非就是尺度因子s有所改变。
- 比如我们把H乘以 1 h 33 \frac{1}{h_{33}} h331可以得到：
- H ′ = 1 h 33 H = 1 h 33 [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] = [ h 11 ′ h 12 ′ h 13 ′ h 21 ′ h 22 ′ h 23 ′ h 31 ′ h 32 ′ 1 ] H'=\frac{1}{h_{33}}H=\frac{1}{h_{33}}\begin{bmatrix} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{bmatrix}=\begin{bmatrix} h_{11}' & h_{12}' & h_{13}' \\ h_{21}' & h_{22}' & h_{23}' \\ h_{31}' & h_{32}' & 1 \end{bmatrix} H′=h331H=h331⎣⎡h11h21h31h12h22h32h13h23h33⎦⎤=⎣⎡h11′h21′h31′h12′h22′h32′h13′h23′1⎦⎤
- 而上述等式依然成立
- 同理H乘以 1 ∑ i = 1 3 ∑ j = 1 3 h i j 2 \frac{1}{\sqrt{\sum_{i=1}^3\sum_{j=1}^3h_{ij}^{2}}} ∑i=13∑j=13hij21 可以得到约束：
- h 11 ′ 2 + h 12 ′ 2 + h 13 ′ 2 + h 21 ′ 2 + h 22 ′ 2 + h 23 ′ 2 + h 31 ′ 2 + h 32 ′ 2 + h 33 ′ 2 = 1 h_{11}'^{2} + h_{12}'^{2} + h_{13}'^{2} + h_{21}'^{2} + h_{22}'^{2} + h_{23}'^{2} + h_{31}'^{2} + h_{32}'^{2} + h_{33}'^{2}=1 h11′2+h12′2+h13′2+h21′2+h22′2+h23′2+h31′2+h32′2+h33′2=1
- 即 ∑ i = 1 3 ∑ j = 1 3 h i j ′ 2 = 1 \sum_{i=1}^3\sum_{j=1}^3h_{ij}'^{2}=1 ∑i=13∑j=13hij′2=1
- 由此我们可以得出单应性矩阵有8个自由度，并非9个自由度。
- 比如我们把H乘以 1 h 33 \frac{1}{h_{33}} h331可以得到：
3.3 求解
-
根据2.2节中式(2)展开，并且单应性矩阵 H H H隐含比例因子 s s s，2.2节注释中说明仍然使用 M ~ \tilde{M} M~表示 [ X Y 1 ] T \begin{bmatrix} X & Y & 1 \end{bmatrix}^{T} [XY1]T，综上可以得到：
- [ u v 1 ] = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] [ X Y 1 ] \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=\begin{bmatrix} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{bmatrix}\begin{bmatrix} X \\Y \\ 1 \end{bmatrix} ⎣⎡uv1⎦⎤=⎣⎡h11h21h31h12h22h32h13h23h33⎦⎤⎣⎡XY1⎦⎤
-
由上式可得:
- { u = h 11 X + h 12 Y + h 13 h 31 X + h 32 Y + h 33 v = h 21 X + h 22 Y + h 23 h 31 X + h 32 Y + h 33 \left\{\begin{array}{ll} u=\frac{h_{11}X + h_{12}Y + h_{13}}{h_{31}X + h_{32}Y + h_{33}} \\ v=\frac{h_{21}X + h_{22}Y + h_{23}}{h_{31}X + h_{32}Y + h_{33}}\end{array} \right. {u=h31X+h32Y+h33h11X+h12Y+h13v=h31X+h32Y+h33h21X+h22Y+h23，进一步变换得：
- { u ( h 31 X + h 32 Y + h 33 ) = h 11 X + h 12 Y + h 13 v ( h 31 X + h 32 Y + h 33 ) = h 21 X + h 22 Y + h 23 \left\{\begin{array}{ll} u(h_{31}X + h_{32}Y + h_{33})=h_{11}X + h_{12}Y + h_{13} \\ v(h_{31}X + h_{32}Y + h_{33})=h_{21}X + h_{22}Y + h_{23}\end{array} \right. {u(h31X+h32Y+h33)=h11X+h12Y+h13v(h31X+h32Y+h33)=h21X+h22Y+h23，进一步得到：
- { X h 11 + Y h 12 + h 13 − u X h 31 − u Y h 32 − u h 33 = 0 X h 21 + Y h 22 + h 23 − v X h 31 − v Y h 32 − v h 33 = 0 \left\{\begin{array}{ll} Xh_{11} + Yh_{12} + h_{13} - uXh_{31} - uYh_{32} - uh_{33}=0\\ Xh_{21} + Yh_{22} + h_{23} - vXh_{31} - vYh_{32} - vh_{33}=0\end{array} \right. {Xh11+Yh12+h13−uXh31−uYh32−uh33=0Xh21+Yh22+h23−vXh31−vYh32−vh33=0，化成矩阵形式有：
- [ X Y 1 0 0 0 − u X − u Y − u 0 0 0 X Y 1 − v X − v Y − v . . . . . . . . . . . . . . . . . . . . . ] [ h 11 h 12 h 113 h 21 h 22 h 23 h 31 h 32 h 33 ] = 0 \begin{bmatrix} X & Y & 1& 0&0&0& - uX& -uY& -u \\ 0&0&0&X & Y& 1& - vX& -vY& -v \\ ...\\...\\...\\...\\...\\...\\... \end{bmatrix}\begin{bmatrix} h_{11} \\ h_{12} \\ h_{113}\\h_{21}\\h_{22}\\h_{23}\\h_{31}\\h_{32}\\h_{33} \end{bmatrix}=0 ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡X0.....................Y0100X0Y01−uX−vX−uY−vY−u−v⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡h11h12h113h21h22h23h31h32h33⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0，更进一步抽象：
- [ M ~ T 0 ⃗ T − u M ~ T 0 ⃗ T M ~ T − v M ~ T ] h = 0 \begin{bmatrix} \tilde{M}^{T} & \vec{0}^{T} & - u\tilde{M}^{T} \\ \vec{0}^{T} &\tilde{M}^{T} &- v\tilde{M}^{T} \end{bmatrix}h=0 [M~T0T0TM~T−uM~T−vM~T]h=0
- 其中 h = [ h 11 h 12 h 113 h 21 h 22 h 23 h 31 h 32 h 33 ] T h=\begin{bmatrix} h_{11} & h_{12}& h_{113}&h_{21}&h_{22}&h_{23}&h_{31}&h_{32}&h_{33} \end{bmatrix}^{T} h=[h11h12h113h21h22h23h31h32h33]T
- 0 ⃗ T = [ 0 0 0 ] \vec{0}^{T}=\begin{bmatrix} 0&0&0 \end{bmatrix} 0T=[000]
- 我们发现一对点 m ~ \tilde{m} m~ 和 M ~ \tilde{M} M~可以提供两个方程
- 由于单应矩阵H包含了 ∣ ∣ H ∣ ∣ F = ∑ i = 1 3 ∑ j = 1 3 h i j ′ 2 = 1 ||H||_{F}=\sum_{i=1}^3\sum_{j=1}^3h_{ij}'^{2}=1 ∣∣H∣∣F=∑i=13∑j=13hij′2=1或者 h 33 = 1 h_{33}=1 h33=1约束，因此根据上式的线性方程组8自由度的H我们至少需要4对点才能计算出单应矩阵。
这里我们以棋盘格为例，如果我们想要求解出单应性矩阵,那么我们就需要检测至少4个角点，由于棋盘格平面的物理参数我们是知道的(如棋盘格方格的尺寸、数量和排列方式),由此可以得到 M ~ \tilde{M} M~，而 m ~ \tilde{m} m~可以之间从图像中检测到，得到方程组求解得到 H H H，那么根据上面的推导也就解释了问题二
3.4 实际优化估计
以上只是理论推导，在真实的应用场景中，我们计算的点对中都会包含噪声。比如点的位置偏差几个像素，甚至出现特征点对误匹配的现象，如果只使用4个点对来计算单应矩阵，那会出现很大的误差。因此，为了使得计算更精确，一般都会使用远大于4个点对来计算单应矩阵。
3.4.1 求解优化初值
- 假设一张图片检测到 m m m个角点( m > 4 m>4 m>4)，那么我们就可以得到 2 m 2m 2m个方程；
- 我们可以用矩阵的形式表示方程组，设 L L L为 2 m × 9 2m×9 2m×9的系数矩阵， h h h仍为 [ h 11 h 12 h 113 h 21 h 22 h 23 h 31 h 32 h 33 ] T \begin{bmatrix} h_{11} & h_{12}& h_{113}&h_{21}&h_{22}&h_{23}&h_{31}&h_{32}&h_{33} \end{bmatrix}^{T} [h11h12h113h21h22h23h31h32h33]T；
- 那么方程组可以表示为： L h = 0 Lh=0 Lh=0；
- 同样 h h h中包含比例因子
- 求解这样的方程组可以使用奇异值分解法或者与 L T L L^{T}L LTL的最小特征值对应的特征向量法求解。
在实际求解过程中有一点需要注意一下：一般来说像素坐标会以像素（pixel）为单位，而实际的世界坐标以毫米（mm）或者米（m）为单位，这样看来，方程组中存在常数1、以像素为单位的坐标值u、v和以毫米为单位的世界坐标值，以及他们的乘积。这些值的尺度不统一，相差的数量级较大，在数值计算中一般会引起数值不稳定，可以先对方程组进行归一化处理，再进行计算
上述的方程组的解可以为接下来的优化提供一个较好的初值。
3.4.2 极大似然估计
- 一些假设
- 设 M i M_{i} Mi和 m i m_{i} mi为角点的实际物理坐标和像素坐标，其中 i = 1 , . . . , m i=1,...,m i=1,...,m， m m m为角点数量；
- 实际计算中图片的噪声主要为高斯噪声，假设噪声的均值为0，协方差 Λ m i = σ 2 I \Lambda_{m_{i}}=\sigma^{2}I Λmi=σ2I
- 单应性矩阵H的极大似然估计可以通过最小化下面的损失函数获得：
- 其中 m i ^ = 1 h ˉ 3 T M i [ h ˉ 1 T M i h ˉ 2 T M i ] = [ u ^ v ^ ] \hat{m_{i}}=\frac{1}{\bar{h}_{3}^{T}M_{i}}\begin{bmatrix} \bar{h}_{1}^{T}M_{i} \\ \bar{h}_{2}^{T}M_{i} \end{bmatrix}=\begin{bmatrix}\hat{u} \\ \hat{v} \end{bmatrix} mi^=hˉ3TMi1[hˉ1TMihˉ2TMi]=[u^v^]
- h ˉ i \bar{h}_{i} hˉi是 H H H的第 i i i行
简单说明一下： m i ^ \hat{m_{i}} mi^其实是 M i M_{i} Mi通过单应性矩阵变换得到的投影点，而 m i m_{i} mi是角点实际的像素坐标，这里可以将 m i ^ \hat{m_{i}} mi^的表达式展开来验证
- 由上面的假设 Λ m i = σ 2 I \Lambda_{m_{i}}=\sigma^{2}I Λmi=σ2I，那么损失函数就转化为非线性最小二乘估计
也可以类比地把它理解为最小化重投影误差
- 结合3.4.1节求得的初值，可以实施**Levenberg-Marquardt算法（L-M算法）**来获得最优 H H H
4. 相机内参估计
4.1 相机内参的约束条件
- 假设
，由2.2节式（2）可得：
- 式中 λ \lambda λ是任意比例因子， r 1 r_{1} r1和 r 2 r_{2} r2相互正交且模为1（2.1节注释中也提到了）
- 那么讲上式展开可以得到一组关系式：
- { h 1 = λ A r 1 h 2 = λ A r 2 h 3 = λ A t \left\{\begin{array}{ll} h_{1}= \lambda Ar_{1} \\ h_{2}= \lambda Ar_{2} \\ h_{3}= \lambda At\end{array} \right. ⎩⎨⎧h1=λAr1h2=λAr2h3=λAt ，反过来也可以得到
- { r 1 = 1 λ A − 1 h 1 r 2 = 1 λ A − 1 h 2 t = 1 λ A − 1 h 3 \left\{\begin{array}{ll} r_{1}= \frac{1}{\lambda} A^{-1}h_{1} \\ r_{2}= \frac{1}{\lambda} A^{-1}h_{2} \\t= \frac{1}{\lambda} A^{-1}h_{3}\end{array} \right. ⎩⎨⎧r1=λ1A−1h1r2=λ1A−1h2t=λ1A−1h3
- 由旋转矩阵得性质： r 1 T r 2 = 0 、 ∣ ∣ r 1 ∣ ∣ = ∣ ∣ r 2 ∣ ∣ = r 1 T r 1 = r 2 T r 2 = 1 r_{1}^{T}r_{2}=0、||r_{1}||=||r_{2}||=r_{1}^{T}r_{1}=r_{2}^{T}r_{2}=1 r1Tr2=0、∣∣r1∣∣=∣∣r2∣∣=r1Tr1=r2Tr2=1，将 r i r_{i} ri用 h i h_{i} hi表示得到：
- { r 1 T r 2 = [ 1 λ A − 1 h 1 ] T [ 1 λ A − 1 h 1 ] = ( 1 λ ) 2 h 1 T A − T A − 1 h 2 = 0 r 1 T r 1 = r 2 T r 2 = ( 1 λ ) 2 h 1 T A − T A − 1 h 1 = ( 1 λ ) 2 h 2 T A − T A − 1 h 2 = 1 \left\{\begin{array}{ll}r_{1}^{T}r_{2}=[\frac{1}{\lambda} A^{-1}h_{1}]^{T}[\frac{1}{\lambda} A^{-1}h_{1}]=(\frac{1}{\lambda})^{2}h_{1}^{T}A^{-T}A^{-1}h_{2}=0\\r_{1}^{T}r_{1}=r_{2}^{T}r_{2}=(\frac{1}{\lambda})^{2}h_{1}^{T}A^{-T}A^{-1}h_{1}=(\frac{1}{\lambda})^{2}h_{2}^{T}A^{-T}A^{-1}h_{2}=1\end{array} \right. {r1Tr2=[λ1A−1h1]T[λ1A−1h1]=(λ1)2h1TA−TA−1h2=0r1Tr1=r2Tr2=(λ1)2h1TA−TA−1h1=(λ1)2h2TA−TA−1h2=1，将比例因子 ( 1 λ ) 2 (\frac{1}{\lambda})^{2} (λ1)2隐含在等式中可得：
由上面的推导可知，相机内参数有两个基本约束条件即式（3）（4），由于单应性矩阵有8个自由度，并且外参数有6个自由度（旋转3个，平移3个，这句话的理解请参考罗德里格斯公式），那么现在仅仅有两个内参的约束，该怎么求解呢？突破口就在 A − T A − 1 A^{-T}A^{-1} A−TA−1上！ A − T A − 1 A^{-T}A^{-1} A−TA−1本质上描述的是绝对二次曲线的图像。
4.2 相机内参的封闭解
4.2.1 B = A − T A − 1 B=A^{-T}A^{-1} B=A−TA−1的具体表达式
-
在上一节我们提到求解内参矩阵 A A A的关键在于 A − T A − 1 A^{-T}A^{-1} A−TA−1，令 B = A − T A − 1 = [ B 11 B 12 B 13 B 21 B 22 B 23 B 31 B 32 B 33 ] B=A^{-T}A^{-1}=\begin{bmatrix} B_{11} & B_{12} & B_{13} \\ B_{21} & B_{22} & B_{23} \\ B_{31} & B_{32} & B_{33} \end{bmatrix} B=A−TA−1=⎣⎡B11B21B31B12B22B32B13B23B33⎦⎤
-
由 A = [ α γ u 0 0 β v 0 0 0 1 ] A=\begin{bmatrix} \alpha & \gamma & u_{0} \\ 0 & \beta & v_{0} \\ 0 & 0 & 1 \end{bmatrix} A=⎣⎡α00γβ0u0v01⎦⎤根据矩阵的运算，我们可以的到：
-
-
观察式（5）发现 B B B是对称矩阵，那么它可以由一个6D向量 b b b定义：
4.2.2 向量 b b b的求解
-
设 H H H的列向量为 h i = [ h i 1 h i 2 h i 3 ] T h_{i}=\begin{bmatrix} h_{i1}&h_{i2}&h_{i3}\end{bmatrix}^{T} hi=[hi1hi2hi3]T，然后将4.1节中式（3）（4）两个约束写成下面的表达：
-
进一步推导可以将两个约束表达式转化为下面未知数为向量 b b b的齐次方程：
一张图片对应一个单应性矩阵，而由式（8）可以知道一张图片可以提供两个约束，然而 b b b有6个位置参数，因此至少需要6个约束才能求解，也就是说，至少需要拍摄3张图片才能将b求解出来，这里就解释了问题四。
对于上面的式（7）（8），我来详细地推导一下：
- 在实际计算中，假设我们拍摄了n张不同方位的棋盘格图像，那么就有n个式（8）叠加形成：
- 其中 V V V是 2 n × 6 2n×6 2n×6的矩阵。
- 对于拍摄特图像数量 n > 3 n>3 n>3那么我们求解这样的超定方程可以用奇异值分解法，即 V T V V_{T}V VTV的特征向量，与其对应的最小特征值（相当于与最小奇异值相关的 V V V的右奇异向量），具体可以查阅相关资料；
由此我们就求解出来 b b b，需要注意的是，这里的 b b b是带有比例因子的，接下来就是内参的计算。
-
其他几种情况：
- n = 3 n=3 n=3，那么我们可以得到唯一解；
- n = 2 n=2 n=2，我么可以令偏移常数 γ = 0 \gamma=0 γ=0，也就是增加一个 [ 0 1 0 0 0 0 ] b = 0 \begin{bmatrix} 0&1&0&0&0&0\end{bmatrix}b=0 [010000]b=0约束； 3. n = 1 n=1 n=1，那么只能求解出两个相机内参数，比如根据先验知识我们可以知道像素坐标平面的中心点 ( u 0 , v 0 ) (u_0,v_0) (u0,v0)，同时令 γ = 0 \gamma=0 γ=0,那么我们就可以得到 α \alpha α和 β \beta β。
在实际操作中我们一般会获取远多于3张不同方位的棋盘格图片大约10-30张
4.2.3 内参矩阵A的求解
- 由上一节求解，带有比例因子的
我解释一下带比例因子是什么意思，由式(5)我们知道 B = A − T A − 1 B=A^{-T}A^{-1} B=A−TA−1，带比例因子的意思就是说，严格意义上 B = λ A − T A − 1 B=\lambda A^{-T}A^{-1} B=λA−TA−1
还有一点需要强调，这里的比例因子 λ \lambda λ和4.1节中提到的不是同一个，这里的只是一个中间变量。由于原文中使用的是同一个符号,因此这里做一个说明。
- 结合式(5)即
- 可以得到
由此我们的内参矩阵的封闭解就完成了
5. 外参估计
- 就单目标定而言，只要获取相机内参即可，但是还有许多系统的标定需要相机外参数，比如双目系统、结构光系统，需要获得相机与相机之间或者相机与投影仪之间的变换矩阵，也就是外参数，计算这些参数时会需要用到单目相机的外参数。
5.1 什么是单目相机的外参
- 对于单目标定而言，它的外参数具体是什么呢？
- 其实单目的外参数就是世界坐标系与相机坐标系之间的变换矩阵，也就是旋转矩阵和平移向量。
- 一般的，我们会把相机坐标系的原点选在相机的光心，Z轴与光轴所在直线重合，而2.2节中提到，世界坐标系选在棋盘格平面上，且Z坐标为0。
- 试想一下，假如相机是固定不动的，通过移动棋盘格来捕捉不同方位的棋盘格图像，那么每次移动都会改变世界坐标系的位置，因此每一张图像都对应一个变换矩阵。
说明一下这个不同方位的问题，通过上面的求解过程我们可以发现，标准旋转矩阵的性质是很重要的约束条件，不同方位的意思就是要强调旋转运动，如果棋盘格只是做纯平移运动，那么张正友标定法就会失效！
5.2 单目外参的求解
-
根据4.1节提到的关系式，即：
- { h 1 = λ A r 1 h 2 = λ A r 2 h 3 = λ A t \left\{\begin{array}{ll} h_{1}= \lambda Ar_{1} \\ h_{2}= \lambda Ar_{2} \\ h_{3}= \lambda At\end{array} \right. ⎩⎨⎧h1=λAr1h2=λAr2h3=λAt ，反过来也可以得到 { r 1 = 1 λ A − 1 h 1 r 2 = 1 λ A − 1 h 2 t = 1 λ A − 1 h 3 \left\{\begin{array}{ll} r_{1}= \frac{1}{\lambda} A^{-1}h_{1} \\ r_{2}= \frac{1}{\lambda} A^{-1}h_{2} \\t= \frac{1}{\lambda} A^{-1}h_{3}\end{array} \right. ⎩⎨⎧r1=λ1A−1h1r2=λ1A−1h2t=λ1A−1h3
- 又由标准旋转矩阵的性质可以得到 r 3 = r 1 × r 2 r_{3}=r_{1}×r_{2} r3=r1×r2， ∣ ∣ r 1 ∣ ∣ = ∣ ∣ r 2 ∣ ∣ = ∣ ∣ 1 λ A − 1 h 2 ∣ ∣ = ∣ ∣ 1 λ A − 1 h 1 ∣ ∣ = 1 ||r_{1}||=||r_{2}||= ||\frac{1}{\lambda} A^{-1}h_{2}||=|| \frac{1}{\lambda} A^{-1}h_{1}||=1 ∣∣r1∣∣=∣∣r2∣∣=∣∣λ1A−1h2∣∣=∣∣λ1A−1h1∣∣=1得：
- λ = 1 ∣ ∣ A − 1 h 1 ∣ ∣ = 1 ∣ ∣ A − 1 h 2 ∣ ∣ \lambda=\frac{1}{|| A^{-1}h_{1}||}=\frac{1}{|| A^{-1}h_{2}||} λ=∣∣A−1h1∣∣1=∣∣A−1h2∣∣1
-
综合起来就是 { λ = 1 ∣ ∣ A − 1 h 1 ∣ ∣ = 1 ∣ ∣ A − 1 h 2 ∣ ∣ r 1 = 1 λ A − 1 h 1 r 2 = 1 λ A − 1 h 2 r 3 = r 1 × r 2 t = 1 λ A − 1 h 3 \left\{\begin{array}{ll} \lambda=\frac{1}{|| A^{-1}h_{1}||}=\frac{1}{|| A^{-1}h_{2}||}\\r_{1}= \frac{1}{\lambda} A^{-1}h_{1} \\ r_{2}= \frac{1}{\lambda} A^{-1}h_{2} \\r_{3}=r_{1}×r_{2}\\t= \frac{1}{\lambda} A^{-1}h_{3}\end{array} \right. ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧λ=∣∣A−1h1∣∣1=∣∣A−1h2∣∣1r1=λ1A−1h1r2=λ1A−1h2r3=r1×r2t=λ1A−1h3
-
现在已经得到了每一张图像对应的单应性矩阵 H H H，也计算出了相机的内参矩阵 A A A，带入上式即可获得每张图像得 R 3 × 3 = [ r 1 r 2 r 3 ] R_{3×3}=\begin{bmatrix} r_{1}&r_{2}&r_{3}\end{bmatrix} R3×3=[r1r2r3]和 t t t
6. 极大似然优化（1）
-
经过前面的计算，我们得到了相机内参和外参的一个较好的初始值，我们知道，在实际情况中，图片存在很多噪声，上面的求解会有较大的误差，我们需要对上面的封闭解进行优化。
-
前提假设
- 拍摄了n张不同方位的棋盘格图片，每张图像都检测到m个角点；
- 假设所有图像的噪声都是独立同分布的
-
根据以上假设我们就可以通过下面的惩罚函数进行优化：
可以理解为最小化重投影误差
-
式中 m i j m_{ij} mij为第 i i i张图像第 j j j个角点的实际像素坐标值；
-
m ^ ( A , R i , t i , M j ) \hat{m}(A,R_{i},t_{i},M_{j}) m^(A,Ri,ti,Mj)为 m i j m_{ij} mij对应的 M j M_{j} Mj通过内外参数投影得到的计算像素坐标值
-
式中 A , R i , t i A,R_{i},t_{i} A,Ri,ti的优化初始值就是上面的封闭解，然后只用L-M算法不断迭代优化使得下惩罚函数达到最小，得到优化后的相机内外参数。
7. 畸变参数估计
7.1 畸变简介
- 由于相机镜头中透镜的存在，使得成像过程中光线的传播会产生新的影响：一、透镜形状对光线传播的影响，称为径向畸变，二、透镜位置对光线传播的影响，成为切向畸变。用极坐标的表示方法 [ r , θ ] T [r,\theta]^{T} [r,θ]T能更好的理解相机畸变模型，其中， r r r表示图像中的一点到原点的距离， θ \theta θ表示与过原点的水平线之间的夹角。径向畸变主要和 r r r有关，切向畸变主要和 θ \theta θ有关。
- 详细内容请参考博客相机模型
7.2 畸变估计
7.2.1 假设
- 一般，相机都存在镜头畸变，尤其式径向畸变。
- 复杂的畸变模型对精度的提升效果并不大，反而会导致数值不稳定。
- 设 ( u , v ) 、 ( x , y ) (u,v)、(x,y) (u,v)、(x,y)为理想情况（也就是没有畸变）下的像素坐标和图像坐标， ( u ˘ , v ˘ ) 、 ( x ˘ , y ˘ ) (\breve{u},\breve{v})、(\breve{x},\breve{y}) (u˘,v˘)、(x˘,y˘)为畸变后的真实像素坐标。
7.2.2 径向畸变模型
- 对于图像坐标系，畸变模型有：
- { x ˘ = x ( 1 + k 1 ρ 2 + k 2 ρ 4 ) y ˘ = y ( 1 + k 1 ρ 2 + k 2 ρ 4 ) \left\{\begin{array}{ll} \breve{x}=x (1+k_{1}\rho^{2}+k_{2}\rho^{4}) \\ \breve{y}=y (1+k_{1}\rho^{2}+k_{2}\rho^{4})\end{array} \right. {x˘=x(1+k1ρ2+k2ρ4)y˘=y(1+k1ρ2+k2ρ4)
- 其中 ρ 2 = x 2 + y 2 \rho^{2}=x^{2}+y^{2} ρ2=x2+y2
径向畸变参数取前两项，即 k 1 、 k 2 k_{1}、k_{2} k1、k2
-
对于像素坐标系，畸变模型有
- { u ˘ = u 0 + α x ˘ + γ y ˘ v ˘ = v 0 + β y ˘ \left\{\begin{array}{ll} \breve{u}=u_{0} +\alpha \breve{x}+\gamma\breve{y} \\ \breve{v}=v_{0}+\beta \breve{y}\end{array} \right. {u˘=u0+αx˘+γy˘v˘=v0+βy˘，一般我们都会令轴倾斜程度 γ = 0 \gamma=0 γ=0，结合图像坐标系畸变模型，和下面的关系式：
- { α x = u − u 0 β y = v − v 0 \left\{\begin{array}{ll} \alpha x=u-u_{0} \\ \beta y=v-v_{0}\end{array} \right. {αx=u−u0βy=v−v0，这个关系是由相机图像坐标系和像素坐标系之间的转换关系，具体请参考7.1节中提到的博客；
-
综合上述关系式得到：
- { u ˘ = u 0 + α x ˘ = u 0 + α x ( 1 + k 1 ρ 2 + k 2 ρ 4 ) = u + ( u − u 0 ) ( k 1 ρ 2 + k 2 ρ 4 ) v ˘ = v 0 + β y ˘ = v 0 + β y ( 1 + k 1 ρ 2 + k 2 ρ 4 ) = v + ( v − v 0 ) ( k 1 ρ 2 + k 2 ρ 4 ) \left\{\begin{array}{ll} \breve{u}=u_{0} +\alpha \breve{x}=u_{0}+\alpha x (1+k_{1}\rho^{2}+k_{2}\rho^{4})=u+(u-u_{0})( k_{1}\rho^{2}+k_{2}\rho^{4})\\ \breve{v}=v_{0}+\beta \breve{y} =v_{0}+\beta y (1+k_{1}\rho^{2}+k_{2}\rho^{4})=v+(v-v_{0})( k_{1}\rho^{2}+k_{2}\rho^{4}) \end{array} \right. {u˘=u0+αx˘=u0+αx(1+k1ρ2+k2ρ4)=u+(u−u0)(k1ρ2+k2ρ4)v˘=v0+βy˘=v0+βy(1+k1ρ2+k2ρ4)=v+(v−v0)(k1ρ2+k2ρ4)，即原文中：
- 写成矩阵形式有：
- 上面公式中红色方框内的是可以直接从图片中读取的实际坐标，蓝色方框中的是根据上面优化出来的内外参数计算得到的理想坐标。
7.2.3 k 1 、 k 2 k_{1}、k_{2} k1、k2求解
- 首先得到第一次优化得到的相机内外参数： α 、 β 、 γ 、 u 0 、 v 0 、 R 、 t \alpha、\beta、\gamma、u_{0}、v_{0}、R、t α、β、γ、u0、v0、R、t
- 角点的世界坐标通过上面的参数计算出理想的坐标 ( x , y ) 、 ( u , v ) (x,y)、(u,v) (x,y)、(u,v)
- 直接从图片中读取畸变后的像素坐标 （ u ˘ , v ˘ ) （\breve{u},\breve{v}) （u˘,v˘)
- 现在假设有n张图像，每张图像有m个角点，那么根据7.2.2节中的矩阵方程即就可以得到 2 m n 2mn 2mn个方程。
- 设 D D D是方程组的系数矩阵为 2 m n × 2 2mn×2 2mn×2的矩阵， k = [ k 1 , k 2 ] T k=[k_{1},k_{2}]^{T} k=[k1,k2]T，d为 2 m n 2mn 2mn维列向量，那么有 D k = d Dk=d Dk=d，k的最小二乘解为：
由此计算得到的值可以继续作为后面极大似然优化的初值
8. 极大似然优化（2）
- 下面是假如畸变参数后的完成版极大似然估计：
- 其中的 k 1 、 k 2 k_{1}、k_{2} k1、k2，可以使用上面求解出来的值作为初始值，也可以简单地将初值设置为0。
- 最终优化出来一个比较好的相机内外参数。
这里优化时并没有增加对旋转矩阵地约束，因此优化出来的旋转矩阵并不满足单位正交的性质，接下来会给出一种估计最佳标准旋转矩阵的方法。
9. 标准旋转矩阵估计
- 经过上面的优化后得到的旋转矩阵并不满足标准的单位正交这个性质，那么我们可以通过下面的方法得到一个和优化出来的旋转矩阵最接近同时满足单位正交这个性质的旋转矩阵。
- 假设优化出来的旋转矩为 Q Q Q；满足上述条件的旋转矩阵为 R R R。
-
使用矩阵的F范数作为惩罚函数，即
-
那么式（15）的问题就等价于最小化 R T Q R^{T}Q RTQ的迹即 t r a c e ( R T Q ) trace(R^{T}Q) trace(RTQ)
-
对 Q Q Q进行奇异值分解有 Q = U S V T Q=USV^{T} Q=USVT，其中 S = d i a g ( σ 1 , σ 2 , σ 3 ) S=diag(\sigma_1,\sigma_{2},\sigma_{3}) S=diag(σ1,σ2,σ3)
-
现在定义一个正交矩阵 Z = V T R T U Z=V^{T}R^{T}U Z=VTRTU，然后有：
-
-
- 通过上面的推导，可以很自然地发现当 Z = I Z=I Z=I时 t r a c e ( R T Q ) trace(R^{T}Q) trace(RTQ)最小也就是式（15）最小，此时 R = U V T R=UV^{T} R=UVT。由此满足单位正交条件的旋转矩阵 R R R就得到了。
10. 总结
10.1 标定流程
- 准备个标定板，图案可以是棋盘格、圆点等等。标定板图案生成网站；
- 拍摄一组（一般大于10张图片）不同方位（注意要有旋转运动）标定板的图片，可以通过移动相机也可以移动棋盘格来实现；
- 检测每张图片的特征点；
- 定义标定板位于世界坐标系 Z w = 0 Z_{w}=0 Zw=0的平面上
- 世界坐标系的原点位于标定板上
- 像素坐标原点位于图片左上角
- 利用第4、第5节介绍的封闭解法计算得到相机内外参数的优化初值；
- 利用式（10）使用L-M算法初步优化出相机的内外参数；
- 利用上面的道德内外参数求解径向畸变的优化初值通过式（13）求解
- 总后通过式（14）使用L-M算法优化所有参数，得到一个较好的相机参数。
- 通过式（15）对优化的旋转矩阵进行标准化。
10.2 展望
- 评价标定结果好坏的指标一般是重投影误差，那么有没有更好的评判标准呢？
- 影响重投影误差的因素有哪些？
- 角点检测精度
- 相机噪声
- 相机分辨率（当所有条件都一样的情况下，高分辨率 相机比低分辨率相机的重投影误差要大）
- 优化方法
- 影响重投影误差的因素有哪些？
- 一般标定板图案都会选取棋盘格，那么有没有比棋盘格效果更好的图案呢？
- 有一些说法是带背光的透明圆形标定板的标定精度会更好，因为圆的圆心的检测精度和鲁棒性比方形的角点的检测精度和鲁棒性要好很多。
- 但是圆心存在偏心误差
- 如果想做高精度的标定：
- 需要做一块透明带背光（因为图片锐度越大，重投影误差就越小）的标定板，标定板一定要在同一平面上；
- 相机的快门时间调低，使曝光减小，从而减小环境光对成像的影响；
- 光圈调小，因为光圈越大，景深越小，所以当移动相机或者标定板超出景深范围，拍出来的照片就会很模糊，标定效果就会变差。