（超详细）张氏标定法原理及公式推导
文章目录
- （超详细）张氏标定法原理及公式推导
- 0. 前言
- 1. 带着问题阅读
- 2. 基本公式与符号定义
- 3. 单应性矩阵估计
- 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. 外参估计
- 6. 极大似然优化（1）
- 7. 畸变参数估计
- 8. 极大似然优化（2）
- 9. 标准旋转矩阵估计
- 10. 总结
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)
- 特殊符号： A − T A^{-T} A−T表示 ( A − 1 ) T (A^{-1})^{T} (A−1)T或者 ( A T ) − 1 (A^{T})^{-1} (AT)−1
2.2 模型平面（棋盘格）及其图像之间得单应性矩阵
-
假定世界坐标系在棋盘格平面，且棋盘格平面处于Z=0得平面上，如下图所示：
-
-
根据以上假定，我们可以得到：
-
-
单应性矩阵 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对点才能计算出单应矩阵。
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的最小特征值对应的特征向量法求解。
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
=
σ
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隐含在等式中可得：
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的齐次方程：
- 在实际计算中，假设我们拍摄了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的右奇异向量），具体可以查阅相关资料；
-
其他几种情况：
- n = 3 n=3 n=3，那么我们可以得到唯一解；
-
n
=
2
n=2
n=2，我么可以令偏移常数
γ
=
0
\gamma=0
γ=0，也就是增加一个
约束； - n = 1 n=1 n=1，那么只能求解出两个相机内参数，比如根据先验知识我们可以知道像素坐标平面的中心点 ( u 0 , v 0 ) (u_0,v_0) (u0,v0)，同时令 γ = 0 \gamma=0 γ=0,那么我们就可以得到 α \alpha α和 β \beta β。
4.2.3 内参矩阵A的求解
- 由上一节求解，带有比例因子的
- 结合式(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得：
- [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-k4Jw7SKA-1648649906092)(D:\研一文件\笔记\Markdown\标定专题\单目标定\张正友标定法\张氏标定法公式推导\image-20220330145139515.png)]
-
综合起来就是 { λ = 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
-
对于像素坐标系，畸变模型有
- { 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)，即原文中：
- [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6Lcbsyf2-1648649906093)(D:\研一文件\笔记\Markdown\标定专题\单目标定\张正友标定法\张氏标定法公式推导\image-20220330203558297.png)]
- 写成矩阵形式有：
- 上面公式中红色方框内的是可以直接从图片中读取的实际坐标，蓝色方框中的是根据上面优化出来的内外参数计算得到的理想坐标。
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范数作为惩罚函数，即
- 由于[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-YkKh4mCA-1648649906097)(D:\研一文件\笔记\Markdown\标定专题\单目标定\张正友标定法\张氏标定法公式推导\image-20220330151953029.png)]
- 那么式（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，然后有：
- 使用矩阵的F范数作为惩罚函数，即
- 通过上面的推导，可以很自然地发现当 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 展望
- 评价标定结果好坏的指标一般是重投影误差，那么有没有更好的评判标准呢？
- 影响重投影误差的因素有哪些？
- 角点检测精度
- 相机噪声
- 相机分辨率（当所有条件都一样的情况下，高分辨率 相机比低分辨率相机的重投影误差要大）
- 优化方法
- 影响重投影误差的因素有哪些？
- 一般标定板图案都会选取棋盘格，那么有没有比棋盘格效果更好的图案呢？
- 有一些说法是带背光的透明圆形标定板的标定精度会更好，因为圆的圆心的检测精度和鲁棒性比方形的角点的检测精度和鲁棒性要好很多。
- 但是圆心存在偏心误差
- 如果想做高精度的标定：
- 需要做一块透明带背光（因为图片锐度越大，重投影误差就越小）的标定板，标定板一定要在同一平面上；
- 相机的快门时间调低，使曝光减小，从而减小环境光对成像的影响；
- 光圈调小，因为光圈越大，景深越小，所以当移动相机或者标定板超出景深范围，拍出来的照片就会很模糊，标定效果就会变差。