如何理解PnP问题的DLT解法以及Python编程实践

PnP问题的DLT解法

欢迎关注知乎@司南牧
已知：上一帧相机坐标系下的点的三维齐次坐标$Q$，和，那$n$个点在当前帧中的二维齐次坐标$q$, 和相机内参矩阵$K$。

待求解变量 :当前帧相对上一帧的位姿变换矩阵$[R|t]$ 。

约束方程：
$$K[R|t]Q=\lambda q$$

我们记

$$[R|t]=\left[\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& a_{14}\\ a_{21}& a_{22}& a_{23}& a_{24}\\ a_{31}& a_{32}& a_{33}& a_{34}\end{array}\right]$$

DLT的思路就是把$a_{ij}$代入式$(1)$，然后求出$a_{ij}$就可以求出$[R|t]$。可以看到]现在我们有12个未知数一对点只能提供两个方程，所以需要6对点。

目前已知相机内参矩阵
$$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

还已知在上一帧相机坐标系下的三维齐次坐标
$$Q=\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$
还已知对应点在当前帧的二维坐标

$$q=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$
将式子$(2),(3),(4),(5)$代入到式$(1)$中得到：
$$K[R|t]Q=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& a_{14}\\ a_{21}& a_{22}& a_{23}& a_{24}\\ a_{31}& a_{32}& a_{33}& a_{34}\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\lambda \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$
进行矩阵乘法得到：
$$\begin{gathered} \left[\begin{array}{ccccc}{f}_x{a}_{11}+c_x{a}_{31}& f_x{a}_{12}+c_x{a}_{32}& f_x{a}_{13}+c_x{a}_{33}& f_x{a}_{14}+c_x{a}_{34}& \\ f_y{a}_{21}+c_y{a}_{31}& f_y{a}_{22}+c_y{a}_{32}& f_y{a}_{23}+c_y{a}_{33}& f_y{a}_{24}+c_y{a}_{34}& \\ a_{31}& a_{32}& a_{33}& a_{34}& \end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right] \\ =\left[\begin{array}{c}x(f_x{a}_{11}+c_x{a}_{31})+y(f_x{a}_{12}+c_x{a}_{32})+z(f_x{a}_{13}+c_x{a}_{33})+(f_x{a}_{14}+c_x{a}_{34})\\ x(f_y{a}_{21}+c_y{a}_{31})+y(f_y{a}_{22}+c_y{a}_{32})+z(f_y{a}_{23}+c_y{a}_{33})+(f_y{a}_{24}+c_y{a}_{34})\\ x{a}_{31}+y{a}_{32}+z{a}_{33}+a_{34}\end{array}\right]=\lambda \left[\begin{array}{c}u\\ v\\ 1\end{array}\right] \end{gathered}$$
根据式子$(7)$我们就得到了三个方程。我们将最后一行代入前两行消除$\lambda$可以得到：
$$\left[\begin{array}{c}x(f_x{a}_{11}+c_x{a}_{31})+y(f_x{a}_{12}+c_x{a}_{32})+z(f_x{a}_{13}+c_x{a}_{33})+(f_x{a}_{14}+c_x{a}_{34})\\ x(f_y{a}_{21}+c_y{a}_{31})+y(f_y{a}_{22}+c_y{a}_{32})+z(f_y{a}_{23}+c_y{a}_{33})+(f_y{a}_{24}+c_y{a}_{34})\end{array}\right]=\left[\begin{array}{c}ux{a}_{31}+uy{a}_{32}+uz{a}_{33}+u{a}_{34}\\ vx{a}_{31}+vy{a}_{32}+vz{a}_{33}+v{a}_{34}\end{array}\right]$$
整理得到：
$$\begin{array}{c}x{f}_x{a}_{11}+y{f}_x{a}_{12}+z{f}_x{a}_{13}+f_x{a}_{14}+x(c_x-u)a_{31}+y(c_x-u)a_{32}+z(c_x-u)a_{33}+(c_x-u)a_{34}=0\\ x{f}_y{a}_{21}+y{f}_y{a}_{22}+z{f}_y{a}_{23}+f_y{a}_{24}+x(c_y-v)a_{31}+y(c_y-v)a_{32}+z(c_y-v)a_{33}+(c_y-v)a_{34}=0\end{array}$$
所以一对点对应两个方程。需要6对点才能解该方程。

式子$(9)$写成矩阵的形式得到：
$$\left[\begin{array}{cccccccccccc}x{f}_x& y{f}_x& z{f}_x& f_x& 0& 0& 0& 0& x(c_x-u)& y(c_x-u)& z(c_x-u)& (c_x-u)\\ 0& 0& 0& 0& x{f}_y& y{f}_y& z{f}_y& f_y& x(c_y-v)& y(c_y-v)& z(c_y-v)& (c_y-v)\end{array}\right]\left[\begin{array}{c}{a}_{11}\\ a_{12}\\ a_{13}\\ a_{14}\\ a_{21}\\ a_{22}\\ a_{23}\\ a_{24}\\ a_{31}\\ a_{32}\\ a_{33}\\ a_{34}\end{array}\right]=0$$
这就变成了求解$Ax=0$问题。在线性代数里面有很多方式可以求解这个方程。

在SLAM中常用的方法是这样看$Ax=0x$，所以只用求矩阵$A$特征值为0所对应的特征向量。用SVD对矩阵A分解得到$A=UDV$，其中$V$的最后一列就是$Ax=0$的解。

根据此时求出的x可以算出$[R^{\prime }|t^{\prime }]$，但是Ax=0它也可以看做是Acx=c0，所以此时求出的x它是真实的[R|t]乘上一个常数后得到的结果。我们需要计算出那个常数。而且求出的$R^{\prime }$并不是一个正交矩阵。而我们的约束条件要求$R^{\prime }$是一个正交矩阵。为了将它变成一个正交矩阵需要对求得的$R^{\prime }$进行SVD分解让让其变成正交矩阵
$$U^{\prime }{D}^{\prime }{V}^{\prime }=svd(R^{\prime })$$
其中$U^{\prime },V^{\prime }$都是正交矩阵，$D^{\prime }$是对角矩阵，为了让$R^{\prime }$变成正交矩阵我们需要让$D^{\prime }$对角线元素全部相等。
$$\begin{gathered} E^{\prime }=dia(\frac{tr(D^{\prime })}{3}) \\ {R}^{\prime }=U^{\prime }{E}^{\prime }{V}^{\prime } \end{gathered}$$
而且旋转矩阵R还有一个性质就是行列式为1.所以放缩因子就是$c=\pm \frac{tr(D^{\prime })}{3}$。到底取正还是负有两种方式：

1. 代入式子$(12)$计算$R^{\prime }$的行列式看是否大于0
2. $\lambda$它是点在相机坐标系下的z轴方向的坐标值，而点肯定是在相机的前方，所以$\lambda >0$。我们计算下面这个式子看是否大于0.（推荐使用这个，因为计算量相对较小）

$$c(x{a}_{31}+y{a}_{32}+z{a}_{33}+a_{34})=\lambda >0$$

编程实践

Python代码

import numpy as np

fx = 1
fy = 1
cx = 0
cy= 0
K = np.array([
        [fx,0,cx],
        [0,fy,cy],
        [0,0,1]
        ])
Rt_groundtruth = np.array([
        [1,0,0,3],
        [0,1,0,3],
        [0,0,1,3]
        ])
Qn = np.random.rand(4,6)
qn = K @ Rt_groundtruth @ Qn

#对qn 归一化
qn = qn/qn[-1]

zeros_nx4 = np.zeros((Qn.shape[1],4))

A_up = np.column_stack((fx*Qn.T,zeros_nx4, Qn.T * np.expand_dims((cx-qn[0]).T,1)))
A_below = np.column_stack((zeros_nx4,fy*Qn.T, Qn.T * np.expand_dims((cy-qn[1]).T,1)))
A = np.row_stack((A_up,A_below))
U,D,V=np.linalg.svd(A)
x = V[-1]
Rt = x.reshape((3,4))
R = Rt[:3,:3]
t = Rt[:3,-1]

U,D,V = np.linalg.svd(R)
c=1/(np.sum(D)/3)

# 验证c的正负是否正确
Q_verify = np.random.rand(4,1)
q_verify = K @ Rt_groundtruth @ Q_verify
if np.sum(c*(Q_verify.T*Rt[-1]))<0:
    c = -c


D = np.diag(D*c)
R = U @ D @ V
print(R)
print(t*c)