DLT求解PnP问题

PnP问题解法

问题描述

Perspective-n-Point (PnP) 问题：如下图，
1）给定 [公式] 个3D参考点 [公式] 到摄像机图像上2D投影点 [公式] 的匹配点对；
2）已知 3D点在世界坐标系下的坐标，2D点在图像坐标系下的坐标；
3）已知摄像机的内参数 [公式] 。
目的：求世界坐标系与摄像机坐标系之间的位姿变换 [公式]
用途：相机位姿跟踪，物体位姿跟踪，AR/VR，机器人操作，SLAM中位姿初值求解……
常用解法：DLT，P3P，EPnP，UPnP。

PnP算法分为直接发和优化法两大类，
直接法包括：P3P、 DLT 、 EPnP等等；
优化法有： LHM 、 Only pos BA 等。

DLT解法 （直接线性变换）

理论推导部分

3D参考点在世界坐标系的齐次坐标：
$\mathbf{c}=\left[\begin{array}{l}x\\ y\\ z\\ 1\end{array}\right]$
对应的2D投影点在图像坐标系下的齐次坐标是：
$\mathbf{u}=\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]$
相机内参一般是已知的 slam’问题里
$\mathbf{K}=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right]$
那么3D点到2D点投影可表示为
$\lambda \left[\begin{array}{l}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{lll}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right][\mathbf{R}\mid \mathbf{t}]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$ (1)

$[\mathbf{R}\mid \mathbf{t}]$ 是一个3x4的增广矩阵，包含了旋转和平移信息。
理论上由于 $[\mathbf{R}]$这个玩意儿就只有三个自由度，因为存在旋转矩阵的正交约束，DLT算法中先不管这个，忽略这个正交约束，按照$[\mathbf{R}\mid \mathbf{t}]$有12个未知参数$\mathbf{x}={\left[a_1,\cdots ,a_{12}\right]}^T$来考虑计算。
这样，(1)式变化为：
$\lambda =\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& & c_x\\ & f_y& c_y\\ & & 1\end{array}\right]\left[\begin{array}{cccc}{a}_1& a_2& a_3& a_4\\ a_5& a_6& a_7& a_8\\ a_9& a_{10}& a_{11}& a_{12}\end{array}\right]\left[\begin{array}{l}x\\ y\\ z\\ 1\end{array}\right]$ (2)
先消去第三个等式，也就是消去scale factor尺度因子$\lambda$ ，
有：
$\{\begin{array}{l}x{f}_x{a}_1+x{c}_x{a}_9+y{f}_x{a}_2+y{c}_x{a}_{10}+z{f}_x{a}_3+z{c}_x{a}_{11}+f_x{a}_4+c_x{a}_{12}-ux{a}_9-uy{a}_{10}-uz{a}_{11}-u{a}_{12}=0\\ x{f}_y{a}_5+x{c}_y{a}_9+y{f}_y{a}_6+y{c}_y{a}_{10}+z{f}_y{a}_7+z{c}_y{a}_{11}+f_y{a}_8+c_y{a}_{12}-vx{a}_9-vy{a}_{10}-vz{a}_{11}-v{a}_{12}=0\end{array}$
写成矩阵形式如下：
$\left[\begin{array}{cccccccccccc}x{f}_x& y{f}_x& z{f}_x& f_x& 0& 0& 0& 0& x{c}_x-ux& y{c}_x-uy& z{c}_x-uz& c_x-u\\ 0& 0& 0& 0& x{f}_y& y{f}_y& z{f}_y& f_y& x{c}_y-vx& y{c}_y-vy& z{c}_y-vz& c_y-v\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ ⋮\\ a_{12}\end{array}\right]=0$

高翔的14讲 用了简化表示
定义$a=[\begin{array}{cccc}{a}_1& a_2& a_3& a_4\\ a_5& a_6& a_7& a_8\\ a_9& a_{10}& a_{11}& a_{12}\end{array}]$的三个行向量为：
$t_1=(a_1,a_2,a_3,a_4{)}^T$
$t_2=(a_5,a_6,a_7,a_8{)}^T$
$t_3=(a_9,a_{10},a_{11},a_{12}{)}^T$
于是又有：
$t_1^{T}P-t_3^{T}P{u}_1=0,$
$t_2^{T}P-t_3^{T}P{v}_1=0.$

综上所述，每个特征点（3D-2D坐标对）提供了两个方程，也就是两个关于t的线性约束。
假设一共有N个特征点，$N\ge 6$ 时， 产生一个方程组：

$$\mathrm{A}\mathrm{x}=0$$
其中， $\mathbf{A}$ 的大小为 $2n\times 12$ 。这个方程无法求出精确解，但是可以获得一个 $|\mathbf{x}|=1$ 约束 下的最小二乘解 $\mathrm{argmin}\Vert \mathbf{A}\mathbf{x}{\Vert }^2$ (这部分可以参考： Ax=0求解以及MVG 教程) 。具体的， 对 $\mathbf{A}$ 进行SVD分解，可得
$$[\mathbf{U}\mathbf{\Sigma }\mathbf{V}]=\mathrm{SVD}(\mathbf{A})$$
$\mathbf{V}$ 矩阵的最后一列 $\overline{\mathbf{x}}$, 便为式 (6) 的解。注意，求解的结果是没有尺度的，也就是说实际的解 为
$$\mathbf{x}=\beta \overline{\mathbf{x}}$$
其中， $\beta$ 为比例系数， $\overline{\mathbf{x}}={\left[{\bar{a}}_1,\cdots ,{\bar{a}}_{12}\right]}^T$ 。旋转部分为
$$\overline{\mathbf{R}}=\left[\begin{array}{lll}{\bar{a}}_1& {\bar{a}}_2& {\bar{a}}_3\\ {\bar{a}}_4& {\bar{a}}_5& {\bar{a}}_6\\ {\bar{a}}_7& {\bar{a}}_8& {\bar{a}}_9\end{array}\right]$$
为带有尺度的正交矩阵，为求最优的旋转矩阵，将其进行SVD分解
$$[\mathbf{U}\mathbf{\Sigma }\mathbf{V}]=\mathrm{SVD}(\overline{\mathbf{R}})$$

最优的旋转矩阵即为
$$\mathbf{R}=\pm {\mathbf{U}\mathbf{V}}^T（为什么这样？后续证明之）$$
理论上， $\mathbf{\Sigma }$ 的对角线应该非常相近，取均值，求解得到比例系数为
$$\beta =\pm 1/(\mathrm{tr}(\mathbf{\Sigma })/3)$$
在加上一个限制条件，3D点应该在摄像机的前方
$$\lambda >0:\beta \left(x{\bar{a}}_9+y{\bar{a}}_{10}+z{\bar{a}}_{11}+{\bar{a}}_{12}\right)>0$$
可确定 $\beta$ 和 $\mathbf{R}$ 的 $\pm$ 符号。接着，可以求得平移向量
$$\mathbf{t}=\beta {\left[{\bar{a}}_4,{\bar{a}}_8,{\bar{a}}_{12}\right]}^T$$

《视觉14讲》原文：
t矩阵一共有12维，因此最少通过6对匹配的3D-2D特征点即可实现变换矩阵T的线性求解，这种方法就是DLT 直接线性变换法。 当匹配点大于6个时，使用SVD分解对超定方程求最小二乘解。如上述步骤所示。

鉴于在DLT求解过程中，我们忽略了R矩阵的正交约束，用DLT求出的结果不一定满足这个约束，是一个一般性的矩阵，平移向量比较好办，它属于三维向量空间。但是旋转矩阵属于SO(3)旋转流形，必须针对$[\mathbf{R}\mid \mathbf{t}]$左边3X3的矩阵块，寻找一个最好的旋转矩阵对他进行近似，可以借助QR分解，如下：
$R\leftarrow (R{R}^T{)}^{-\frac{1}{2}}R$
这就相当于把结果从矩阵空间重新投影到了SE(3)流形上，转换成旋转和平移两部分。

再次说明：通常SLAM问题内参K都是已知的，如果即使未知也可以使用PnP去估计K,R,t三个量。然而由于未知量增多，效果会差一些。

证明的补充：

background:
在ICP 问题中，就业涉及到SVD分解球旋转矩阵的问题，这里也遇到了类似的问题，为什么$\mathbf{R}=\pm {\mathbf{U}\mathbf{V}}^T.$就是最优的?

• 问题描述 之间的刚性转置信息，可以知道这其实是一个最小二乘求优问题，问题可以用如下计算式描述:
  $$(R,\mathbf{t})=\underset{R,\mathbf{t}}{\mathrm{armgin}}\sum _{i=1}^n{w}_i{\Vert \left(R{\mathbf{p}}_i+\mathbf{t}\right)-{\mathbf{q}}_i\Vert }^2\text{, }$$
  其中 $w_i>0$ ，是点集中每个点对的权重。
  要求(1)式中的最小值，即为求式中对 $\mathrm{R}$ 和求导数为 0 的解。

计算位移
将(1)式中的R设为不变量对进行求导，同时令 $F(t)=(R,t)$ ，对 $F(t)$ 求导可得:
$0=\frac{\partial F}{\partial \mathbf{t}}={\sum }_{i=1}^{n}2w_i\left(R{\mathbf{p}}_i+\mathbf{t}-{\mathbf{q}}_i\right)=$

$=2\mathrm{t}\left({\sum }_{i=1}^n{w}_i\right)+2R\left({\sum }_{i=1}^n{w}_i{\mathbf{p}}_i\right)-2{\sum }_{i=1}^n{w}_i{\mathbf{q}}_i.$

$\overline{\mathbf{p}}=\frac{{\sum }_{i=1}^n{w}_i{\mathbf{p}}_i}{{\sum }_{i=1}^n{w}_i}$ ,

$\overline{\mathbf{q}}=\frac{{\sum }_{i=1}^n{w}_i{\mathbf{q}}_i}{{\sum }_{i=1}^n{w}_i}$

$\mathbf{t}=\overline{\mathbf{q}}-R\overline{\mathbf{p}}$
在将(4)式代入(1)式可得:
$$\begin{array}{rl}\sum _{i=1}^n{w}_i{\Vert \left(R{\mathbf{p}}_i+\mathbf{t}\right)-{\mathbf{q}}_i\Vert }^2& =\sum _{i=1}^n{w}_i{\Vert R{\mathbf{p}}_i+\overline{\mathbf{q}}-R\overline{\mathbf{p}}-{\mathbf{q}}_i\Vert }^2=\\ & =\sum _{i=1}^n{w}_i{\Vert R\left({\mathbf{p}}_i-\overline{\mathbf{p}}\right)-\left({\mathbf{q}}_i-\overline{\mathbf{q}}\right)\Vert }^2\\ {\mathbf{x}}_i:={\mathbf{p}}_i-\overline{\mathbf{p}},{\mathbf{y}}_i:=& {\mathbf{q}}_i-\overline{\mathbf{q}}.——去质心坐标\\ R=\underset{R}{\mathrm{argmin}}\sum _{i=1}^n{w}_i{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2& \end{array}$$
从上可以看出，问题经过转化后变得更加简单，对原来的点集做一个减中心点的预处理，然后再求两个最小二乘的旋转量。

计算旋转量
将(8)式用矩阵表示形式展开，可得:
$$\begin{array}{rl}{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2& ={\left(R{\mathbf{x}}_i-{\mathbf{y}}_i\right)}^T\left(R{\mathbf{x}}_i-{\mathbf{y}}_i\right)=\left({\mathbf{x}}_i^T{R}^T-{\mathbf{y}}_i^T\right)\left(R{\mathbf{x}}_i-{\mathbf{y}}_i\right)=\\ & ={\mathbf{x}}_i^T{R}^{T}R{\mathbf{x}}_i-{\mathbf{y}}_i^{T}R{\mathbf{x}}_i-{\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i=\\ & ={\mathbf{x}}_i^T{\mathbf{x}}_i-{\mathbf{y}}_i^{T}R{\mathbf{x}}_i-{\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i.\end{array}$$
(由于旋转矩阵 $R$ 是正交矩阵，因而有 $R{R}^{\top }=1$ )。同时可以知道上式中 $y^{\top }Rx{x}^{\top }$ 和 $x{i}^{\top }{R}^{\top }y$ 都是标量，而一个标量的转置仍然等于标量本身，因而有:
$$\begin{array}{rl}& {\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i={\left({\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i\right)}^T={\mathbf{y}}_i^{T}R{\mathbf{x}}_i\\ & {\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2={\mathbf{x}}_i^T{\mathbf{x}}_i-2{\mathbf{y}}_i^{T}R{\mathbf{x}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i\end{array}$$
现在变成要求(11)式的最小值，而该式中只有一项与R有关，其他两项(xi $xi$ i和 $y{i}^{\top }yi$ 都是常量，所以问题转换为求其中一项可变量的最小值，即
$$\begin{array}{rl}& \underset{R}{\mathrm{argmin}}\left(-2\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{T}R{\mathbf{x}}_i\right).\\ & \underset{R}{\mathrm{argmin}}\left(-2\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{T}R{\mathbf{x}}_i\right)=\underset{R}{\mathrm{argmax}}\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{T}R{\mathbf{x}}_i.\end{array}$$
${\sum }_{i=1}^n{w}_i{\mathbf{y}}_i^{T}R{\mathbf{x}}_i=\mathrm{tr}\left(W{Y}^{T}RX\right)$

上式中的转换是将标量累加转换成矩阵相乘，其中W是 $n\times n$ 的对角矩阵，X和Y是 $3\times n$ 的矩阵，这些矩阵相乘后的迹就等于等式左边的值。同时，对于 矩阵的迹，有如下变换关系:
$$\mathrm{tr}(AB)=\mathrm{tr}(BA)$$
$$tr\left(W{Y}^{T}RX\right)=\mathrm{tr}\left(\left(W{Y}^T\right)(RX)\right)=\mathrm{tr}\left(RXW{Y}^T\right)$$
$$\mathrm{S}=X\mathrm{X}\mathrm{V}{Y}^{\top },\mathrm{svd}(\mathrm{S})==\Rightarrow S=U\Sigma V^T.$$
$$\mathrm{tr}\left(RXW{Y}^T\right)=\mathrm{tr}(RS)=\mathrm{tr}\left(RU\Sigma V^T\right)=\mathrm{tr}\left(\Sigma V^{T}RU\right)$$
(18)式中最后一步的变换也用到了(15)式的性质。由于 $U、R、V$ 都是正交矩阵，那么 $M=V^{\top }RU$ 也是正交矩阵。
$$1={\mathbf{m}}_j^T{\mathbf{m}}_j=\sum _{i=1}^d{m}_{ij}^2\Rightarrow m_{ij}\le 1\Rightarrow \left|m_{ij}\right|<1$$
$$\mathrm{tr}(\Sigma M)=\left(\begin{array}{lllll}{\sigma }_1& & & & \\ & {\sigma }_2& & & \\ & & \ddots & & \\ & & & {\sigma }_d& \end{array}\right)\left(\begin{array}{cccc}{m}_{11}& m_{12}& \dots & m_{1d}\\ m_{21}& m_{22}& \dots & m_{2d}\\ ⋮& ⋮& ⋮& ⋮\\ m_{d1}& m_{d2}& \dots & m_{dd}\end{array}\right)=\sum _{i=1}^d{\sigma }_i{m}_{ii}\le \sum _{i=1}^d{\sigma }_i$$
由上述两式可以知道，要求最大迹，就必须使得$m_{ii}$的值等于1，而M又是正交矩阵，那么M就必然是单位矩阵，即有
$I=M=V^{\top }RU\Rightarrow V=RU\Rightarrow R=V{U}^T$

该段证明来自：链接: link.

EPnP