(01)ORB-SLAM2源码无死角解析-(39) EPnP 算法原理详解→理论基础三:高斯牛顿迭代
讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
通过上一篇博客以及分情况求得 β 10 × 1 \boldsymbol{\beta}_{10 \times 1} β10×1 的初始解。但是很明显这个解并不是我们需要的最终解，那么如何使得 初始解 转换成 最终解 呢？通过前面的博客，可以知道其目标是优化两个坐标系下控制点间距的差，使得其误差最小，如下所示： f ( β ) = ∑ ( i , j s.t. i < j ) ( ∥ c i c − c j c ∥ 2 − ∥ c i w − c j w ∥ 2 ) (01) \color{Green} \tag{01} f(\boldsymbol{\beta})=\sum_{(i, j~\text {s.t. } i
推荐 \color{red}推荐 推荐：史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解
二、原理推导
( 1 ) : \color{blue}{(1)}: (1): 假设已知 m m m 个点，如下：
( x 1 , y 1 ) , ( x 2 , y 2 ) , ⋯ , ( x m , y m ) (04) \color{Green} \tag{04}(x_1,y_1), (x_2,y_2),\cdots,(x_m,y_m) (x1,y1),(x2,y2),⋯,(xm,ym)(04)
( 2 ) : \color{blue}{(2)}: (2):令 n = < m n=
( 3 ) : \color{blue}{(3)}: (3):目的是找到最优解 β \boldsymbol{\beta} β,也就是残差平方和 S = ∑ i = 1 m r i 2 S=\sum_{i=1}^{m} r_{i}^{2} S=∑i=1mri2 最小: r i = y i − f ( x i , β ) i = 1 , 2 , 3 , ⋯ , m (06) \color{Green} \tag{06} r_{i}=y_{i}-f\left(x_{i}, \boldsymbol{\beta}\right)\\ i=1,2,3,\cdots,m ri=yi−f(xi,β)i=1,2,3,⋯,m(06)
( 4 ) : \color{blue}{(4)}: (4):要求最小值，即 S S S 对 β \boldsymbol{\beta} β 求偏导数等于0： ∂ S ∂ β j = 2 ∑ i r i ∂ r i ∂ β j = 0 ( j = 1 , … , n ) (07) \color{Green} \tag{07} \frac{\partial S}{\partial \beta_{j}}=2 \sum_{i} r_{i} \frac{\partial r_{i}}{\partial \beta_{j}}=0 \quad(j=1, \ldots, n) ∂βj∂S=2i∑ri∂βj∂ri=0(j=1,…,n)(07)
( 5 ) : \color{blue}{(5)}: (5): 在非线性系统中， ∂ r i ∂ β j \frac{\partial r_{i}}{\partial \beta_{j}} ∂βj∂ri 是变量和参数的函数，没有闭解。因此给定一个初始值，用迭代法逼近解： β j ≈ β j k + 1 = β j k + Δ β j (08) \color{Green} \tag{08} \beta_{j} \approx \beta_{j}^{k+1}=\beta_{j}^{k}+\Delta \beta_{j} βj≈βjk+1=βjk+Δβj(08)其中 K K K 是迭代次数， Δ β j \Delta \boldsymbol{\beta_j} Δβj 是迭代矢量。
( 6 ) : \color{blue}{(6)}: (6): 而每次迭代函数是线性的，在 β k \beta^{k} βk 处用泰勒级数展开：
f ( x i , β ) ≈ f ( x i , β k ) + ∑ j = 1 n ∂ f ( x i , β k ) ∂ β j ( β j − β j k ) ≈ f ( x i , β k ) + ∑ j = 1 n J i j Δ β j . (09) \color{Green} \tag{09} f\left(x_{i}, \boldsymbol{\beta}\right) \approx f\left(x_{i}, \boldsymbol{\beta}^{k}\right)+\sum_{j=1}^{n} \frac{\partial f\left(x_{i}, \boldsymbol{\beta}^{k}\right)}{\partial \beta_{j}}\left(\beta_{j}-\beta_{j}^{k}\right) \approx f\left(x_{i}, \boldsymbol{\beta}^{k}\right)+\sum_{j=1}^{n} J_{i j} \Delta \beta_{j} . f(xi,β)≈f(xi,βk)+j=1∑n∂βj∂f(xi,βk)(βj−βjk)≈f(xi,βk)+j=1∑nJijΔβj.(09)其中 J \mathbf J J 是已知矩阵，为了迭代方便，令: ∂ r i ∂ β j = − J i j \frac{\partial r_{i}}{\partial \beta_{j}}=- J_{ij} ∂βj∂ri=−Jij
( 7 ) : \color{blue}{(7)}: (7): 此时残差表示为： Δ y i = y i − f ( x i , β k ) r i = y i − f ( x i , β ) = ( y i − f ( x i , β k ) ) + ( f ( x i , β k ) − f ( x i , β ) ) = Δ y i − ∑ j = 1 n J i j Δ β j (10) \color{Green} \tag{10} \Delta y_{i}=y_{i}-f\left(x_{i}, \beta^{k}\right)\\r_{i}=y_{i}-f\left(x_{i}, \boldsymbol{\beta}\right)=\left(y_{i}-f\left(x_{i}, \boldsymbol{\beta}^{k}\right)\right)+\left(f\left(x_{i}, \boldsymbol{\beta}^{k}\right)-f\left(x_{i}, \boldsymbol{\beta}\right)\right)=\Delta y_{i}-\sum_{j=1}^{n} J_{i j} \Delta \beta_{j} Δyi=yi−f(xi,βk)ri=yi−f(xi,β)=(yi−f(xi,βk))+(f(xi,βk)−f(xi,β))=Δyi−j=1∑nJijΔβj(10)
( 8 ) : \color{blue}{(8)}: (8): 现在将公式(09)与(10)带入到公式(07)有: − 2 ∑ i = 1 m J i j ( Δ y i − ∑ j = 1 n J i j Δ β j ) = 0 (11) \color{Green} \tag{11} -2 \sum_{i=1}^{m} J_{i j}\left(\Delta y_{i}-\sum_{j=1}^{n} J_{i j} \Delta \beta_{j}\right)=0 −2i=1∑mJij(Δyi−j=1∑nJijΔβj)=0(11)
( 10 ) : \color{blue}{(10)}: (10): 写成矩阵形式: ( J T J ) Δ β = J T Δ y J Δ β = Δ y (12) \color{Green} \tag{12} \left(\mathbf{J}^{{T}} \mathbf{J}\right) \Delta \boldsymbol{\beta}=\mathbf{J}^{{T}} \Delta \mathbf{y}\\ \mathbf{J} \Delta \boldsymbol{\beta}= \Delta \mathbf{y} (JTJ)Δβ=JTΔyJΔβ=Δy(12)其中 J \mathbf J J 是函数 f ( x , β ) f(x,\boldsymbol{\beta}) f(x,β) 对 β \boldsymbol{\beta} β 的雅可比矩阵。通过上式 J Δ β = Δ y \mathbf{J} \Delta \boldsymbol{\beta}= \Delta \mathbf{y} JΔβ=Δy 即可求解 Δ β \Delta \boldsymbol{\beta} Δβ
( 11 ) : \color{blue}{(11)}: (11): 所以最终迭代公式为(参考公式(10))：
β ( k + 1 ) = β ( k ) + Δ β (13) \color{Green} \tag{13} \boldsymbol{\beta}^{(k+1)}=\boldsymbol{\beta}^{(k)}+\Delta \boldsymbol{\beta} β(k+1)=β(k)+Δβ(13)
五、EPnP高斯牛顿
通过上面的讲解，相信大家以及注意到了，其核心就是为了求解 Δ β \Delta \boldsymbol{\beta} Δβ, 然而求解 Δ β \Delta \boldsymbol{\beta} Δβ 的关键就是建立 J Δ β = Δ y \mathbf{J} \Delta \boldsymbol{\beta}= \Delta \mathbf{y} JΔβ=Δy 这个矩阵方程。现在回到前面你的讨论，也就是(03)式 f ( β ) = L β − ρ f(\boldsymbol{\beta})=\mathbf{L} \boldsymbol{\beta}-\boldsymbol{\rho} f(β)=Lβ−ρ，其目的是是优化两个坐标系下控制点间距的差，使得其误差最小，等价如下：
f ( β ) = ∑ ( i , j s.t. i < j ) ( ∥ c i c − c j c ∥ 2 − ∥ c i w − c j w ∥ 2 ) (14) \color{Green} \tag{14} f(\boldsymbol{\beta})=\sum_{(i, j~\text {s.t. } i
J = ∂ f ( β ) β = [ ∂ f ( β ) ∂ β 1 ∂ f ( β ) ∂ β 2 ∂ f ( β ) ∂ β 3 ∂ f ( β ) ∂ β 4 ] = [ ∂ ( L β ) ∂ β 1 ∂ ( L β ) ∂ β 2 ∂ ( L β ) ∂ β 3 ∂ ( L β ) ∂ β 4 ] (15) \color{Green} \tag{15} \begin{aligned} \mathbf{J}=\frac{\partial f(\boldsymbol{\beta})}{\boldsymbol{\beta}} &=\left[\begin{array}{llll} \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{1}} & \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{2}} & \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{3}} & \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{4}} \end{array}\right] \\ &=\left[\begin{array}{llll} \frac{\partial(\mathbf{L} \boldsymbol{\beta})}{\partial \beta_{1}} & \frac{\partial(\mathbf{L} \boldsymbol{\beta})}{\partial \beta_{2}} & \frac{\partial(\mathbf{L} \beta)}{\partial \beta_{3}} & \frac{\partial(\mathbf{L} \boldsymbol{\beta})}{\partial \beta_{4}} \end{array}\right] \end{aligned} J=β∂f(β)=[∂β1∂f(β)∂β2∂f(β)∂β3∂f(β)∂β4∂f(β)]=[∂β1∂(Lβ)∂β2∂(Lβ)∂β3∂(Lβ)∂β4∂(Lβ)](15)前面我们已经知道 L \mathbf{L} L 的维度是 6x10， β \boldsymbol{\beta} β 的维度是 10x1，以 L \mathbf{L} L 第一行 L 1 \mathbf{L}^1 L1 为例来推导： L 1 β = [ L 1 1 L 2 1 L 3 1 L 4 1 L 5 1 L 6 1 L 7 1 L 8 1 L 9 1 L 10 1 ] [ β 11 β 12 β 22 β 13 β 23 β 33 β 14 β 24 β 34 β 44 ] (16) \color{Green} \tag{16} \mathbf{L}^{1} \boldsymbol{\beta}=\left[\begin{array}{llllllllll} L_{1}^{1} & L_{2}^{1} & L_{3}^{1} & L_{4}^{1} & L_{5}^{1} & L_{6}^{1} & L_{7}^{1} & L_{8}^{1} & L_{9}^{1} & L_{10}^{1} \end{array}\right]\left[\begin{array}{c} \beta_{11} \\ \beta_{12} \\ \beta_{22} \\ \beta_{13} \\ \beta_{23} \\ \beta_{33} \\ \beta_{14} \\ \beta_{24} \\ \beta_{34} \\ \beta_{44} \end{array}\right] L1β=[L11L21L31L41L51L61L71L81L91L101]⎣ ⎡β11β12β22β13β23β33β14β24β34β44⎦ ⎤(16)等价于： L 1 β = L 1 1 β 11 + L 2 1 β 12 + L 3 1 β 22 + L 4 1 β 13 + L 5 1 β 23 + L 6 1 β 33 + L 7 1 β 14 + L 8 1 β 24 + L 9 1 β 34 + L 10 1 β 44 (17) \color{Green} \tag{17} \mathbf{L}^{1} \boldsymbol{\beta}=L_{1}^{1} \beta_{11}+L_{2}^{1} \beta_{12}+L_{3}^{1} \beta_{22}+L_{4}^{1} \beta_{13}+L_{5}^{1} \beta_{23}+L_{6}^{1} \beta_{33}+L_{7}^{1} \beta_{14}+L_{8}^{1} \beta_{24}+L_{9}^{1} \beta_{34}+L_{10}^{1} \beta_{44} L1β=L11β11+L21β12+L31β22+L41β13+L51β23+L61β33+L71β14+L81β24+L91β34+L101β44(17)分别求偏导后得到
∂ ( L 1 β ) ∂ β 1 = 2 L 1 1 β 1 + L 2 1 β 2 + L 4 1 β 3 + L 7 1 β 4 ∂ ( L 1 β ) ∂ β 2 = L 2 1 β 1 + 2 L 3 1 β 2 + L 5 1 β 3 + L 8 1 β 4 ∂ ( L 1 β ) ∂ β 3 = L 4 1 β 1 + L 5 1 β 2 + 2 L 6 1 β 3 + L 9 1 β 4 ∂ ( L 1 β ) ∂ β 4 = L 7 1 β 1 + L 8 1 β 2 + L 9 1 β 3 + 2 L 10 1 β 4 (18) \color{Green} \tag{18} \begin{array}{l} \frac{\partial\left(\mathbf{L}_{1} \boldsymbol{\beta}\right)}{\partial \beta_{1}}=2 L_{1}^{1} \beta_{1}+L_{2}^{1} \beta_{2}+L_{4}^{1} \beta_{3}+L_{7}^{1} \beta_{4} \\ \frac{\partial\left(\mathbf{L}_{1} \boldsymbol{\beta}\right)}{\partial \beta_{2}}=L_{2}^{1} \beta_{1}+2 L_{3}^{1} \beta_{2}+L_{5}^{1} \beta_{3}+L_{8}^{1} \beta_{4} \\ \frac{\partial\left(\mathbf{L}_{1} \boldsymbol{\beta}\right)}{\partial \beta_{3}}=L_{4}^{1} \beta_{1}+L_{5}^{1} \beta_{2}+2 L_{6}^{1} \beta_{3}+L_{9}^{1} \beta_{4} \\ \frac{\partial\left(\mathbf{L}_{1} \boldsymbol{\beta}\right)}{\partial \beta_{4}}=L_{7}^{1} \beta_{1}+L_{8}^{1} \beta_{2}+L_{9}^{1} \beta_{3}+2 L_{10}^{1} \beta_{4} \end{array} ∂β1∂(L1β)=2L11β1+L21β2+L41β3+L71β4∂β2∂(L1β)=L21β1+2L31β2+L51β3+L81β4∂β3∂(L1β)=L41β1+L51β2+2L61β3+L91β4∂β4∂(L1β)=L71β1+L81β2+L91β3+2L101β4(18)
高斯牛顿法的增量方程： J Δ β = Δ y Δ y = f ( β ) = L β − ρ (19) \color{Green} \tag{19} \mathbf{J} \Delta \boldsymbol{\beta}= \Delta \mathbf{y}\\ \Delta \mathbf{y}=f(\boldsymbol{\beta})=\mathbf{L} \boldsymbol{\beta}-\boldsymbol{\rho} JΔβ=ΔyΔy=f(β)=Lβ−ρ(19)
六、结语
通过上面的推导，最终建立了方程 J Δ β = Δ y \mathbf{J} \Delta \boldsymbol{\beta}= \Delta \mathbf{y} JΔβ=Δy, 针对于该方程的求解有很多种方式，EPnP 中使用的是豪斯霍尔德变换的QR分解，在下一篇博客中，会进行详细的理论讲解。
本文内容来自计算机视觉life ORB-SLAM2 课程课件