四、标定相机的畸变参数
张正友标定法仅仅考虑了畸变模型中影响较大的径向畸变。
径向畸变公式（2阶）如下：
x
^
=
x
(
1
+
k
1
r
2
+
k
2
r
4
)
y
^
=
y
(
1
+
k
1
r
2
+
k
2
r
4
)
\begin{aligned} &\hat{x}=x\left(1+k_{1} r^{2}+k_{2} r^{4}\right) \\ &\hat{y}=y\left(1+k_{1} r^{2}+k_{2} r^{4}\right) \end{aligned}
?x^=x(1+k1?r2+k2?r4)y^?=y(1+k1?r2+k2?r4)?
其中，
(
x
,
y
)
,
(
x
^
,
y
^
)
(x, y),(\hat{x}, \hat{y})
(x,y),(x^,y^?) 分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标，
r
r
r 为图像像素点到图像中心点 的距离，即
r
2
=
x
2
+
y
2
r^{2}=x^{2}+y^{2}
r2=x2+y2 。
图像坐标和像素坐标的转化关系为:
(
u
v
1
)
=
(
1
d
X
?
cot
?
θ
d
X
u
0
0
1
d
Y
sin
?
θ
v
0
0
0
1
)
(
x
y
1
)
\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\left(\begin{array}{ccc} \frac{1}{d X} & -\frac{\cot \theta}{d X} & u_{0} \\ 0 & \frac{1}{d Y \sin \theta} & v_{0} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ 1 \end{array}\right)
???uv1????=???dX1?00??dXcotθ?dYsinθ1?0?u0?v0?1???????xy1????
其中，
(
u
,
v
)
(u, v)
(u,v) 为理想的无畸变的像素坐标。由于
θ
\theta
θ 接近于
9
0
°
90^{\circ}
90° ，则上式近似为:
u
=
x
d
X
+
u
0
v
=
y
d
Y
+
v
0
\begin{aligned} &u=\frac{x}{d X}+u_{0} \\ &v=\frac{y}{d Y}+v_{0} \end{aligned}
?u=dXx?+u0?v=dYy?+v0??
同理可得畸变后的像素坐标
(
u
^
,
v
^
)
(\hat{u}, \hat{v})
(u^,v^) 的表达式为:
u
^
=
x
^
d
X
+
u
0
v
^
=
y
^
d
Y
+
v
0
\begin{aligned} &\hat{u}=\frac{\hat{x}}{d X}+u_{0} \\ &\hat{v}=\frac{\hat{y}}{d Y}+v_{0} \end{aligned}
?u^=dXx^?+u0?v^=dYy^??+v0??
代入径向畸变公式 (2阶) 则有:
u
^
?
u
0
=
(
u
?
u
0
)
(
1
+
k
1
r
2
+
k
2
r
4
)
v
^
?
v
0
=
(
v
?
v
0
)
(
1
+
k
1
r
2
+
k
2
r
4
)
\begin{aligned} &\hat{u}-u_{0}=\left(u-u_{0}\right)\left(1+k_{1} r^{2}+k_{2} r^{4}\right) \\ &\hat{v}-v_{0}=\left(v-v_{0}\right)\left(1+k_{1} r^{2}+k_{2} r^{4}\right) \end{aligned}
?u^?u0?=(u?u0?)(1+k1?r2+k2?r4)v^?v0?=(v?v0?)(1+k1?r2+k2?r4)?
可化简得:
u
^
=
u
+
(
u
?
u
0
)
(
k
1
r
2
+
k
2
r
4
)
v
^
=
v
+
(
v
?
v
0
)
(
k
1
r
2
+
k
2
r
4
)
\begin{aligned} &\hat{u}=u+\left(u-u_{0}\right)\left(k_{1} r^{2}+k_{2} r^{4}\right) \\ &\hat{v}=v+\left(v-v_{0}\right)\left(k_{1} r^{2}+k_{2} r^{4}\right) \end{aligned}
?u^=u+(u?u0?)(k1?r2+k2?r4)v^=v+(v?v0?)(k1?r2+k2?r4)?
即为:
[
(
u
?
u
0
)
r
2
(
u
?
u
0
)
r
4
(
v
?
v
0
)
r
2
(
v
?
v
0
)
r
4
]
[
k
1
k
2
]
=
[
u
^
?
u
v
^
?
v
]
\left[\begin{array}{cc} \left(u-u_{0}\right) r^{2} & \left(u-u_{0}\right) r^{4} \\ \left(v-v_{0}\right) r^{2} & \left(v-v_{0}\right) r^{4} \end{array}\right]\left[\begin{array}{l} k_{1} \\ k_{2} \end{array}\right]=\left[\begin{array}{l} \hat{u}-u \\ \hat{v}-v \end{array}\right]
[(u?u0?)r2(v?v0?)r2?(u?u0?)r4(v?v0?)r4?][k1?k2??]=[u^?uv^?v?]
每一个角点，只要知道畸变后的像素坐标
(
u
^
,
v
^
)
(\hat{u}, \hat{v})
(u^,v^) 、理想的无畸变的像素坐标
(
u
,
v
)
(u, v)
(u,v) ，就可以构造两个上述等式。那么，有
m
m
m 幅图像，每幅图像上有
n
n
n 个标定板角点，则将得到的所有等式组合起来，可以得到
m
n
m n
mn 个末知数为
k
=
[
k
1
,
k
2
]
T
k=\left[k_{1}, k_{2}\right]^{T}
k=[k1?,k2?]T 的约束 方程，将约束方程系数矩阵记为
D
D
D ，等式右端非齐次项记为
d
d
d ，可将其记为矩阵形式:
D
k
=
d
D k=d
Dk=d
之后，利用最小二乘法可得:
k
=
[
k
1
k
2
]
=
(
D
T
D
)
?
1
D
T
d
k=\left[\begin{array}{l} k_{1} \\ k_{2} \end{array}\right]=\left(D^{T} D\right)^{-1} D^{T} d
k=[k1?k2??]=(DTD)?1DTd
此时，相机畸变矫正参数已经标定好。
那么，如何获得畸变后的像素坐标
(
u
^
,
v
^
)
(\hat{u}, \hat{v})
(u^,v^) 和理楒的无畸变的像素坐标
(
u
,
v
)
(u, v)
(u,v) 呢?
(
u
^
,
v
^
)
(\hat{u}, \hat{v})
(u^,v^) 可以通过识别标定板的角点获得，
(
u
,
v
)
(u, v)
(u,v) 可以通过如下方法近似求得。世界坐标系下的每一个角点的坐标
(
U
,
V
)
(U, V)
(U,V) 是 可以计算得到的，我们利用已经求得的外参矩阵
(
R
1
R
2
T
)
(R 1 \quad R 2 \quad T)
(R1R2T) 和内参矩阵
A
A
A 进行反投影。
Z
(
u
v
1
)
=
A
(
R
1
R
2
T
)
(
U
V
1
)
=
H
(
U
V
1
)
Z\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=A\left(\begin{array}{lll} R 1 & R 2 & T \end{array}\right)\left(\begin{array}{l} U \\ V \\ 1 \end{array}\right)=H\left(\begin{array}{l} U \\ V \\ 1 \end{array}\right)
Z???uv1????=A(R1?R2?T?)???UV1????=H???UV1????
利用上式，消去尺度因子
Z
Z
Z ，可得:
u
=
H
11
U
+
H
12
V
+
H
13
H
31
U
+
H
32
V
+
H
33
v
=
H
21
U
+
H
22
V
+
H
23
H
31
U
+
H
32
V
+
H
33
\begin{gathered} u=\frac{H_{11} U+H_{12} V+H_{13}}{H_{31} U+H_{32} V+H_{33}} \\ v=\frac{H_{21} U+H_{22} V+H_{23}}{H_{31} U+H_{32} V+H_{33}} \end{gathered}
u=H31?U+H32?V+H33?H11?U+H12?V+H13??v=H31?U+H32?V+H33?H21?U+H22?V+H23???
通过上式即可得到理想的、无畸变的像素坐标
(
u
,
v
)
(u, v)
(u,v) 。当然，由于外参矩阵
(
R
1
R
2
T
)
(R 1 \quad R 2 \quad T)
(R1R2T) 和内参矩阵
A
A
A 是在有畸变的情况下获得 的，这里得到的像素坐标
(
u
,
v
)
(u, v)
(u,v) 并不是完全理想的、无畸变的。我们的总逻辑是，在进行内参矩阵和外参矩阵的求解的时 候，我们假设不存在畸变; 在进行畸变系数的求解的时候，我们假设求得的内参矩阵和外参矩阵都是无误差的。最后，我 们再通过L-M算法对于参数进行迭代优化。
需要指出，上述公式推导的时候以2阶径向畸变为例，但实际上更高阶的径向畸变同理，只是需要的约束方程个数更多而 已。
注意:
在
D
D
D 矩阵的构建过程中，需要用到
r
2
=
x
2
+
y
2
r^{2}=x^{2}+y^{2}
r2=x2+y2 。而由于张正友标定法不能直接求出焦距
f
f
f ，理想的无畸变的归一化的图 像坐标
(
x
,
y
)
(x, y)
(x,y) 无法求解，造成
D
D
D 矩阵无法构建的问题。
以下方案可以作为一种解决方案。
世界坐标系下的标定板角点的坐标
(
U
,
V
)
(U, V)
(U,V) 乘上刚体变换矩阵 (外参矩阵) 即可转化为相机坐标系下的标定板角点坐标
(
X
,
Y
,
Z
)
(X, Y, Z)
(X,Y,Z) 。
(
X
Y
Z
)
=
(
R
1
R
2
T
)
(
U
V
1
)
\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right)=\left(\begin{array}{lll} R 1 & R 2 & T \end{array}\right)\left(\begin{array}{l} U \\ V \\ 1 \end{array}\right)
???XYZ????=(R1?R2?T?)???UV1????
此时，相机坐标系下的标定板角点坐标
(
X
,
Y
,
Z
)
(X, Y, Z)
(X,Y,Z) 乘上透视投影矩阵可得图像坐标
（
x
,
y
）
（x,y）
（x,y）:
Z
(
x
y
1
)
=
(
f
0
0
0
0
f
0
0
0
0
1
0
)
(
X
Y
Z
)
Z\left(\begin{array}{l} x \\ y \\ 1 \end{array}\right)=\left(\begin{array}{llll} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}\right)\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right)
Z???xy1????=???f00?0f0?001?000???????XYZ????
其中，
(
x
,
y
)
(x, y)
(x,y) 为理想的无畸变的归一化的图像坐标。即为:
x
=
f
X
Z
y
=
f
Y
Z
\begin{aligned} &x=\frac{f X}{Z} \\ &y=\frac{f Y}{Z} \end{aligned}
?x=ZfX?y=ZfY??
记
R
2
=
X
2
+
Y
2
R^{2}=X^{2}+Y^{2}
R2=X2+Y2 ，则有
r
2
=
x
2
+
y
2
=
f
2
R
2
Z
2
r^{2}=x^{2}+y^{2}=\frac{f^{2} R^{2}}{Z^{2}}
r2=x2+y2=Z2f2R2? 。
代入
D
k
=
d
D k=d
Dk=d 中，可得:
[
(
u
?
u
0
)
R
2
Z
2
(
u
?
u
0
)
R
4
Z
4
(
v
?
v
0
)
R
2
Z
2
(
v
?
v
0
)
R
4
Z
4
]
[
f
2
k
1
f
4
k
2
]
=
[
u
^
?
u
v
^
?
v
]
\left[\begin{array}{ll} \left(u-u_{0}\right) \frac{R^{2}}{Z^{2}} & \left(u-u_{0}\right) \frac{R^{4}}{Z^{4}} \\ \left(v-v_{0}\right) \frac{R^{2}}{Z^{2}} & \left(v-v_{0}\right) \frac{R^{4}}{Z^{4}} \end{array}\right]\left[\begin{array}{l} f^{2} k_{1} \\ f^{4} k_{2} \end{array}\right]=\left[\begin{array}{l} \hat{u}-u \\ \hat{v}-v \end{array}\right]
[(u?u0?)Z2R2?(v?v0?)Z2R2??(u?u0?)Z4R4?(v?v0?)Z4R4??][f2k1?f4k2??]=[u^?uv^?v?]
我们将上式重新记为
D
′
k
′
=
d
D \prime k \prime=d
D′k′=d ，此时这个系数矩阵
D
′
D \prime
D′ 是可以完全求出来的，利用最小二乘法求解
k
′
k^{\prime}
k′ 为:
k
′
=
[
k
1
′
k
2
′
]
=
[
f
2
k
1
f
2
k
2
]
=
(
D
′
T
D
′
)
?
1
D
′
T
d
k \prime=\left[\begin{array}{l} k_{1} \prime \\ k_{2} \prime \end{array}\right]=\left[\begin{array}{c} f^{2} k_{1} \\ f^{2} k_{2} \end{array}\right]=\left(D^{\prime^{T}} D^{\prime}\right)^{-1} D \prime^{T} d
k′=[k1?′k2?′?]=[f2k1?f2k2??]=(D′TD′)?1D′Td
这里解得的
k
k
k 虽然不是真正的畸变系数，但是由于焦距
f
f
f 是定值，因此
k
k
k 也是定值，当求得
k
′
k \prime
k′ 之后，可以将畸变模型化 为:
u
^
?
u
0
=
(
u
?
u
0
)
(
1
+
k
1
′
R
2
Z
2
+
k
2
′
R
4
Z
4
)
v
^
?
v
0
=
(
v
?
v
0
)
(
1
+
k
1
′
R
2
Z
2
+
k
2
′
R
4
Z
4
)
\begin{gathered} \hat{u}-u_{0}=\left(u-u_{0}\right)\left(1+k_{1} \prime \frac{R^{2}}{Z^{2}}+k_{2} \prime \frac{R^{4}}{Z^{4}}\right) \\ \hat{v}-v_{0}=\left(v-v_{0}\right)\left(1+k_{1} \prime \frac{R^{2}}{Z^{2}}+k_{2} \prime \frac{R^{4}}{Z^{4}}\right) \end{gathered}
u^?u0?=(u?u0?)(1+k1?′Z2R2?+k2?′Z4R4?)v^?v0?=(v?v0?)(1+k1?′Z2R2?+k2?′Z4R4?)?
此时可以直接在像素坐标系下对畸变参数进行矫正。
论文中的求解畸变参数例子：
相机畸变的数学模型如公式(4.1)所示：
{
x
^
=
x
+
x
[
k
1
(
x
2
+
y
2
)
+
k
2
(
x
2
+
y
2
)
2
+
2
p
1
y
+
p
2
(
3
x
2
+
y
2
)
]
y
^
=
y
+
y
[
k
1
(
x
2
+
y
2
)
+
k
2
(
x
2
+
y
2
)
2
+
2
p
2
x
+
p
2
(
x
2
+
3
y
2
)
]
（
4.1
）
\left\{\begin{array}{l} \hat{x}=x+x\left[k_{1}\left(x^{2}+y^{2}\right)+k_{2}\left(x^{2}+y^{2}\right)^{2}+2 p_{1} y+p_{2}\left(3 x^{2}+y^{2}\right)\right] \\ \hat{y}=y+y\left[k_{1}\left(x^{2}+y^{2}\right)+k_{2}\left(x^{2}+y^{2}\right)^{2}+2 p_{2} x+p_{2}\left(x^{2}+3 y^{2}\right)\right] \end{array}\right. （4.1）
????x^=x+x[k1?(x2+y2)+k2?(x2+y2)2+2p1?y+p2?(3x2+y2)]y^?=y+y[k1?(x2+y2)+k2?(x2+y2)2+2p2?x+p2?(x2+3y2)]?（4.1）
公式(4.1)中,
k
1
、
k
2
k_{1} 、 k_{2}
k1?、k2? 为径向畸变系数,
p
1
、
p
2
p_{1} 、 p_{2}
p1?、p2? 为切向畸变系数, 对公式(3.24)进 行变换得:
[
u
?
u
0
v
?
v
0
]
[
(
x
2
+
y
2
)
(
x
2
+
y
2
)
2
2
y
(
3
x
2
+
y
2
)
(
x
2
+
y
2
)
(
x
2
+
y
2
)
2
(
x
2
+
3
y
2
)
2
x
]
[
k
1
k
2
p
1
p
2
]
=
[
u
ˉ
?
u
v
ˉ
?
v
]
（
4.2
）
\left[\begin{array}{ll} u-u_{0} & v-v_{0} \end{array}\right]\left[\begin{array}{cccc} \left(x^{2}+y^{2}\right) & \left(x^{2}+y^{2}\right)^{2} & 2 y & \left(3 x^{2}+y^{2}\right) \\ \left(x^{2}+y^{2}\right) & \left(x^{2}+y^{2}\right)^{2} & \left(x^{2}+3 y^{2}\right) & 2 x \end{array}\right]\left[\begin{array}{l} k_{1} \\ k_{2} \\ p_{1} \\ p_{2} \end{array}\right]=\left[\begin{array}{c} \bar{u}-u \\ \bar{v}-v \end{array}\right] （4.2）
[u?u0??v?v0??][(x2+y2)(x2+y2)?(x2+y2)2(x2+y2)2?2y(x2+3y2)?(3x2+y2)2x?]?????k1?k2?p1?p2???????=[uˉ?uvˉ?v?]（4.2）
在公式(4.2)中, ( u ˉ , v ˉ ) (\bar{u}, \bar{v}) (uˉ,vˉ) 为畸变前的像素坐标, ( u , v ) (u, v) (u,v) 为畸变后的像素坐标, ( u 0 , v 0 ) \left(u_{0}, v_{0}\right) (u0?,v0?)
为图像主点坐标，(x, y)为理想无畸变的图像物理坐标，若有n 幅标定图像，将所有图像按此形式进行叠加，并简写成：
D
k
=
d
\boldsymbol{D k=d}
Dk=d
公式中,
k
=
[
k
1
k
2
p
1
p
2
]
,
D
\boldsymbol{k}=\left[\begin{array}{llll}k_{1} & k_{2} & p_{1} & p_{2}\end{array}\right], \boldsymbol{D}
k=[k1??k2??p1??p2??],D 是方程等式左边
k
\boldsymbol{k}
k 的系数矩阵, 通过最小二 乘算法, 可得
k
\boldsymbol{k}
k 的解为:
k
=
(
D
T
D
)
?
1
D
T
d
\boldsymbol{k}=\left(\boldsymbol{D}^{T} \boldsymbol{D}\right)^{-1} \boldsymbol{D}^{T} \boldsymbol{d}
k=(DTD)?1DTd
在得到所有参数的初始解后, 通过采用极大似然估计的方法获得更加精确的 解。假设在标定过程中, 总共有
n
n
n 个标定板图像, 每个标定板图像有
m
m
m 个角点, 则 参数优化模型为:
∑
i
=
1
n
∑
j
=
1
m
∥
m
i
j
?
m
^
(
A
,
k
1
,
k
2
,
p
1
,
p
2
,
r
i
,
t
i
,
Q
i
j
)
∥
2
\sum_{i=1}^{n} \sum_{j=1}^{m}\left\|m_{i j}-\hat{m}\left(\boldsymbol{A}, k_{1}, k_{2}, p_{1}, p_{2}, \boldsymbol{r}_{i}, \boldsymbol{t}_{i}, \boldsymbol{Q}_{i j}\right)\right\|^{2}
i=1∑n?j=1∑m?∥∥?mij??m^(A,k1?,k2?,p1?,p2?,ri?,ti?,Qij?)∥∥?2
其中,
m
i
j
m_{i j}
mij? 为第
i
i
i 幅标定图像中第
j
j
j 个角点的图像像素坐标,
m
^
(
A
,
k
1
,
k
2
,
p
1
,
p
2
,
r
i
,
t
i
,
Q
i
j
)
\hat{m}\left(\boldsymbol{A}, k_{1}, k_{2}, p_{1}, p_{2}, \boldsymbol{r}_{i}, \boldsymbol{t}_{i}, \boldsymbol{Q}_{i j}\right)
m^(A,k1?,k2?,p1?,p2?,ri?,ti?,Qij?)
为根据内外参数获取的第
i
i
i 幅标定图像中第
j
j
j 个角点的图像像素坐标。最后通过最 小二乘算法获得优化过后的参数。
五、 双目相机位置参数求解
基于上文求解出的单目相机参数, 可以求解出描述两个相机坐标系转换关系 的旋转矩阵
R
\boldsymbol{R}
R 和平移向量
T
\boldsymbol{T}
T 。假设
R
l
\boldsymbol{R}_{l}
Rl? 和
T
l
\boldsymbol{T}_{l}
Tl? 分别左相机标定出的旋转矩阵和平移向 量,
R
r
\boldsymbol{R}_{\boldsymbol{r}}
Rr? 和
T
r
\boldsymbol{T}_{\boldsymbol{r}}
Tr? 分别是右相机标定出的旋转矩阵和平移向量。假设点
P
P
P 是空间中任意 一个点, 该点在左右相机坐标系下的坐标分别为
P
l
\boldsymbol{P}_{l}
Pl? 和
P
r
\boldsymbol{P}_{r}
Pr?, 在世界坐标系下的坐标 为
P
w
\boldsymbol{P}_{w}
Pw?, 则可以根据公式(4.4)求解出
R
\boldsymbol{R}
R 和
T
\boldsymbol{T}
T 。
{
P
l
=
R
l
P
w
+
T
l
P
r
=
R
r
P
w
+
T
r
(
4.4
)
\left\{\begin{array}{l} \boldsymbol{P}_{l}=\boldsymbol{R}_{l} \boldsymbol{P}_{w}+\boldsymbol{T}_{l} \\ \boldsymbol{P}_{r}=\boldsymbol{R}_{r} \boldsymbol{P}_{w}+\boldsymbol{T}_{r} \end{array}\right. (4.4)
{Pl?=Rl?Pw?+Tl?Pr?=Rr?Pw?+Tr??(4.4)
消除变量
P
w
\boldsymbol{P}_{w}
Pw?, 可得:
P
r
=
R
r
R
l
?
1
P
l
+
T
r
?
R
r
R
l
?
1
T
l
(
4.5
)
\boldsymbol{P}_{r}=\boldsymbol{R}_{r} \boldsymbol{R}_{l}^{-1} \boldsymbol{P}_{l}+\boldsymbol{T}_{r}-\boldsymbol{R}_{r} \boldsymbol{R}_{l}^{-1} \boldsymbol{T}_{l} (4.5)
Pr?=Rr?Rl?1?Pl?+Tr??Rr?Rl?1?Tl?(4.5)
从公式 (4.5)可以得出旋转矩阵和平移向量为:
{
R
=
R
r
R
l
?
1
T
=
T
r
?
R
r
R
l
?
1
T
(
4.6
)
\left\{\begin{array}{l} \boldsymbol{R}=\boldsymbol{R}_{r} \boldsymbol{R}_{l}^{-1} \\ \boldsymbol{T}=\boldsymbol{T}_{r}-\boldsymbol{R}_{r} \boldsymbol{R}_{l}^{-1} \boldsymbol{T} \end{array}\right. (4.6)
{R=Rr?Rl?1?T=Tr??Rr?Rl?1?T?(4.6)
综上所述, 张正友标定算法可总结如下：
(1) 准备一张棋盘格标定物，将其贴在平整的硬板上。
(2) 通过双目相机获取不同姿态下的标定板左右视图。
(3) 提取棋盘格标定板上所有角点。
(4) 利用多幅标定板图像上的角点坐标和其对应的像素坐标求解出相机内参数和外参数。
(5) 应用最小二乘算法求解畸变系数。
(6) 利用极大似然估计算法对所有参数进行优化求精。
(7) 根据左右相机参数，求解出双目相机联合标定参数。 从上述张正友标定算法的推导过程可以看出，张正友标定算法只需要一个二维平面标定板就可以完成对双目相机的标定，同时该算法充分考虑了相机中存在的畸变，因此该算法相对于其它未考虑相机畸变的标定算法标定精度更高。基于这些优点该算法得到了广泛的应用，因此本文利用该算法完成对双目相机的标定实验。
六、L-M算法参数优化
从上述推导过程就可以看出，张正友标定法是有很多近似的，所以仅仅利用上述的过程进行标定误差肯定是很大的。所以张正友标定法还利用L-M（Levenberg-Marquardt）算法对参数进行了优化。
七、相机标定的步骤
- 准备一个张正友标定法的棋盘格，棋盘格大小已知，用相机对其进行不同角度的拍摄，得到一组图像;
- 对图像中的特征点如标定板角点进行检测，得到标定板角点的像素坐标值，根据已知的棋盘格大小和世界坐标系原 点，计算得到标定板角点的物理坐标值;
- 求解内参矩阵与外参矩阵;
根据物理坐标值和像素坐标值的关系，求出 H H H 矩阵，进而构造 v v v 矩阵，求解 B B B 矩阵，利用 B B B 矩阵求解相机内参矩阵 A A A ，最后求解每张图片对应的相机外参矩阵 ( R T 0 1 ) \left(\begin{array}{cc}R & T \\ 0 & 1\end{array}\right) (R0?T1?); - 求解畸变参数；
利用 u ^ , u , v ^ , v \hat{u}, u, \hat{v}, v u^,u,v^,v 构造 D D D 矩阵，计算径向畸变参数; - 利用L-M (Levenberg-Marquardt) 算法对上述参数进行优化。
八、 标定误差分析
根据以上标定实验，本文总结出了以下几种影响标定精度的因素：
(1) 标定板精度。如果标定板精度不高，那么提取的标定板中每个角点的世界坐标值将会出现偏差，根据立体标定理论，立体标定的精度将会下降。
(2) 微小抖动造成的图像模糊。在拍摄过程中，相机或者标定板的运动会造成图像质量的下降，进而影响到标定精度。
(3) 标定板的平整度。如果标定板不平整，那么拍摄的图像就会有微小的误差，从而导致标定精度降低。
(4) 相机的分辨率。当相机的分辨率较低时，会直接影响到像素坐标的提取，进而影响到标定精度。
十、 立体校正
据双目测距系统原理，双目相机的搭建是需要严格满足同高度且光轴平行的要求，然而在实际情况下双目相机的搭建总会有微小的安装偏差，因此就需要对这种微小的偏差就行修正。当双目相机的搭建不满足要求时，左右视图就不会满足共面行对准的要求，此时就需要立体校正技术[43]将左右视图调整为共面行对准。校正过程如图 3.11 所示。
目前 Bouguet 是一种有效的双目校正算法, 该算法将左右视图相向旋转一半 的
R
\boldsymbol{R}
R, 从而使得两个视图达到共面的要求, 但是这两个视图并没有达到行对准。 为了实现左右成像平面行对准的目的, 需要将左右视图进行旋转以实现行对准。 Bouguet 算法的具体步骤如下:
(1) 将左右视图分别旋转一半的
R
\boldsymbol{R}
R, 从而实现左右视图共面的要求, 则左右视 图需要旋转的矩阵
r
l
\boldsymbol{r}_{l}
rl? 和
r
r
\boldsymbol{r}_{r}
rr? 满足以下关系:
{
R
=
r
l
r
r
?
1
r
l
r
r
=
E
\left\{\begin{array}{l} \boldsymbol{R}=\boldsymbol{r}_{\boldsymbol{l}} \boldsymbol{r}_{r}^{-1} \\ \boldsymbol{r}_{l} \boldsymbol{r}_{r}=\boldsymbol{E} \end{array}\right.
{R=rl?rr?1?rl?rr?=E?
上述公式中
E
\boldsymbol{E}
E 为单位矩阵，经过该操作后，两个视图将会达到共面状态。图 3.12为经过初步调整后的左右视图，该左右视图达到了共面要求，但未达到行对准要求。
经过上述操作后，两幅图像实现了共面但行不对准，此时需要将左图像和右图像分别沿着两个主点之间的连线进行旋转进而实现行对准。
(2) 将旋转矩阵
R
rect?
\boldsymbol{R}_{\text {rect }}
Rrect?? 沿 3 个坐标轴进行分解得:
R
rect?
=
[
e
1
T
e
2
T
e
3
T
]
\boldsymbol{R}_{\text {rect }}=\left[\begin{array}{c} \boldsymbol{e}_{1}^{T} \\ \boldsymbol{e}_{2}^{T} \\ \boldsymbol{e}_{3}^{T} \end{array}\right]
Rrect??=???e1T?e2T?e3T?????
如图
3.12
3.12
3.12 所示, 为了实现行对准, 需要对左右视图进行旋转, 而旋转的方向 为左右成像平面中心主点的连线方向, 而这个连线方向即为上文标定出的平移向 量
T
\boldsymbol{T}
T, 因此
e
1
\boldsymbol{e}_{1}
e1? 为:
e
1
=
T
∥
T
∥
e_{1}=\frac{T}{\|T\|}
e1?=∥T∥T?
上述公式中
T
=
[
T
x
T
y
T
z
]
T
\boldsymbol{T}=\left[\begin{array}{lll}T_{x} & T_{y} & T_{z}\end{array}\right]^{T}
T=[Tx??Ty??Tz??]T, 由于向量
e
2
\boldsymbol{e}_{2}
e2? 与主光轴方向正交, 且与
e
1
\boldsymbol{e}_{1}
e1? 垂直, 因此
e
2
\boldsymbol{e}_{2}
e2? 可通过主光轴方向
(
0
,
0
,
1
)
(0,0,1)
(0,0,1) 与
e
1
\boldsymbol{e}_{1}
e1? 进行义积运算得到:
e
2
=
[
?
T
y
T
x
0
]
T
x
2
+
T
y
2
（
3.35
）
\boldsymbol{e}_{2}=\frac{\left[\begin{array}{lll} -T_{y} & T_{x} & 0 \end{array}\right]}{\sqrt{T_{x}^{2}+T_{y}^{2}}} （3.35）
e2?=Tx2?+Ty2??[?Ty??Tx??0?]?（3.35）
通过
e
2
e_{2}
e2? 和
e
1
e_{1}
e1? 正交可以获得
e
3
e_{3}
e3?, 所以:
e
3
=
e
1
×
e
2
（
3.36
）
\boldsymbol{e}_{3}=\boldsymbol{e}_{1} \times \boldsymbol{e}_{2} （3.36）
e3?=e1?×e2?（3.36）
最终通过公式(3.36)使得左右视图达到共面行对准的要求:
{
R
l
=
R
r
e
c
t
r
l
R
r
=
R
r
e
c
t
r
r
（
3.37
）
\left\{\begin{array}{l} \boldsymbol{R}_{l}=\boldsymbol{R}_{r e c t} \boldsymbol{r}_{l} \\ \boldsymbol{R}_{r}=\boldsymbol{R}_{r e c t} \boldsymbol{r}_{r} \end{array}\right. （3.37）
{Rl?=Rrect?rl?Rr?=Rrect?rr??（3.37）
通过公式(3.37)就可以将左右视图实现共面且行对准，进而得到了校正后的图像。图 3.13 和图 3.14 为左右视图校正前后效果图：
通过对比图 3.13 和图 3.14 中黄色椭圆区域可以看出，经过立体校正后，两幅图像完全实现了共面行对准的要求，从而也间接证明了双目相机的标定精度是较高的，进而为下一步的立体匹配打下了基础。
十一、源代码
python基于opencv的源代码：
Calibration_ZhangZhengyou_Method
参考博客：张正友标定法
7.1 main.py
#!usr/bin/env/ python
# _*_ coding:utf-8 _*_
import cv2 as cv
import numpy as np
import os
from step.homography import get_homography
from step.intrinsics import get_intrinsics_param
from step.extrinsics import get_extrinsics_param
from step.distortion import get_distortion
from step.refine_all import refinall_all_param
def calibrate():
#求单应矩阵
H = get_homography(pic_points, real_points_x_y)
#求内参
intrinsics_param = get_intrinsics_param(H)
#求对应每幅图外参
extrinsics_param = get_extrinsics_param(H, intrinsics_param)
#畸变矫正
k = get_distortion(intrinsics_param, extrinsics_param, pic_points, real_points_x_y)
#微调所有参数
[new_intrinsics_param, new_k, new_extrinsics_param] = refinall_all_param(intrinsics_param,
k, extrinsics_param, real_points, pic_points)
print("intrinsics_parm:\t", new_intrinsics_param)
print("distortionk:\t", new_k)
print("extrinsics_parm:\t", new_extrinsics_param)
if __name__ == "__main__":
file_dir = r'..\pic'
# 标定所用图像
pic_name = os.listdir(file_dir)
# 由于棋盘为二维平面，设定世界坐标系在棋盘上，一个单位代表一个棋盘宽度，产生世界坐标系三维坐标
cross_corners = [7, 4] #棋盘方块交界点排列
real_coor = np.zeros((cross_corners[0] * cross_corners[1], 3), np.float32)
real_coor[:, :2] = np.mgrid[0:7, 0:4].T.reshape(-1, 2)
real_points = []
real_points_x_y = []
pic_points = []
for pic in pic_name:
pic_path = os.path.join(file_dir, pic)
pic_data = cv.imread(pic_path)
# 寻找到棋盘角点
succ, pic_coor = cv.findChessboardCorners(pic_data, (cross_corners[0], cross_corners[1]), None)
if succ:
# 添加每幅图的对应3D-2D坐标
pic_coor = pic_coor.reshape(-1, 2)
pic_points.append(pic_coor)
real_points.append(real_coor)
real_points_x_y.append(real_coor[:, :2])
calibrate()
7.2 homography.py
这是用于求解单应性矩阵的文件
#!usr/bin/env/ python
# _*_ coding:utf-8 _*_
import numpy as np
from scipy import optimize as opt
#求输入数据的归一化矩阵
def normalizing_input_data(coor_data):
x_avg = np.mean(coor_data[:, 0])
y_avg = np.mean(coor_data[:, 1])
sx = np.sqrt(2) / np.std(coor_data[:, 0])
sy = np.sqrt(2) / np.std(coor_data[:, 1])
norm_matrix = np.matrix([[sx, 0, -sx * x_avg],
[0, sy, -sy * y_avg],
[0, 0, 1]])
return norm_matrix
#求取初始估计的单应矩阵
def get_initial_H(pic_coor, real_coor):
# 获得归一化矩阵
pic_norm_mat = normalizing_input_data(pic_coor)
real_norm_mat = normalizing_input_data(real_coor)
M = []
for i in range(len(pic_coor)):
#转换为齐次坐标
single_pic_coor = np.array([pic_coor[i][0], pic_coor[i][1], 1])
single_real_coor = np.array([real_coor[i][0], real_coor[i][1], 1])
#坐标归一化
pic_norm = np.dot(pic_norm_mat, single_pic_coor)
real_norm = np.dot(real_norm_mat, single_real_coor)
#构造M矩阵
M.append(np.array([-real_norm.item(0), -real_norm.item(1), -1,
0, 0, 0,
pic_norm.item(0) * real_norm.item(0), pic_norm.item(0) * real_norm.item(1), pic_norm.item(0)]))
M.append(np.array([0, 0, 0,
-real_norm.item(0), -real_norm.item(1), -1,
pic_norm.item(1) * real_norm.item(0), pic_norm.item(1) * real_norm.item(1), pic_norm.item(1)]))
#利用SVD求解M * h = 0中h的解
U, S, VT = np.linalg.svd((np.array(M, dtype='float')).reshape((-1, 9)))
# 最小的奇异值对应的奇异向量,S求出来按大小排列的，最后的最小
H = VT[-1].reshape((3, 3))
H = np.dot(np.dot(np.linalg.inv(pic_norm_mat), H), real_norm_mat)
H /= H[-1, -1]
return H
#返回估计坐标与真实坐标偏差
def value(H, pic_coor, real_coor):
Y = np.array([])
for i in range(len(real_coor)):
single_real_coor = np.array([real_coor[i, 0], real_coor[i, 1], 1])
U = np.dot(H.reshape(3, 3), single_real_coor)
U /= U[-1]
Y = np.append(Y, U[:2])
Y_NEW = (pic_coor.reshape(-1) - Y)
return Y_NEW
#返回对应jacobian矩阵
def jacobian(H, pic_coor, real_coor):
J = []
for i in range(len(real_coor)):
sx = H[0]*real_coor[i][0] + H[1]*real_coor[i][1] +H[2]
sy = H[3]*real_coor[i][0] + H[4]*real_coor[i][1] +H[5]
w = H[6]*real_coor[i][0] + H[7]*real_coor[i][1] +H[8]
w2 = w * w
J.append(np.array([real_coor[i][0]/w, real_coor[i][1]/w, 1/w,
0, 0, 0,
-sx*real_coor[i][0]/w2, -sx*real_coor[i][1]/w2, -sx/w2]))
J.append(np.array([0, 0, 0,
real_coor[i][0]/w, real_coor[i][1]/w, 1/w,
-sy*real_coor[i][0]/w2, -sy*real_coor[i][1]/w2, -sy/w2]))
return np.array(J)
#利用Levenberg Marquart算法微调H
def refine_H(pic_coor, real_coor, initial_H):
initial_H = np.array(initial_H)
final_H = opt.leastsq(value,
initial_H,
Dfun=jacobian,
args=(pic_coor, real_coor))[0]
final_H /= np.array(final_H[-1])
return final_H
#返回微调后的H
def get_homography(pic_coor, real_coor):
refined_homographies =[]
error = []
for i in range(len(pic_coor)):
initial_H = get_initial_H(pic_coor[i], real_coor[i])
final_H = refine_H(pic_coor[i], real_coor[i], initial_H)
refined_homographies.append(final_H)
return np.array(refined_homographies)
7.3 intrinsics.py
这是用于求解内参矩阵的文件
#!usr/bin/env/ python
# _*_ coding:utf-8 _*_
import numpy as np
#返回pq位置对应的v向量
def create_v(p, q, H):
H = H.reshape(3, 3)
return np.array([
H[0, p] * H[0, q],
H[0, p] * H[1, q] + H[1, p] * H[0, q],
H[1, p] * H[1, q],
H[2, p] * H[0, q] + H[0, p] * H[2, q],
H[2, p] * H[1, q] + H[1, p] * H[2, q],
H[2, p] * H[2, q]
])
#返回相机内参矩阵A
def get_intrinsics_param(H):
#构建V矩阵
V = np.array([])
for i in range(len(H)):
V = np.append(V, np.array([create_v(0, 1, H[i]), create_v(0, 0 , H[i])- create_v(1, 1 , H[i])]))
#求解V*b = 0中的b
U, S, VT = np.linalg.svd((np.array(V, dtype='float')).reshape((-1, 6)))
#最小的奇异值对应的奇异向量,S求出来按大小排列的，最后的最小
b = VT[-1]
#求取相机内参
w = b[0] * b[2] * b[5] - b[1] * b[1] * b[5] - b[0] * b[4] * b[4] + 2 * b[1] * b[3] * b[4] - b[2] * b[3] * b[3]
d = b[0] * b[2] - b[1] * b[1]
alpha = np.sqrt(w / (d * b[0]))
beta = np.sqrt(w / d**2 * b[0])
gamma = np.sqrt(w / (d**2 * b[0])) * b[1]
uc = (b[1] * b[4] - b[2] * b[3]) / d
vc = (b[1] * b[3] - b[0] * b[4]) / d
return np.array([
[alpha, gamma, uc],
[0, beta, vc],
[0, 0, 1]
])
7.4 extrinsics.py
这是用于求解外参矩阵的文件
#!usr/bin/env/ python
# _*_ coding:utf-8 _*_
import numpy as np
#返回每一幅图的外参矩阵[R|t]
def get_extrinsics_param(H, intrinsics_param):
extrinsics_param = []
inv_intrinsics_param = np.linalg.inv(intrinsics_param)
for i in range(len(H)):
h0 = (H[i].reshape(3, 3))[:, 0]
h1 = (H[i].reshape(3, 3))[:, 1]
h2 = (H[i].reshape(3, 3))[:, 2]
scale_factor = 1 / np.linalg.norm(np.dot(inv_intrinsics_param, h0))
r0 = scale_factor * np.dot(inv_intrinsics_param, h0)
r1 = scale_factor * np.dot(inv_intrinsics_param, h1)
t = scale_factor * np.dot(inv_intrinsics_param, h2)
r2 = np.cross(r0, r1)
R = np.array([r0, r1, r2, t]).transpose()
extrinsics_param.append(R)
return extrinsics_param
7.5 distortion.py
这是用于求解畸变矫正系数的文件
#!usr/bin/env/ python
# _*_ coding:utf-8 _*_
import numpy as np
#返回畸变矫正系数k0,k1
def get_distortion(intrinsic_param, extrinsic_param, pic_coor, real_coor):
D = []
d = []
for i in range(len(pic_coor)):
for j in range(len(pic_coor[i])):
#转换为齐次坐标
single_coor = np.array([(real_coor[i])[j, 0], (real_coor[i])[j, 1], 0, 1])
#利用现有内参及外参求出估计图像坐标
u = np.dot(np.dot(intrinsic_param, extrinsic_param[i]), single_coor)
[u_estim, v_estim] = [u[0]/u[2], u[1]/u[2]]
coor_norm = np.dot(extrinsic_param[i], single_coor)
coor_norm /= coor_norm[-1]
#r = np.linalg.norm((real_coor[i])[j])
r = np.linalg.norm(coor_norm)
D.append(np.array([(u_estim - intrinsic_param[0, 2]) * r ** 2, (u_estim - intrinsic_param[0, 2]) * r ** 4]))
D.append(np.array([(v_estim - intrinsic_param[1, 2]) * r ** 2, (v_estim - intrinsic_param[1, 2]) * r ** 4]))
#求出估计坐标与真实坐标的残差
d.append(pic_coor[i][j, 0] - u_estim)
d.append(pic_coor[i][j, 1] - v_estim)
'''
D.append(np.array([(pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 2, (pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 4]))
D.append(np.array([(pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 2, (pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 4]))
#求出估计坐标与真实坐标的残差
d.append(u_estim - pic_coor[i][j, 0])
d.append(v_estim - pic_coor[i][j, 1])
'''`在这里插入代码片`
D = np.array(D)
temp = np.dot(np.linalg.inv(np.dot(D.T, D)), D.T)
k = np.dot(temp, d)
'''
#也可利用SVD求解D * k = d中的k
U, S, Vh=np.linalg.svd(D, full_matrices=False)
temp_S = np.array([[S[0], 0],
[0, S[1]]])
temp_res = np.dot(Vh.transpose(), np.linalg.inv(temp_S))
temp_res_res = np.dot(temp_res, U.transpose())
k = np.dot(temp_res_res, d)
'''
return k
7.6 refine_all.py
这是用于微调参数的文件
#!usr/bin/env/ python
# _*_ coding:utf-8 _*_
import numpy as np
import math
from scipy import optimize as opt
#微调所有参数
def refinall_all_param(A, k, W, real_coor, pic_coor):
#整合参数
P_init = compose_paramter_vector(A, k, W)
#复制一份真实坐标
X_double = np.zeros((2 * len(real_coor) * len(real_coor[0]), 3))
Y = np.zeros((2 * len(real_coor) * len(real_coor[0])))
M = len(real_coor)
N = len(real_coor[0])
for i in range(M):
for j in range(N):
X_double[(i * N + j) * 2] = (real_coor[i])[j]
X_double[(i * N + j) * 2 + 1] = (real_coor[i])[j]
Y[(i * N + j) * 2] = (pic_coor[i])[j, 0]
Y[(i * N + j) * 2 + 1] = (pic_coor[i])[j, 1]
#微调所有参数
P = opt.leastsq(value,
P_init,
args=(W, real_coor, pic_coor),
Dfun=jacobian)[0]
#raial_error表示利用标定后的参数计算得到的图像坐标与真实图像坐标点的平均像素距离
error = value(P, W, real_coor, pic_coor)
raial_error = [np.sqrt(error[2 * i]**2 + error[2 * i + 1]**2) for i in range(len(error) // 2)]
print("total max error:\t", np.max(raial_error))
#返回拆解后参数，分别为内参矩阵，畸变矫正系数，每幅图对应外参矩阵
return decompose_paramter_vector(P)
#把所有参数整合到一个数组内
def compose_paramter_vector(A, k, W):
alpha = np.array([A[0, 0], A[1, 1], A[0, 1], A[0, 2], A[1, 2], k[0], k[1]])
P = alpha
for i in range(len(W)):
R, t = (W[i])[:, :3], (W[i])[:, 3]
#旋转矩阵转换为一维向量形式
zrou = to_rodrigues_vector(R)
w = np.append(zrou, t)
P = np.append(P, w)
return P
#分解参数集合，得到对应的内参，外参，畸变矫正系数
def decompose_paramter_vector(P):
[alpha, beta, gamma, uc, vc, k0, k1] = P[0:7]
A = np.array([[alpha, gamma, uc],
[0, beta, vc],
[0, 0, 1]])
k = np.array([k0, k1])
W = []
M = (len(P) - 7) // 6
for i in range(M):
m = 7 + 6 * i
zrou = P[m:m+3]
t = (P[m+3:m+6]).reshape(3, -1)
#将旋转矩阵一维向量形式还原为矩阵形式
R = to_rotation_matrix(zrou)
#依次拼接每幅图的外参
w = np.concatenate((R, t), axis=1)
W.append(w)
W = np.array(W)
return A, k, W
#返回从真实世界坐标映射的图像坐标
def get_single_project_coor(A, W, k, coor):
single_coor = np.array([coor[0], coor[1], coor[2], 1])
#'''
coor_norm = np.dot(W, single_coor)
coor_norm /= coor_norm[-1]
#r = np.linalg.norm(coor)
r = np.linalg.norm(coor_norm)
uv = np.dot(np.dot(A, W), single_coor)
uv /= uv[-1]
#畸变
u0 = uv[0]
v0 = uv[1]
uc = A[0, 2]
vc = A[1, 2]
#u = (uc * r**2 * k[0] + uc * r**4 * k[1] - u0) / (r**2 * k[0] + r**4 * k[1] - 1)
#v = (vc * r**2 * k[0] + vc * r**4 * k[1] - v0) / (r**2 * k[0] + r**4 * k[1] - 1)
u = u0 + (u0 - uc) * r**2 * k[0] + (u0 - uc) * r**4 * k[1]
v = v0 + (v0 - vc) * r**2 * k[0] + (v0 - vc) * r**4 * k[1]
'''
uv = np.dot(W, single_coor)
uv /= uv[-1]
# 透镜矫正
x0 = uv[0]
y0 = uv[1]
r = np.linalg.norm(np.array([x0, y0]))
k0 = 0
k1 = 0
x = x0 * (1 + r ** 2 * k0 + r ** 4 * k1)
y = y0 * (1 + r ** 2 * k0 + r ** 4 * k1)
#u = A[0, 0] * x + A[0, 2]
#v = A[1, 1] * y + A[1, 2]
[u, v, _] = np.dot(A, np.array([x, y, 1]))
'''
return np.array([u, v])
#返回所有点的真实世界坐标映射到的图像坐标与真实图像坐标的残差
def value(P, org_W, X, Y_real):
M = (len(P) - 7) // 6
N = len(X[0])
A = np.array([
[P[0], P[2], P[3]],
[0, P[1], P[4]],
[0, 0, 1]
])
Y = np.array([])
for i in range(M):
m = 7 + 6 * i
#取出当前图像对应的外参
w = P[m:m + 6]
# 不用旋转矩阵的变换是因为会有精度损失
'''
R = to_rotation_matrix(w[:3])
t = w[3:].reshape(3, 1)
W = np.concatenate((R, t), axis=1)
'''
W = org_W[i]
#计算每幅图的坐标残差
for j in range(N):
Y = np.append(Y, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))
error_Y = np.array(Y_real).reshape(-1) - Y
return error_Y
#计算对应jacobian矩阵
def jacobian(P, WW, X, Y_real):
M = (len(P) - 7) // 6
N = len(X[0])
K = len(P)
A = np.array([
[P[0], P[2], P[3]],
[0, P[1], P[4]],
[0, 0, 1]
])
res = np.array([])
for i in range(M):
m = 7 + 6 * i
w = P[m:m + 6]
R = to_rotation_matrix(w[:3])
t = w[3:].reshape(3, 1)
W = np.concatenate((R, t), axis=1)
for j in range(N):
res = np.append(res, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))
#求得x, y方向对P[k]的偏导
J = np.zeros((K, 2 * M * N))
for k in range(K):
J[k] = np.gradient(res, P[k])
return J.T
#将旋转矩阵分解为一个向量并返回，Rodrigues旋转向量与矩阵的变换,最后计算坐标时并未用到，因为会有精度损失
def to_rodrigues_vector(R):
p = 0.5 * np.array([[R[2, 1] - R[1, 2]],
[R[0, 2] - R[2, 0]],
[R[1, 0] - R[0, 1]]])
c = 0.5 * (np.trace(R) - 1)
if np.linalg.norm(p) == 0:
if c == 1:
zrou = np.array([0, 0, 0])
elif c == -1:
R_plus = R + np.eye(3, dtype='float')
norm_array = np.array([np.linalg.norm(R_plus[:, 0]),
np.linalg.norm(R_plus[:, 1]),
np.linalg.norm(R_plus[:, 2])])
v = R_plus[:, np.where(norm_array == max(norm_array))]
u = v / np.linalg.norm(v)
if u[0] < 0 or (u[0] == 0 and u[1] < 0) or (u[0] == u[1] and u[0] == 0 and u[2] < 0):
u = -u
zrou = math.pi * u
else:
zrou = []
else:
u = p / np.linalg.norm(p)
theata = math.atan2(np.linalg.norm(p), c)
zrou = theata * u
return zrou
#把旋转矩阵的一维向量形式还原为旋转矩阵并返回
def to_rotation_matrix(zrou):
theta = np.linalg.norm(zrou)
zrou_prime = zrou / theta
W = np.array([[0, -zrou_prime[2], zrou_prime[1]],
[zrou_prime[2], 0, -zrou_prime[0]],
[-zrou_prime[1], zrou_prime[0], 0]])
R = np.eye(3, dtype='float') + W * math.sin(theta) + np.dot(W, W) * (1 - math.cos(theta))
return R
