【机器人-基础知识】标定 - 相机内参求解原理（单应性矩阵、内参约束方程）
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
uv1
=K[Rt]
XwYwZw1
其中
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=
fx000fy0cxcy1
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=(AvTt1)=
h11h21h31h12h22h32h13h231
其中：
-
仿射部分（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=(h11h21h12h22) -
平移（ 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=(h13h23)
-
-
透视（项目）部分（Projective Component）：
如果 v T = [ h 31 , h 32 ] v^T=[h_{31},\,h_{32}] vT=[h31,h32] 不为零，则会引入额外的除法操作（透视除法）：- 变换后，一个点
(
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}] vT=[h31,h32] 为零，则 w ′ w' w′为1，没有透视效果。
- 变换后，一个点
(
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′)。
当观察角度改变或相机位置不在正前方时，物体会出现远处变小、近处变大的效果。矩阵中 h 31 h_{31} h31 和 h 32 h_{32} h32 控制了这种“深度依赖性”的变化，最终通过除以 w = h 31 x + h 32 y + 1 w = h_{31}x + h_{32}y + 1 w=h31x+h32y+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 uv1 =K[r1r2t] XY1
由于棋盘格位于 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[r1r2t]
其中：
-
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= fx00γfy0u0v01
-
[ r 1 r 2 t ] [r_1 \quad r_2 \quad t] [r1r2t] 是外参矩阵（旋转矩阵前两列 r 1 , r 2 r_1, r_2 r1,r2 以及平移 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 h1=Kr1,h2=Kr2,h3=Kt
由于旋转矩阵的列向量满足 正交性约束*（正交 和 等范数）：
r 1 T r 2 = 0 , ∥ r 1 ∥ = ∥ r 2 ∥ r_1^T r_2 = 0, \quad \|r_1\| = \|r_2\| r1Tr2=0,∥r1∥=∥r2∥
将其转换到单应性矩阵表示，我们得到：
h
1
T
K
−
T
K
−
1
h
2
=
0
h_1^T K^{-T} K^{-1} h_2 = 0
h1TK−TK−1h2=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)
h1TK−TK−1h1=h2TK−TK−1h2(h1TK−TK−1h1−h2TK−TK−1h2=0)
定义 B = K − T K − 1 B = K^{-T} K^{-1} B=K−TK−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= B11B12B13B12B22B23B13B23B33
设
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},
h1=
h11h21h31
,h2=
h12h22h32
,
将上述方程展开，可以得到：
- 正交约束展开：
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 h1xh2xB11+(h1xh2y+h1yh2x)B12+h1yh2yB22+(h1xh2z+h1zh2x)B13+(h1yh2z+h1zh2y)B23+h1zh2zB33=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 = 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} = 0. (h112−h122)B11+2(h11h21−h12h22)B12+(h212−h222)B22+2(h11h31−h12h32)B13+2(h21h31−h22h32)B23+(h312−h322)B33=0.
-
求解定义的 B B B
定义的 B = K − T K − 1 B = K^{-T}K^{-1} B=K−TK−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} B11,B12,B22,B13,B23,B33）。每幅图像能提供 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=LLT,
其中 L L L 是下三角矩阵。如果需要得到一个上三角因子，也可以写为
A = R T R , A = R^T R, A=RTR,
其中 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−TK−1⟹B−1=KKT.
由于 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=LLT.
若希望获得一个上三角矩阵 K K K（内参矩阵通常约定为上三角形式），可以令
K = L T . K = L^T. K=LT.
这样，我们就通过 Cholesky 分解间接得到内参矩阵 K K K。