最小化重投影误差求解PnP
问题描述
已知n个空间点 P i = [ x i , y i , z i ] T P_i=[x_i,y_i,z_i]^T Pi=[xi,yi,zi]T，其投影的像素坐标 p i = [ u i , v i ] T p_i=[u_i,v_i]^T pi=[ui,vi]T求相机的位姿R，T。
问题分析
根据相机模型，像素点和空间点的位置关系：
s
i
[
u
i
v
i
1
]
=
K
T
[
x
i
y
i
z
i
1
]
s_i\begin{bmatrix}u_i \\v_i \\1\\\end{bmatrix}=KT \begin{bmatrix} x_i \\ y_i \\ z_i\\ 1\\ \end{bmatrix}
si⎣⎡uivi1⎦⎤=KT⎣⎢⎢⎡xiyizi1⎦⎥⎥⎤
即：
s
i
u
i
=
K
T
P
i
s_iu_i=KTP_i
siui=KTPi
由于存在噪声误差，因此以最小化误差平方和为目标构建最小二乘问题：
T
∗
=
a
r
g
min
T
1
2
∑
i
=
1
n
∥
u
i
−
1
s
i
K
T
P
i
∥
2
2
T^{*}=arg \min_{T} \frac{1}{2} \sum_{i=1}^{n} \begin{Vmatrix}u_i-\frac{1}{s_i}KTP_i\end{Vmatrix}_{2}^{2}
T∗=argTmin21i=1∑n∥∥ui−si1KTPi∥∥22
因为这个误差是将3D点的理论投影位置与观测到的实际投影位置之间的误差，因此称为重投影误差：
e
i
=
u
i
−
1
s
i
K
T
P
i
e_i=u_i-\frac{1}{s_i}KTP_i
ei=ui−si1KTPi
按照之前讲过的高斯牛顿法进行求解：
旋转矩阵本身带有约束，即正交且行列式为1。而有约束的优化问题比无约束的优化问题复杂的多。因为李代数的特点，李代数表示的天然满足旋转矩阵的约束，因此通常使用李代数进行表示来求解。