如何理解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)