机器人-基础知识标定-相机内参求解原理单应性矩阵内参约束方程
【机器人-基础知识】标定 - 相机内参求解原理（单应性矩阵、内参约束方程）
1. 求解目标：内参
从世界坐标系到像素坐标系的齐次坐标形式：
s [ u v 1 ]
K [ R t ] [ X w Y w Z w 1 ] s \begin{bmatrix} u \ v \ 1 \end{bmatrix} = K , [, R \quad t ,] \begin{bmatrix} X_w \ Y_w \ Z_w \ 1 \end{bmatrix}
s
u
v
1
=
K
[
R
t
]
X
w
Y
w
Z
w
1
其中
s s
s 是尺度因子，内参矩阵
K K
K 为：
K
[ f x 0 c x 0 f y c y 0 0 1 ] K = \begin{bmatrix} f_x & 0 & c_x \ 0 & f_y & c_y \ 0 & 0 & 1 \end{bmatrix}
K
=
f
x
0
0
0
f
y
0
c
x
c
y
1
2. 单应性矩阵
参考大佬的详细文章：https://zhuanlan.zhihu.com/p/74597564
2.1. 单应性矩阵的定义
单应性矩阵（Homography Matrix）是一个平面映射到另一个平面的映射矩阵，即将一个平面上的点映射到另一个平面上。
2.2. 透视变换矩阵（或称单应性变换矩阵）
- 刚体变换：平移+旋转，只改变物体位置，不改变物体形状。
- 仿射变换：改变物体位置和形状，但是原来平行的边依然平行。
- 透视变换（也称投影变换）：彻底改变物体位置和形状
3 × 3 3\times3
3
×
3 的透视（或称单应性）变换矩阵
H H
H ：
H
( A t v T 1 )
( h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 1 ) H = \begin{pmatrix} A & t \ v^T & 1 \end{pmatrix}= \begin{pmatrix} h_{11} & h_{12} & h_{13} \ h_{21} & h_{22} & h_{23} \ h_{31} & h_{32} & 1 \end{pmatrix}
H
=
(
A
v
T
t
1
)
=
h
11
h
21
h
31
h
12
h
22
h
32
h
13
h
23
1
其中：
仿射部分（Affine Component）：
**线性变换（
2 × 2 2\times2
2
×
2 部分
A A
A ）：** 这部分包含旋转、缩放和剪切。
**A
( h 11 h 12 h 21 h 22 ) A = \begin{pmatrix} h_{11} & h_{12} \ h_{21} & h_{22} \end{pmatrix}
A
=
(
h
11
h
21
h
12
h
22
)**
**平移（
2 × 1 2\times1
2
×
1 向量
t t
t ）：**
H H
H 的第一、二行第三列构成了平移分量。
**t
( h 13 h 23 ) t = \begin{pmatrix} h_{13} \ h_{23} \end{pmatrix}
t
=
(
h
13
h
23
)**
透视（项目）部分（Projective Component）：
如果
v T
[ h 31 , h 32 ] v^T=[h_{31},,h_{32}]
v
T
=
[
h
31
,
h
32
] 不为零，则会引入额外的除法操作（透视除法）：
变换后，一个点
( x , y , 1 ) (x,,y,,1)
(
x
,
y
,
1
) 会变为
( x ′ , y ′ , w ′ ) (x’,,y’,,w')
(
x
′
,
y
′
,
w
′
) ，最后映射到实际图像坐标为
( x ′ w ′ , y ′ w ′ ) \left(\frac{x’}{w’},,\frac{y’}{w’}\right)
(
w
′
x
′
,
w
′
y
′
) 。
**如果
v T
[ h 31 , h 32 ] v^T=[h_{31},,h_{32}]
v
T
=
[
h
31
,
h
32
] 为零，则
w ′ w'
w
′ 为1，没有透视效果。**
当观察角度改变或相机位置不在正前方时，物体会出现远处变小、近处变大的效果。矩阵中
h 31 h_{31}
h
31
和
h 32 h_{32}
h
32
控制了这种“深度依赖性”的变化，最终通过除以
w
h 31 x + h 32 y + 1 w = h_{31}x + h_{32}y + 1
w
=
h
31
x
h
32
y
1 实现非线性映射，使得平行线汇聚于消失点。
2.3. 单应性矩阵的求解
不是很恰当的描述 ：世界坐标系z=0的平面上图形（图中书籍），从左右两个视角（两个图像平面）观察，得到两个图像平面上的两个不同图像。
求解一个包含 𝑛个未知数的方程组，理论上至少需要 𝑛个独立的方程。所以，为了求解单应性矩阵里的8个未知数，需要8个独立方程。1次图像变换的每组匹配点（2个点）对应2个方程，因此需要4个点的坐标变换关系。
3. 相机内参计算
- 从单应性矩阵得到约束条件（约束公式）
对于一个固定平面（如棋盘格所在的平面），我们有：
s [ u v 1 ]
K [ r 1 r 2 t ] [ X Y 1 ] s \begin{bmatrix} u \ v \ 1 \end{bmatrix} = K [r_1 \quad r_2 \quad t] \begin{bmatrix} X \ Y \ 1 \end{bmatrix}
s
u
v
1
=
K
[
r
1
r
2
t
]
X
Y
1
由于棋盘格位于
Z
0 Z=0
Z
=
0 的平面上，我们可以定义一个 单应性矩阵
H H
H ：
H
K [ r 1 r 2 t ] H = K [r_1 \quad r_2 \quad t]
H
=
K
[
r
1
r
2
t
]
其中：
K K
K 是相机内参矩阵：
K
[ f x γ u 0 0 f y v 0 0 0 1 ] K = \begin{bmatrix} f_x & \gamma & u_0 \ 0 & f_y & v_0 \ 0 & 0 & 1 \end{bmatrix}
K
=
f
x
0
0
γ
f
y
0
u
0
v
0
1
[ r 1 r 2 t ] [r_1 \quad r_2 \quad t]
[
r
1
r
2
t
] 是外参矩阵（旋转矩阵前两列
r 1 , r 2 r_1, r_2
r
1
,
r
2
以及平移
t t
t ）。
单应性矩阵
H H
H 的每一列分别是
K K
K 乘以旋转和平移矩阵的各列：
h 1
K r 1 , h 2
K r 2 , h 3
K t h_1 = K r_1, \quad h_2 = K r_2, \quad h_3 = K t
h
1
=
K
r
1
,
h
2
=
K
r
2
,
h
3
=
K
t
由于旋转矩阵的列向量满足 正交性约束 *（ 正交 和 等范数 ）：
r 1 T r 2
0 , ∥ r 1 ∥
∥ r 2 ∥ r_1^T r_2 = 0, \quad |r_1| = |r_2|
r
1
T
r
2
=
0
,
∥
r
1
∥
=
∥
r
2
∥
将其转换到单应性矩阵表示，我们得到：
h 1 T K − T K − 1 h 2
0 h_1^T K^{-T} K^{-1} h_2 = 0
h
1
T
K
−
T
K
−
1
h
2
=
0
h 1 T K − T K − 1 h 1
h 2 T K − T K − 1 h 2 ( h 1 T K − T K − 1 h 1 − h 2 T K − T K − 1 h 2
0 ) h_1^T K^{-T} K^{-1} h_1 = h_2^T K^{-T} K^{-1} h_2\ (h_1^T K^{-T} K^{-1} h_1 - h_2^T K^{-T} K^{-1} h_2 = 0)
h
1
T
K
−
T
K
−
1
h
1
=
h
2
T
K
−
T
K
−
1
h
2
(
h
1
T
K
−
T
K
−
1
h
1
−
h
2
T
K
−
T
K
−
1
h
2
=
0
)
定义
B
K − T K − 1 B = K^{-T} K^{-1}
B
=
K
−
T
K
−
1 为对称矩阵，即：
B
[ B 11 B 12 B 13 B 12 B 22 B 23 B 13 B 23 B 33 ] B = \begin{bmatrix} B_{11} & B_{12} & B_{13} \ B_{12} & B_{22} & B_{23} \ B_{13} & B_{23} & B_{33} \end{bmatrix}
B
=
B
11
B
12
B
13
B
12
B
22
B
23
B
13
B
23
B
33
设
h 1
[ h 11 h 21 h 31 ] , h 2
[ h 12 h 22 h 32 ] , h_1 = \begin{bmatrix} h_{11}\ h_{21}\ h_{31} \end{bmatrix},\quad h_2 = \begin{bmatrix} h_{12}\ h_{22}\ h_{32} \end{bmatrix},
h
1
=
h
11
h
21
h
31
,
h
2
=
h
12
h
22
h
32
,
将上述方程展开，可以得到：
正交约束展开：
h 1 x h 2 x B 11 + ( h 1 x h 2 y + h 1 y h 2 x ) B 12 + h 1 y h 2 y B 22 + ( h 1 x h 2 z + h 1 z h 2 x ) B 13 + ( h 1 y h 2 z + h 1 z h 2 y ) B 23 + h 1 z h 2 z B 33
0 h_{1x} h_{2x} B_{11} + (h_{1x} h_{2y} + h_{1y} h_{2x}) B_{12} + h_{1y} h_{2y} B_{22} + (h_{1x} h_{2z} + h_{1z} h_{2x}) B_{13} + (h_{1y} h_{2z} + h_{1z} h_{2y}) B_{23} + h_{1z} h_{2z} B_{33} = 0
h
1
x
h
2
x
B
11
(
h
1
x
h
2
y
h
1
y
h
2
x
)
B
12
h
1
y
h
2
y
B
22
(
h
1
x
h
2
z
h
1
z
h
2
x
)
B
13
(
h
1
y
h
2
z
h
1
z
h
2
y
)
B
23
h
1
z
h
2
z
B
33
=
0
等范数约束展开：
( h 11 2 − h 12 2 ) B 11 + 2 ( h 11 h 21 − h 12 h 22 ) B 12 + ( h 21 2 − h 22 2 ) B 22 + 2 ( h 11 h 31 − h 12 h 32 ) B 13 + 2 ( h 21 h 31 − h 22 h 32 ) B 23 + ( h 31 2 − h 32 2 ) B 33
(h_{11}^2 - h_{12}^2)B_{11} + 2(h_{11}h_{21} - h_{12}h_{22})B_{12} + (h_{21}^2 - h_{22}^2)B_{22} + 2(h_{11}h_{31} - h_{12}h_{32})B_{13} + 2(h_{21}h_{31} - h_{22}h_{32})B_{23} + (h_{31}^2 - h_{32}^2)B_{33} = 0.
(
h
11
2
−
h
12
2
)
B
11
2
(
h
11
h
21
−
h
12
h
22
)
B
12
(
h
21
2
−
h
22
2
)
B
22
2
(
h
11
h
31
−
h
12
h
32
)
B
13
2
(
h
21
h
31
−
h
22
h
32
)
B
23
(
h
31
2
−
h
32
2
)
B
33
=
**求解定义的
B B
B**
定义的
B
K − T K − 1 B = K^{-T}K^{-1}
B
=
K
−
T
K
−
1 是一个对称
3 × 3 3\times3
3
×
3 矩阵，它有 6 个独立的参数（例如
B 11 , B 12 , B 22 , B 13 , B 23 , B 33 B_{11}, B_{12}, B_{22}, B_{13}, B_{23}, B_{33}
B
11
,
B
12
,
B
22
,
B
13
,
B
23
,
B
33
）。
每幅图像能提供 2 个线性方程。
由于
B B
B 有 6 个未知数，所以至少需要 3 幅图像（提供共 6 个方程）才能构造出足够的独立约束来求解
B B
B 。
实际应用中，为了提高解的鲁棒性和精度，通常会采集更多图像，将所有方程叠加后使用最小二乘法求解。
**分解出相机内参
K K
K**
Cholesky 分解（或称为“科列斯基分解”）
是一种将对称正定矩阵分解为下三角矩阵与其转置乘积的方法。
形式上，对于一个对称正定矩阵
A A
A ，可以分解为
A
L L T , A = L,L^T,
A
=
L
L
T
,
其中
L L
L 是下三角矩阵。如果需要得到一个上三角因子，也可以写为
A
R T R , A = R^T R,
A
=
R
T
R
,
其中
R R
R 是上三角矩阵，并且通常要求对角线元素为正。
分解得到内参矩阵
B
K − T K − 1 ⟹ B − 1
K K T . B = K^{-T}K^{-1} \quad \Longrightarrow \quad B^{-1} = K,K^T.
B
=
K
−
T
K
−
1
⟹
B
−
1
=
K
K
T
.
由于
B − 1 B^{-1}
B
−
1 是对称正定的，我们可以对
B − 1 B^{-1}
B
−
1 进行 Cholesky 分解，得到一个下三角矩阵
L L
L 满足
B − 1
L L T . B^{-1} = L,L^T.
B
−
1
=
L
L
T
.
若希望获得一个上三角矩阵
K K
K （内参矩阵通常约定为上三角形式），可以令
K
L T . K = L^T.
K
=
L
T
.
这样，我们就通过 Cholesky 分解间接得到内参矩阵
K K
K 。