视觉SLAM十四讲学习5 位姿估计（5）PNP方法，BA

前言

本篇记录PnP中的BA光束平差法。

重投影误差

对于3D-2D的位姿估计，还可以通过最小化重投影误差实现。重投影误差公式如下： ∑ ∣ ∣ e ∣ ∣ = ∑ ∣ ∣ x − 1 s K T P ∣ ∣ P = [ X , Y , Z ] \sum ||e||=\sum ||x-\frac{1}{s}KTP|| \\ P=[X,Y,Z] ∑∣∣e∣∣=∑∣∣x−s1​KTP∣∣P=[X,Y,Z]

一般，对于向量范数的最小化，可以转化为最小二乘问题： arg min ⁡ T ∑ 1 2 ∣ ∣ e ∣ ∣ 2 = arg min ⁡ T ∑ 1 2 ∣ ∣ x − 1 Z K T P ∣ ∣ 2 \argmin_{T} \sum \frac {1}{2} ||e||^2=\argmin_{T} \sum \frac{1}{2}||x-\frac{1}{Z}KTP||^2 Targmin​∑21​∣∣e∣∣2=Targmin​∑21​∣∣x−Z1​KTP∣∣2

对于非线性最小二乘问题，可以转化为非线性优化问题，通过牛顿法，拟牛顿法，共轭梯度等方法解决。这里使用高斯牛顿法求解： Δ = − ( J T J ) − 1 J T f \Delta =-(J^TJ)^{-1}J^Tf Δ=−(JTJ)−1JTf

那么，现在BA的问题就转变为求梯度 J T = ∇ e J^T =\nabla e JT=∇e的问题。

重投影误差关于位姿的梯度

设 , P ′ = T P [ 1 : 3 ] = R P + t = [ X ′ , Y ′ , Z ′ ] , x = [ u , v ] T , s = Z ′ , P'=TP_{[1:3]}=RP+t=[X',Y',Z'],x=[u,v]^T,s=Z' ,P′=TP[1:3]​=RP+t=[X′,Y′,Z′],x=[u,v]T,s=Z′，则： e = x − 1 s K P ′ ∇ e = ∂ e ∂ P ′ ∂ P ′ ∂ T e=x-\frac{1}{s}KP' \\ \quad \\ \nabla e=\frac{\partial e}{\partial P'} \frac{\partial P'}{\partial T} e=x−s1​KP′∇e=∂P′∂e​∂T∂P′​ 梯度的前一项可以根据相机模型计算： e = x − [ 1 Z ′ f x 0 1 Z ′ c x 0 1 Z ′ f y 1 Z ′ c y ] [ X ′ , Y ′ , Z ′ ] T [ u v ] − [ 1 Z ′ f x X ′ + c x 1 Z ′ f y Y ′ + c y ] ∂ e ∂ P ′ = − [ 1 Z ′ f x 0 0 1 Z ′ f y − 1 Z ′ 2 f x X ′ − 1 Z ′ 2 f y Y ′ ] T e = x - \begin{bmatrix} \frac{1}{Z'}f_x & 0 & \frac{1}{Z'}c_x \\ 0 & \frac{1}{Z'}f_y & \frac{1}{Z'}c_y \\ \end{bmatrix}[X',Y',Z']^T \\ \quad \\ \begin{bmatrix}u \\ v \end{bmatrix} - \begin{bmatrix} \frac{1}{Z'}f_xX' + c_x \\ \frac{1}{Z'}f_yY' + c_y \\ \end{bmatrix} \\ \quad \\ \frac {\partial e}{\partial P'} = -\begin{bmatrix} \frac{1}{Z'}f_x & 0 \\ 0 & \frac{1}{Z'}f_y \\ -\frac{1}{Z'^2}f_xX' & -\frac{1}{Z'^2}f_yY' \\ \end{bmatrix}^T \\ e=x−[Z′1​fx​0​0Z′1​fy​​Z′1​cx​Z′1​cy​​][X′,Y′,Z′]T[uv​]−[Z′1​fx​X′+cx​Z′1​fy​Y′+cy​​]∂P′∂e​=−⎣⎡​Z′1​fx​0−Z′21​fx​X′​0Z′1​fy​−Z′21​fy​Y′​⎦⎤​T

梯度的后一项，由于变换矩阵 T T T不好求导，因此可以借助李代数的左乘扰动模型求导： ∂ P ′ ∂ T = ∂ ( ( T P ) [ 1 : 3 ] ) ∂ T = ( ∂ ( e δ ∧ P ) ∂ Δ δ ) [ 1 : 3 ] = [ I − ( R P + t ) ∧ ] = [ I − P ′ ∧ ] \frac{\partial P'}{\partial T}=\frac{\partial ((TP)_{[1:3]})}{\partial T} \\ \quad \\ = (\frac {\partial (e^{\delta^\land}P)}{\partial \Delta\delta})_{[1:3]} \\ \quad \\ = \begin{bmatrix} I & -(RP+t)^\land \end{bmatrix} \\ \quad \\ = \begin{bmatrix} I & -P'^\land \end{bmatrix} \\ ∂T∂P′​=∂T∂((TP)[1:3]​)​=(∂Δδ∂(eδ∧P)​)[1:3]​=[I​−(RP+t)∧​]=[I​−P′∧​] 于是，总梯度就出来了： ∇ e = − [ 1 Z ′ f x 0 0 1 Z ′ f y − 1 Z ′ 2 f x X ′ − 1 Z ′ 2 f y Y ′ ] T [ I − P ′ ∧ ] = − [ 1 Z ′ f x 0 − 1 Z ′ 2 f x X ′ 0 1 Z ′ f y − 1 Z ′ 2 f y Y ′ ] [ I − P ′ ∧ ] = − [ A B ] [ I − P ′ ∧ ] = − [ A − A P ′ ∧ B − B P ′ ∧ ] \nabla e = -\begin{bmatrix} \frac{1}{Z'}f_x & 0 \\ 0 & \frac{1}{Z'}f_y \\ -\frac{1}{Z'^2}f_xX' & -\frac{1}{Z'^2}f_yY' \\ \end{bmatrix}^T \begin{bmatrix} I & -P'^\land \end{bmatrix} \\ \quad \\ = -\begin{bmatrix} \frac{1}{Z'}f_x & 0 & -\frac{1}{Z'^2}f_xX' \\ 0 & \frac{1}{Z'}f_y & -\frac{1}{Z'^2}f_yY' \\ \end{bmatrix} \begin{bmatrix} I & -P'^\land \end{bmatrix} \\ \quad \\ = -\begin{bmatrix} A \\ B \end{bmatrix} \begin{bmatrix} I & -P'^\land \end{bmatrix} = -\begin{bmatrix} A & -AP'^\land \\ B & -BP'^\land \end{bmatrix} \\ \quad \\ ∇e=−⎣⎡​Z′1​fx​0−Z′21​fx​X′​0Z′1​fy​−Z′21​fy​Y′​⎦⎤​T[I​−P′∧​]=−[Z′1​fx​0​0Z′1​fy​​−Z′21​fx​X′−Z′21​fy​Y′​][I​−P′∧​]=−[AB​][I​−P′∧​]=−[AB​−AP′∧−BP′∧​] 展开后长这样： 这样就得到了关于位姿的梯度。

重投影误差关于点的梯度

BA不仅可以用于位姿的优化，也可以对点进行优化，此时的重投影误差最小二乘变成了： arg min ⁡ T , P ∑ 1 2 ∣ ∣ e ∣ ∣ 2 \argmin_{T,P} \sum \frac {1}{2} ||e||^2 T,Pargmin​∑21​∣∣e∣∣2 关于点的梯度： ∇ e = ∂ e ∂ P ′ ∂ P ′ ∂ P = ∂ e ∂ P ′ ∂ ( R P + t ) ∂ P = − [ 1 Z ′ f x 0 − 1 Z ′ 2 f x X ′ 0 1 Z ′ f y − 1 Z ′ 2 f y Y ′ ] R \nabla e = \frac {\partial e}{\partial P'} \frac {\partial P'}{\partial P} = \frac {\partial e}{\partial P'} \frac {\partial (RP+t)}{\partial P} \\ \quad \\ = -\begin{bmatrix} \frac{1}{Z'}f_x & 0 & -\frac{1}{Z'^2}f_xX' \\ 0 & \frac{1}{Z'}f_y & -\frac{1}{Z'^2}f_yY' \\ \end{bmatrix} R ∇e=∂P′∂e​∂P∂P′​=∂P′∂e​∂P∂(RP+t)​=−[Z′1​fx​0​0Z′1​fy​​−Z′21​fx​X′−Z′21​fy​Y′​]R

BA

得到上面这两个梯度之后，代入高斯牛顿法的增量迭代公式，直到达成收敛条件： ∇ e = J T Δ δ = − ( J T J ) − 1 J T f T ′ = e Δ δ T u n t i l ∣ ∣ Δ δ ∣ ∣ < α o r ∣ ∣ ∇ e ∣ ∣ < β \nabla e = J^T \\ \Delta \delta =-(J^TJ)^{-1}J^Tf \\ T' = e^{\Delta \delta} T \\ \quad \\ until \quad ||\Delta \delta||