第八篇 相机位姿优化问题三：不用雅克比，PNP模型SVD分解，3D 到 2D

一 问题背景

在自动驾驶和SLAM中，经常遇到问题，知道3D世界的点集P = [x y z 1]‘，和相机观测到的在2D世界的点坐标P’=[u v 1]'，如何利用这些对应点集求解位姿即R和t。
在前面两篇文章中已经介绍了普适方法，直接求误差函数对位姿向量的雅克比矩阵，然后利用高斯牛顿发进行优化，前面介绍的方法是普遍适用的方法。本节介绍的方法，只对点集匹配求解姿态适用。

二 问题建模

如下式所示，对于一组匹配点，列出一个公式。多组匹配点存在时，需要求解t1~t12。
$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$
令：
$$\begin{gathered} {\mathbf{t}}_1={\left[\begin{array}{cccc}{t}_1& t_2& t_3& t_4\end{array}\right]}^T \\ {\mathbf{t}}_2={\left[\begin{array}{cccc}{t}_5& t_6& t_7& t_8\end{array}\right]}^T \\ {\mathbf{t}}_3={\left[\begin{array}{cccc}{t}_9& t_{10}& t_{11}& t_{12}\end{array}\right]}^T \end{gathered}$$
由上面的式子可得如下两个方程：
$$\begin{gathered} {{\mathbf{t}}_1}^{T}P-u{{\mathbf{t}}_3}^{T}P=0 \\ {{\mathbf{t}}_2}^{T}P-v{{\mathbf{t}}_3}^{T}P=0 \end{gathered}$$
即
$$\left[\begin{array}{ccc}{P}^T& 0& -u{P}^T\\ 0& P^T& -u{P}^T\end{array}\right]\left[\begin{array}{c}{\mathbf{t}}_1\\ {\mathbf{t}}_2\\ {\mathbf{t}}_3\end{array}\right]=0$$
存在n个点时，存在2n 个方程组。
问题因此成为求解如下齐次方程最优解。
$$A\ast \mathbf{t}=0$$

三 问题求解

求解上述问题需要n大于等于6。对A矩阵求SVD分解。
$$\left[\begin{array}{ccc}U& \Sigma & V^T\end{array}\right]=svd(A)$$
t 向量为V矩阵的最后一列，因此求得t1~t12。注意，此时求解结果没有加入尺度s,因此会存在一个尺度效应。
$$\mathbf{t}=\beta V(:,end)$$
进一步可以求出旋转：
$$\overline{R}=\left[\begin{array}{ccc}t1& t2& t3\\ t5& t6& t7\\ t9& t10& t11\end{array}\right]$$
此时估计的 R不是标准旋转矩阵，进一步对其进行SVD分解
$$\left[\begin{array}{ccc}U& \Sigma & V^T\end{array}\right]=svd(\overline{R})$$
$$R=\pm U{V}^T$$
理论上，Sigma的对角线上元素应该非常接近，根据对角线元素可以求出尺度因子。
$$\beta =\pm (tr(\Sigma )/3)$$
符号问题可以通过在相机坐标系下的Z一定大于0求得：
$$\beta \ast {{\mathbf{t}}_3}^T\ast P>0$$
$$t={\left[\begin{array}{ccc}{t}_4& t_8& t_{12}\end{array}\right]}^{\prime }$$