Visual SLAM的BA问题：最小二乘问题的Levenberg-Marquardt算法

版本1

k = 0 ; ν = 2 ; t = t 0 ; calculate  g , J , g , H , b ; s t o p = ( ∣ ∣ b ∣ ∣ ∞ ≤ ε 1 ) ; μ = τ × max ⁡ { H i i } ; while  ( not  s t o p ) and  ( k < k m a x ) k = k + 1 ; g p = g ; t p = t   repeat   Solve  ( H + μ I ) h l m = b if  ∣ ∣ h l m ∣ ∣ ≤ ε 2 ( ∣ ∣ t ∣ ∣ + ε 2 ) s t o p = true else t = t − h l m calculate  g , J , g ϱ = ( g p − g ) / ( h l m ⊤ ( μ h l m + b ) ) if  ϱ > 0 calculate  H , b ;   s t o p = ( ∣ ∣ b ∣ ∣ ∞ ≤ ε 1 )   μ = μ × max ⁡ { 1 3 , 1 − ( 2 ϱ − 1 ) 3 } ; ν = 2    else g = g p ; t = t p   μ = μ × ν ; ν = 2 × ν   until  ϱ > 0  or  s t o p end k=0; \nu = 2; \mathbf{t} = \mathbf{t}_0; \\ \text{calculate } \mathbf{g}, \mathbf{J}, g, \mathbf{H}, \mathbf{b};\\ stop = (||\mathbf{b}||_{\infin} \le \varepsilon_1); \mu = \tau \times \max\{H_{ii}\};\\ \text{while }(\text{not }stop) \text{and }(k \lt k_{max})\\ \quad k = k+1; \\ \quad g_p = g; \mathbf{t}_p = \mathbf{t}\\ \quad \,\text{repeat} \\ \quad\quad \,\text{Solve }(\mathbf{H} + \mu\mathbf{I})\mathbf{h_{lm}} = \mathbf{b} \\ \quad\quad \text{if }||\mathbf{h_{lm}}|| \le \varepsilon_2 (||\mathbf{t}|| + \varepsilon_2)\\ \quad\quad\quad stop = \text{true}\\ \quad\quad \text{else} \\ \quad\quad\quad \mathbf{t} = \mathbf{t} - \mathbf{h_{lm}} \\ \quad\quad\quad \text{calculate } \mathbf{g}, \mathbf{J}, g \\ \quad\quad\quad \varrho = \left(g_p - g \right) / \left(\mathbf{h_{lm}}^\top(\mu\mathbf{h_{lm}} + \mathbf{b}) \right) \\ \quad\quad\quad \text{if } \varrho > 0 \\ \quad\quad\quad\quad \text{calculate } \mathbf{H}, \mathbf{b};\\ \quad\quad\quad\quad \,stop = (||\mathbf{b}||_{\infin} \le \varepsilon_1) \\ \quad\quad\quad\quad \,\mu = \mu \times \max\{\frac{1}{3}, 1-(2\varrho-1)^3\}; \nu = 2 \\ \quad\quad\quad \,\,\text{else} \\ \quad\quad\quad\quad g = g_p; \mathbf{t} = \mathbf{t}_p \\ \quad\quad\quad\quad \,\mu = \mu \times \nu; \nu = 2 \times \nu \\ \quad \,\text{until } \varrho > 0 \text{ or } stop \\ \text{end} k=0;ν=2;t=t0​;calculate g,J,g,H,b;stop=(∣∣b∣∣∞​≤ε1​);μ=τ×max{Hii​};while (not stop)and (k<kmax​)k=k+1;gp​=g;tp​=trepeatSolve (H+μI)hlm​=bif ∣∣hlm​∣∣≤ε2​(∣∣t∣∣+ε2​)stop=trueelset=t−hlm​calculate g,J,gϱ=(gp​−g)/(hlm​⊤(μhlm​+b))if ϱ>0calculate H,b;stop=(∣∣b∣∣∞​≤ε1​)μ=μ×max{31​,1−(2ϱ−1)3};ν=2elseg=gp​;t=tp​μ=μ×ν;ν=2×νuntil ϱ>0 or stopend

版本2

k = 0 ; ν = 2 ; t = t 0 ; ε 1 = 1 e − 3 ; n = 0 calculate  g , J , g , H , b ; μ = τ × max ⁡ { H i i } ; while  ( not  s t o p ) and  ( k < k m a x ) k = k + 1 ; g p = g ; t p = t ; l = 0   repeat   Solve  ( H + μ I ) h l m = b t = t − h l m calculate  g , J , g ϱ = ( g p − g ) / ( h l m ⊤ ( μ h l m + b ) ) if  ϱ > 0 calculate  H , b ;   μ = μ × max ⁡ { 1 3 , 1 − ( 2 ϱ − 1 ) 3 } ; ν = 2     else g = g p ; t = t p   μ = μ × ν ; ν = 2 × ν ; l = l + 1   until  ( ϱ > 0 )  or  ( l < l m a x )   if  ( l = = l m a x )  or  ( ϱ = = 0 ) s t o p = true   if  ( g p − g ) < ε 1 g p n = n + 1   else n = 0   if  n ≥ 3 s t o p = true end k=0; \nu = 2; \mathbf{t} = \mathbf{t}_0; \varepsilon_1 = 1e^{-3};n = 0\\ \text{calculate } \mathbf{g}, \mathbf{J}, g, \mathbf{H}, \mathbf{b};\\ \mu = \tau \times \max\{H_{ii}\};\\ \text{while }(\text{not }stop) \text{and }(k \lt k_{max})\\ \quad k = k+1; \\ \quad g_p = g; \mathbf{t}_p = \mathbf{t}; l = 0\\ \quad \,\text{repeat} \\ \quad\quad \,\text{Solve }(\mathbf{H} + \mu\mathbf{I})\mathbf{h_{lm}} = \mathbf{b} \\ \quad\quad \mathbf{t} = \mathbf{t} - \mathbf{h_{lm}} \\ \quad\quad \text{calculate } \mathbf{g}, \mathbf{J}, g \\ \quad\quad \varrho = \left(g_p - g \right) / \left(\mathbf{h_{lm}}^\top(\mu\mathbf{h_{lm}} + \mathbf{b}) \right) \\ \quad\quad \text{if } \varrho > 0 \\ \quad\quad\quad \text{calculate } \mathbf{H}, \mathbf{b};\\ \quad\quad\quad \,\mu = \mu \times \max\{\frac{1}{3}, 1-(2\varrho-1)^3\}; \nu = 2 \\ \quad\quad \,\,\,\text{else} \\ \quad\quad\quad g = g_p; \mathbf{t} = \mathbf{t}_p \\ \quad\quad\quad \,\mu = \mu \times \nu; \nu = 2 \times \nu; l = l+1 \\ \quad \,\text{until } (\varrho > 0) \text{ or } (l < l_{max}) \\ \quad \,\text{if } (l == l_{max}) \text{ or } (\varrho == 0) \\ \quad\quad stop = \text{true}\\ \quad \,\text{if } (g_p - g) < \varepsilon_1 g_p \\ \quad\quad n = n + 1\\ \quad \,\text{else} \\ \quad\quad n = 0\\ \quad \,\text{if } n \ge 3\\ \quad\quad stop = \text{true}\\ \text{end} k=0;ν=2;t=t0​;ε1​=1e−3;n=0calculate g,J,g,H,b;μ=τ×max{Hii​};while (not stop)and (k<kmax​)k=k+1;gp​=g;tp​=t;l=0repeatSolve (H+μI)hlm​=bt=t−hlm​calculate g,J,gϱ=(gp​−g)/(hlm​⊤(μhlm​+b))if ϱ>0calculate H,b;μ=μ×max{31​,1−(2ϱ−1)3};ν=2elseg=gp​;t=tp​μ=μ×ν;ν=2×ν;l=l+1until (ϱ>0) or (l<lmax​)if (l==lmax​) or (ϱ==0)stop=trueif (gp​−g)<ε1​gp​n=n+1elsen=0if n≥3stop=trueend

• $n$记录了连续三次迭代都满足$(g_p-g)<{\epsilon }_1{g}_p$这个条件的现象，如果出现这个现象，代表迭代可以终止了，已获得了最优解。
• $l$表达的是repeat代码块的执行次数，不能无限次的执行，尝试了$l_{max}$次仍然没找到可使target function下降的${\mathbf{h}}_{\mathbf{lm}}$，那么此时代表迭代可以终止了，此时的解有可能不是最优解，但也没办法了。

版本3（带kernel function）

k = 0 ; ν = 2 ; t = t 0 ; ε 1 = 1 e − 3 ; n = 0 calculate all  f i j , ∂ f i j ∂ x i , ∂ f i j ∂ y j , g i j = f i j ⊤ Ω i j f i j calculate  g = ∑ < i , j > ∈ G g i j calculate  w i j = ρ ′ ( g i j ) calculate  H = ∑ < i , j > ∈ G J i j ⊤ ( w i j Ω i j ) J i j calculate  b = ∑ < i , j > ∈ G J i j ⊤ ( w i j Ω i j ) f i j μ = τ × max ⁡ { H i i } ; while  ( not  s t o p ) and  ( k < k m a x ) k = k + 1 ; g p = g ; t p = t ; l = 0   repeat   Solve  ( H + μ I ) h l m = b t = t − h l m update all  f i j , ∂ f i j ∂ x i , ∂ f i j ∂ y j , g i j update  g ϱ = ( g p − g ) / ( h l m ⊤ ( μ h l m + b ) ) if ϱ > 0 update  H , b ;   μ = μ × max ⁡ { 1 3 , 1 − ( 2 ϱ − 1 ) 3 } ; ν = 2     else g = g p ; t = t p   μ = μ × ν ; ν = 2 × ν ; l = l + 1   until  ( ϱ > 0 )  or  ( l ≥ l m a x )   if  ( l = = l m a x )  or  ( ϱ = = 0 ) s t o p = true   if  ( g p − g ) < ε 1 g p n = n + 1   else n = 0   if  n ≥ 3 s t o p = true end k=0; \nu = 2; \mathbf{t} = \mathbf{t}_0; \varepsilon_1 = 1e^{-3};n = 0\\ \text{calculate all } \mathbf{f}_{ij}, \frac{\partial \mathbf{f}_{ij}}{\partial \mathbf{x}_i}, \frac{\partial \mathbf{f}_{ij}}{\partial \mathbf{y}_j}, g_{ij}= \mathbf{f}_{ij}^\top \mathbf{\Omega}_{ij} \mathbf{f}_{ij} \\ \text{calculate } g = \displaystyle \sum_{<i,j> \in G} g_{ij} \\ \text{calculate } w_{ij} = \rho^\prime(g_{ij}) \\ \text{calculate } \mathbf{H} = \displaystyle \sum_{<i,j> \in G} \mathbf{J}_{ij}^\top \left(w_{ij} \mathbf{\Omega}_{ij}\right) \mathbf{J}_{ij} \\ \text{calculate } \mathbf{b} = \displaystyle \sum_{<i,j> \in G} \mathbf{J}_{ij}^\top \left(w_{ij} \mathbf{\Omega}_{ij}\right) \mathbf{f}_{ij}\\ \mu = \tau \times \max\{H_{ii}\};\\ \text{while }(\text{not }stop) \text{and }(k \lt k_{max})\\ \quad k = k+1; \\ \quad g_p = g; \mathbf{t}_p = \mathbf{t}; l = 0\\ \quad \,\text{repeat} \\ \quad\quad \,\text{Solve }(\mathbf{H} + \mu\mathbf{I})\mathbf{h_{lm}} = \mathbf{b} \\ \quad\quad \mathbf{t} = \mathbf{t} - \mathbf{h_{lm}} \\ \quad\quad \text{update all } \mathbf{f}_{ij}, \frac{\partial \mathbf{f}_{ij}}{\partial \mathbf{x}_i}, \frac{\partial \mathbf{f}_{ij}}{\partial \mathbf{y}_j}, g_{ij} \\ \quad\quad \text{update } g \\ \quad\quad \varrho = \left(g_p - g \right) / \left(\mathbf{h_{lm}}^\top(\mu\mathbf{h_{lm}} + \mathbf{b}) \right) \\ \quad\quad \text{if} \varrho > 0 \\ \quad\quad\quad \text{update } \mathbf{H}, \mathbf{b};\\ \quad\quad\quad \,\mu = \mu \times \max\{\frac{1}{3}, 1-(2\varrho-1)^3\}; \nu = 2 \\ \quad\quad \,\,\,\text{else} \\ \quad\quad\quad g = g_p; \mathbf{t} = \mathbf{t}_p \\ \quad\quad\quad \,\mu = \mu \times \nu; \nu = 2 \times \nu; l = l + 1 \\ \quad \,\text{until } (\varrho > 0) \text{ or } (l \ge l_{max}) \\ \quad \,\text{if } (l == l_{max}) \text{ or } (\varrho == 0) \\ \quad\quad stop = \text{true}\\ \quad \,\text{if } (g_p - g) < \varepsilon_1 g_p \\ \quad\quad n = n + 1\\ \quad \,\text{else} \\ \quad\quad n = 0\\ \quad \,\text{if } n \ge 3\\ \quad\quad stop = \text{true}\\ \text{end} k=0;ν=2;t=t0​;ε1​=1e−3;n=0calculate all fij​,∂xi​∂fij​​,∂yj​∂fij​​,gij​=fij⊤​Ωij​fij​calculate g=<i,j>∈G∑​gij​calculate wij​=ρ′(gij​)calculate H=<i,j>∈G∑​Jij⊤​(wij​Ωij​)Jij​calculate b=<i,j>∈G∑​Jij⊤​(wij​Ωij​)fij​μ=τ×max{Hii​};while (not stop)and (k<kmax​)k=k+1;gp​=g;tp​=t;l=0repeatSolve (H+μI)hlm​=bt=t−hlm​update all fij​,∂xi​∂fij​​,∂yj​∂fij​​,gij​update gϱ=(gp​−g)/(hlm​⊤(μhlm​+b))ifϱ>0update H,b;μ=μ×max{31​,1−(2ϱ−1)3};ν=2elseg=gp​;t=tp​μ=μ×ν;ν=2×ν;l=l+1until (ϱ>0) or (l≥lmax​)if (l==lmax​) or (ϱ==0)stop=trueif (gp​−g)<ε1​gp​n=n+1elsen=0if n≥3stop=trueend