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

PnP问题的DLT解法

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

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

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

我们记

$[R|t]= \begin{bmatrix} 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{bmatrix}$

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

目前已知相机内参矩阵
$K= \begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix}$

还已知在上一帧相机座标系下的三维齐次座标
$Q=\begin{bmatrix}x\\y\\z\\ 1\end{bmatrix}$
还已知对应点在当前帧的二维座标

$q=\begin{bmatrix}u\\v\\1\end{bmatrix}$
将式子$(2),(3),(4),(5)$代入到式$(1)$中得到：
$K[R|t]Q = \begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix} \begin{bmatrix} 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{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1 \end{bmatrix} =\lambda \begin{bmatrix}u\\ v\\1\end{bmatrix}$
进行矩阵乘法得到：
$\begin{bmatrix} f_xa_{11}+c_xa_{31}&f_xa_{12}+c_xa_{32}&f_xa_{13}+c_xa_{33}&f_xa_{14}+c_xa_{34}&\\ f_ya_{21}+c_ya_{31}&f_ya_{22}+c_ya_{32}&f_ya_{23}+c_ya_{33}&f_ya_{24}+c_ya_{34}&\\ a_{31}&a_{32}&a_{33}&a_{34} \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1 \end{bmatrix}\\ =\begin{bmatrix} x(f_xa_{11}+c_xa_{31})+y(f_xa_{12}+c_xa_{32})+z(f_xa_{13}+c_xa_{33})+(f_xa_{14}+c_xa_{34})\\ x(f_ya_{21}+c_ya_{31})+y(f_ya_{22}+c_ya_{32})+z(f_ya_{23}+c_ya_{33})+(f_ya_{24}+c_ya_{34})\\ xa_{31}+ya_{32}+za_{33}+a_{34} \end{bmatrix} =\lambda \begin{bmatrix} u\\ v\\ 1 \end{bmatrix}$
根据式子$(7)$我们就得到了三个方程。我们将最后一行代入前两行消除$\lambda$可以得到：
$\begin{bmatrix} x(f_xa_{11}+c_xa_{31})+y(f_xa_{12}+c_xa_{32})+z(f_xa_{13}+c_xa_{33})+(f_xa_{14}+c_xa_{34})\\ x(f_ya_{21}+c_ya_{31})+y(f_ya_{22}+c_ya_{32})+z(f_ya_{23}+c_ya_{33})+(f_ya_{24}+c_ya_{34})\\ \end{bmatrix}= \begin{bmatrix} uxa_{31}+uya_{32}+uza_{33}+ua_{34}\\ vxa_{31}+vya_{32}+vza_{33}+va_{34}\\ \end{bmatrix}$
整理得到：
$\begin{matrix} xf_xa_{11}+yf_xa_{12}+zf_xa_{13}+f_xa_{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\\ xf_ya_{21}+yf_ya_{22}+zf_ya_{23}+f_ya_{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{matrix}$
所以一对点对应两个方程。需要6对点才能解该方程。

式子$(9)$写成矩阵的形式得到：
$\begin{bmatrix} xf_x&yf_x&zf_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&xf_y&yf_y&zf_y&f_y&x(c_y-v)&y(c_y-v)&z(c_y-v)&(c_y-v)\\ \end{bmatrix} \begin{bmatrix} 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{bmatrix} =\bold 0$
这就变成了求解$Ax=0$问题。在线性代数里面有很多方式可以求解这个方程。

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

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

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

$c(xa_{31}+ya_{32}+za_{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)