Lambda Twist: An Accurate Fast Robust Perspective Three Point （P3P）Solver
特征点的3D可以由三角化或者RGBD相机获取。因此双目或RGBD的视觉里程计中可以直接使用PnP估计相机运动，而在单目视觉里程计中，必须先进行初始化然后才能使用PnP。
[
u
x
u
y
1
]
=
λ
K
⋅
[
R
t
]
[
x
y
z
1
]
\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]
⎣ ⎡ u x u y 1 ⎦ ⎤ = λ K ⋅ [ R t ] ⎣ ⎢ ⎢ ⎡ x y z 1 ⎦ ⎥ ⎥ ⎤
1.
[
x
y
z
1
]
\left[\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right]
⎣ ⎢ ⎢ ⎡ x y z 1 ⎦ ⎥ ⎥ ⎤
P
w
P_w
P w
2.
[
R
,
t
]
[R,t]
[ R , t ]
T
c
w
T_{cw}
T c w
3.
K
K
K
[
u
x
u
y
1
]
\left[\begin{array}{c} u_{x} \\ u_{y} \\ 1 \end{array}\right]
⎣ ⎡ u x u y 1 ⎦ ⎤
λ
\lambda
λ
4.怎么求两帧之间的相对位姿变换？
T
C
1
C
2
=
T
W
C
1
−
1
T
W
C
2
T_{C_1C_2} = T_{WC_1}^{-1}T_{WC_2}
T C 1 C 2 = T W C 1 − 1 T W C 2
即是，PnP只能求解当前帧到世界坐标系下的变换，要求取前后帧的变换，还需要经过一次转换。同时需要注意的是，
w
c
wc
w c
[
u
x
u
y
1
]
=
λ
K
⋅
[
R
t
]
[
x
y
z
1
]
=
λ
[
p
1
p
2
p
3
p
4
p
5
p
6
p
7
p
8
p
9
p
10
p
11
p
12
]
[
x
y
z
1
]
=
λ
[
p
0
T
p
1
T
p
2
T
]
x
\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}
⎣ ⎡ u x u y 1 ⎦ ⎤ = λ K ⋅ [ R t ] ⎣ ⎢ ⎢ ⎡ x y z 1 ⎦ ⎥ ⎥ ⎤ = λ ⎣ ⎡ p 1 p 5 p 9 p 2 p 6 p 1 0 p 3 p 7 p 1 1 p 4 p 8 p 1 2 ⎦ ⎤ ⎣ ⎢ ⎢ ⎡ x y z 1 ⎦ ⎥ ⎥ ⎤ = λ ⎣ ⎡ p 0 T p 1 T p 2 T ⎦ ⎤ x
⇒
u
x
=
p
0
T
x
p
2
T
x
,
u
y
=
p
1
T
x
p
2
T
x
⇒
{
p
0
T
x
−
p
2
T
x
⋅
u
x
=
0
p
1
T
x
−
p
2
T
x
⋅
u
y
=
0
\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.
⇒ u x = p 2 T x p 0 T x , u y = p 2 T x p 1 T x ⇒ { p 0 T x − p 2 T x ⋅ u x = 0 p 1 T x − p 2 T x ⋅ u y = 0
⇒
[
x
T
0
−
x
T
u
x
0
x
T
−
x
T
u
y
]
[
p
0
p
1
p
2
]
=
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}
⇒ [ x T 0 0 x T − x T u x − x T u y ] ⎣ ⎡ p 0 p 1 p 2 ⎦ ⎤ = 0
齐次最小二成得到
[
p
0
p
1
p
2
]
\left[\begin{array}{l} \boldsymbol{p}_{0} \\ \boldsymbol{p}_{1} \\ \boldsymbol{p}_{2} \end{array}\right]
⎣ ⎡ p 0 p 1 p 2 ⎦ ⎤
投影矩阵：
P
=
K
⋅
[
R
t
]
=
[
p
1
p
2
p
3
p
4
p
5
p
6
p
7
p
8
p
9
p
10
p
11
p
12
]
=
[
p
0
T
p
1
T
p
2
T
]
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 ⋅ [ R t ] = ⎣ ⎡ p 1 p 5 p 9 p 2 p 6 p 1 0 p 3 p 7 p 1 1 p 4 p 8 p 1 2 ⎦ ⎤ = ⎣ ⎡ p 0 T p 1 T p 2 T ⎦ ⎤
P
=
K
⋅
[
R
t
]
=
K
⋅
[
R
−
R
c
]
=
[
M
−
M
c
]
M
=
Q
R
⇒
K
,
R
⇒
t
\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 ⋅ [ R t ] = K ⋅ [ R − R c ] = [ M − M c ] M = Q R ⇒ K , R ⇒ t
参考十四讲.
λ
i
y
i
=
R
x
i
+
t
,
i
∈
{
1
,
2
,
3
}
\lambda_{i} \boldsymbol{y}_{i}=\mathbf{R} \boldsymbol{x}_{i}+\boldsymbol{t}, i \in\{1,2,3\}
λ i y i = R x i + t , i ∈ { 1 , 2 , 3 }
x
i
x_i
x i
y
i
y_i
y i
y
i
y_i
y i
∣
y
i
∣
=
1
\left|\boldsymbol{y}_{i}\right|=1
∣ y i ∣ = 1
λ
i
y
i
−
λ
j
y
j
=
R
(
x
i
−
x
j
)
,
⇒
\lambda_{i} \boldsymbol{y}_{i}-\lambda_{j} \boldsymbol{y}_{j}=\mathbf{R}\left(\boldsymbol{x}_{i}-\boldsymbol{x}_{j}\right),\Rightarrow
λ i y i − λ j y j = R ( x i − x j ) , ⇒
∣
λ
i
y
i
−
λ
j
y
j
∣
2
=
∣
x
i
−
x
j
∣
2
=
def
a
i
j
,
⇒
\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 y i − λ j y j ∣ ∣ 2 = ∣ x i − x j ∣ 2 = def a i j , ⇒
λ
i
2
+
λ
j
2
−
2
b
i
j
=
a
i
j
b
i
j
=
def
y
i
T
y
j
\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}
λ i 2 + λ j 2 − 2 b i j = a i j b i j = def y i T y j
i
j
i j
i j
λ
i
\lambda_i
λ i
b
i
j
b_{ij}
b i j
a
i
j
a_{ij}
a i j
i
i
i
j
j
j
x
i
x_i
x i
x
j
x_j
x j
a
i
j
>
0
,
∀
i
j
a_{i j}>0, \forall i j
a i j > 0 , ∀ i j
3.1.三个非齐次二次多项式
上式中深度参数
λ
i
\lambda_i
λ i
Λ
⊤
M
12
Λ
=
a
12
,
Λ
⊤
M
13
Λ
=
a
13
,
Λ
⊤
M
23
Λ
=
a
23
\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}
Λ ⊤ M 1 2 Λ = a 1 2 , Λ ⊤ M 1 3 Λ = a 1 3 , Λ ⊤ M 2 3 Λ = a 2 3
其中：
M
12
=
(
1
−
b
12
0
−
b
12
1
0
0
0
0
)
,
M
13
=
(
1
0
−
b
13
0
0
0
−
b
13
0
1
)
,
M
23
=
(
0
0
0
0
1
−
b
23
0
−
b
23
1
)
\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)
M 1 2 = ⎝ ⎛ 1 − b 1 2 0 − b 1 2 1 0 0 0 0 ⎠ ⎞ , M 1 3 = ⎝ ⎛ 1 0 − b 1 3 0 0 0 − b 1 3 0 1 ⎠ ⎞ , M 2 3 = ⎝ ⎛ 0 0 0 0 1 − b 2 3 0 − b 2 3 1 ⎠ ⎞
此时只关注当
λ
i
>
0
\lambda_i>0
λ i > 0
λ
1
,
λ
2
,
λ
3
>
0
\lambda_{1}, \lambda_{2}, \lambda_{3}>0
λ 1 , λ 2 , λ 3 > 0
3.2.两个齐次二次多项式
公式太多了，就不重复造轮子了，看论文吧。
整体算法流程：
