Lambda Twist: An Accurate Fast Robust Perspective Three Point (P3P) Solver

0.引言

• ECCV2018 paper.
• code1.
• code2.

特征点的3D可以由三角化或者RGBD相机获取。因此双目或RGBD的视觉里程计中可以直接使用PnP估计相机运动，而在单目视觉里程计中，必须先进行初始化然后才能使用PnP。
$\left[\begin{array}{c} u_{x} \\ u_{y} \\ 1 \end{array}\right]=\lambda K \cdot\left[\begin{array}{ll} R & t \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right]$⎣⎡​ux​uy​1​⎦⎤​=λK⋅[R​t​]⎣⎢⎢⎡​xyz1​⎦⎥⎥⎤​
首先需要弄清楚，3D-2D求解的结果是什么？

• 1.$\left[\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right]$⎣⎢⎢⎡​xyz1​⎦⎥⎥⎤​为上一帧特征点在世界坐标系下的坐标，$P_w$Pw​;

• 2.$[R,t]$[R,t]为world系到camera系的变换$T_{cw}$Tcw​;

• 3.$K$K为相机内参，$\left[\begin{array}{c} u_{x} \\ u_{y} \\ 1 \end{array}\right]$⎣⎡​ux​uy​1​⎦⎤​为下一帧的像素坐标系，$\lambda$λ为相机坐标系下的逆深度；

• 4.怎么求两帧之间的相对位姿变换？
  a.把上一帧当做世界坐标系
  b.$T_{C_1C_2} = T_{WC_1}^{-1}T_{WC_2}$TC1​C2​​=TWC1​−1​TWC2​​

即是，PnP只能求解当前帧到世界坐标系下的变换，要求取前后帧的变换，还需要经过一次转换。同时需要注意的是，$wc$wc是更常用的！

1.DLT求解PnP

$\left[\begin{array}{c} u_{x} \\ u_{y} \\ 1 \end{array}\right]=\lambda K \cdot\left[\begin{array}{ll} R & t \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right]=\lambda\left[\begin{array}{cccc} p_{1} & p_{2} & p_{3} & p_{4} \\ p_{5} & p_{6} & p_{7} & p_{8} \\ p_{9} & p_{10} & p_{11} & p_{12} \end{array}\right]\left[\begin{array}{c} x \\ y \\ z \\ 1 \end{array}\right]=\lambda\left[\begin{array}{c} \boldsymbol{p}_{0}^{T} \\ \boldsymbol{p}_{1}^{T} \\ \boldsymbol{p}_{2}^{T} \end{array}\right] \mathbf{x}$⎣⎡​ux​uy​1​⎦⎤​=λK⋅[R​t​]⎣⎢⎢⎡​xyz1​⎦⎥⎥⎤​=λ⎣⎡​p1​p5​p9​​p2​p6​p10​​p3​p7​p11​​p4​p8​p12​​⎦⎤​⎣⎢⎢⎡​xyz1​⎦⎥⎥⎤​=λ⎣⎡​p0T​p1T​p2T​​⎦⎤​x

$\Rightarrow u_{x}=\frac{\boldsymbol{p}_{0}^{T} \mathbf{x}}{\boldsymbol{p}_{2}^{T} \mathbf{x}}, u_{y}=\frac{\boldsymbol{p}_{1}^{T} \mathbf{x}}{\boldsymbol{p}_{2}^{T} \mathbf{x}} \Rightarrow\left\{\begin{array}{l} \boldsymbol{p}_{0}^{T} \mathbf{x}-\boldsymbol{p}_{2}^{T} \mathbf{x} \cdot u_{x}=0 \\ \boldsymbol{p}_{1}^{T} \mathbf{x}-\boldsymbol{p}_{2}^{T} \mathbf{x} \cdot u_{y}=0 \end{array}\right.$⇒ux​=p2T​xp0T​x​,uy​=p2T​xp1T​x​⇒{p0T​x−p2T​x⋅ux​=0p1T​x−p2T​x⋅uy​=0​

$\Rightarrow\left[\begin{array}{ccc} \mathbf{x}^{T} & 0 & -\mathbf{x}^{T} u_{x} \\ 0 & \mathbf{x}^{T} & -\mathbf{x}^{T} u_{y} \end{array}\right]\left[\begin{array}{c} \boldsymbol{p}_{0} \\ \boldsymbol{p}_{1} \\ \boldsymbol{p}_{2} \end{array}\right]=\boldsymbol{0}$⇒[xT0​0xT​−xTux​−xTuy​​]⎣⎡​p0​p1​p2​​⎦⎤​=0

齐次最小二成得到$\left[\begin{array}{l} \boldsymbol{p}_{0} \\ \boldsymbol{p}_{1} \\ \boldsymbol{p}_{2} \end{array}\right]$⎣⎡​p0​p1​p2​​⎦⎤​

投影矩阵：
$P=K \cdot[R t]=\left[\begin{array}{cccc} p_{1} & p_{2} & p_{3} & p_{4} \\ p_{5} & p_{6} & p_{7} & p_{8} \\ p_{9} & p_{10} & p_{11} & p_{12} \end{array}\right]=\left[\begin{array}{c} \boldsymbol{p}_{0}^{T} \\ \boldsymbol{p}_{1}^{T} \\ \boldsymbol{p}_{2}^{T} \end{array}\right]$P=K⋅[Rt]=⎣⎡​p1​p5​p9​​p2​p6​p10​​p3​p7​p11​​p4​p8​p12​​⎦⎤​=⎣⎡​p0T​p1T​p2T​​⎦⎤​

$\begin{array}{l} P=K \cdot[R t]=K \cdot[R-R c]=[M-M c] \\ M=Q R \Rightarrow K, R \Rightarrow t \end{array}$P=K⋅[Rt]=K⋅[R−Rc]=[M−Mc]M=QR⇒K,R⇒t​

2.P3P求解

参考十四讲.

3.Lambda Twist

$\lambda_{i} \boldsymbol{y}_{i}=\mathbf{R} \boldsymbol{x}_{i}+\boldsymbol{t}, i \in\{1,2,3\}$λi​yi​=Rxi​+t,i∈{1,2,3}
其中，$x_i$xi​为3D点，$y_i$yi​为2D点。同时将$y_i$yi​归一化处理：$\left|\boldsymbol{y}_{i}\right|=1$∣yi​∣=1。则有：
$\lambda_{i} \boldsymbol{y}_{i}-\lambda_{j} \boldsymbol{y}_{j}=\mathbf{R}\left(\boldsymbol{x}_{i}-\boldsymbol{x}_{j}\right),\Rightarrow$λi​yi​−λj​yj​=R(xi​−xj​),⇒

$\left|\lambda_{i} \boldsymbol{y}_{i}-\lambda_{j} \boldsymbol{y}_{j}\right|^{2}=\left|\boldsymbol{x}_{i}-\boldsymbol{x}_{j}\right|^{2} \stackrel{\text {def}}{=} a_{i j},\Rightarrow$∣∣​λi​yi​−λj​yj​∣∣​2=∣xi​−xj​∣2=defaij​,⇒$\begin{array}{l} \lambda_{i}^{2}+\lambda_{j}^{2}-2 b_{i j}=a_{i j} \\ b_{i j} \stackrel{\text {def}}{=} \boldsymbol{y}_{i}^{T} \boldsymbol{y}_{j} \end{array}$λi2​+λj2​−2bij​=aij​bij​=defyiT​yj​​$i j$ij i.e. {12,13,23}。此时，就只剩下$\lambda_i$λi​为变量。式中$b_{ij}$bij​与$a_{ij}$aij​几何意义分别为，像素点$i$i,$j$j的投影光线夹角的余弦，三维点$x_i$xi​,$x_j$xj​之间的距离平方。如果3D不共线，则$a_{i j}>0, \forall i j$aij​>0,∀ij

3.1.三个非齐次二次多项式

上式中深度参数$\lambda_i$λi​的约束可以表达为：

$\boldsymbol{\Lambda}^{\top} \mathbf{M}_{12} \boldsymbol{\Lambda}=a_{12}, \quad \boldsymbol{\Lambda}^{\top} \mathbf{M}_{13} \boldsymbol{\Lambda}=a_{13}, \quad \boldsymbol{\Lambda}^{\top} \mathbf{M}_{23} \boldsymbol{\Lambda}=a_{23}$Λ⊤M12​Λ=a12​,Λ⊤M13​Λ=a13​,Λ⊤M23​Λ=a23​

其中：$\mathbf{M}_{12}=\left(\begin{array}{ccc} 1 & -b_{12} & 0 \\ -b_{12} & 1 & 0 \\ 0 & 0 & 0 \end{array}\right), \mathbf{M}_{13}=\left(\begin{array}{rcc} 1 & 0 & -b_{13} \\ 0 & 0 & 0 \\ -b_{13} & 0 & 1 \end{array}\right), \mathbf{M}_{23}=\left(\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 1 & -b_{23} \\ 0 & -b_{23} & 1 \end{array}\right)$M12​=⎝⎛​1−b12​0​−b12​10​000​⎠⎞​,M13​=⎝⎛​10−b13​​000​−b13​01​⎠⎞​,M23​=⎝⎛​000​01−b23​​0−b23​1​⎠⎞​

此时只关注当$\lambda_i>0$λi​>0时的解。同时舍去复数解，$\lambda_{1}, \lambda_{2}, \lambda_{3}>0$λ1​,λ2​,λ3​>0.

3.2.两个齐次二次多项式

公式太多了，就不重复造*了，看论文吧。

整体算法流程：