最小化重投影误差求解PnP

问题描述

已知n个空间点$P_i=[x_i,y_i,z_i{]}^T$，其投影的像素坐标$p_i=[u_i,v_i{]}^T$求相机的位姿R，T。

问题分析

根据相机模型，像素点和空间点的位置关系：
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=KT\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]$$
即：
$$s_i{u}_i=KT{P}_i$$
由于存在噪声误差，因此以最小化误差平方和为目标构建最小二乘问题：
$$T^{\ast }=arg\underset{T}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert \begin{array}{c}{u}_i-\frac{1}{s_i}KT{P}_i\end{array}\Vert }_2^2$$
因为这个误差是将3D点的理论投影位置与观测到的实际投影位置之间的误差，因此称为重投影误差：
$$e_i=u_i-\frac{1}{s_i}KT{P}_i$$

按照之前讲过的高斯牛顿法进行求解：
旋转矩阵本身带有约束，即正交且行列式为1。而有约束的优化问题比无约束的优化问题复杂的多。因为李代数的特点，李代数表示的天然满足旋转矩阵的约束，因此通常使用李代数进行表示来求解。