起源
L-M是參數(shù)估算和數(shù)值擬合都經(jīng)常會(huì)用到的一種數(shù)學(xué)方法析二，在研究全景圖像時(shí)，就留意到Position Adjust的Optimize階段节预，諸多Optimize工具都用到L-M來(lái)估計(jì)準(zhǔn)確的Position;
BundleAdjust的思路
BundleAdjust是論文[1]提出的對(duì)camera parameters進(jìn)行統(tǒng)一估算的算法叶摄，其核心思路就是選定sphere坐標(biāo)系到二維平面坐標(biāo)系的轉(zhuǎn)換矩陣后，根據(jù)Invariant Features得到的匹配點(diǎn)對(duì)安拟，構(gòu)造代價(jià)函數(shù)：
公式14給定的是單個(gè)匹配點(diǎn)在經(jīng)過(guò)初始化的轉(zhuǎn)換矩陣映射后的點(diǎn)的距離蛤吓，而17式對(duì)Inner Point和Outlier進(jìn)行了區(qū)分（非常有價(jià)值的思路）；論文后續(xù)的討論都是對(duì)此式的求解以及降低計(jì)算量的方法糠赦；當(dāng)然会傲，解法的落點(diǎn)也是L-M算法
上述就是研究L-M算法的實(shí)際問(wèn)題背景
基礎(chǔ)概念
從L-M算法的定義論述，就可以知道L-M繞不開(kāi)牛頓法和梯度下降法拙泽，為了對(duì)L-M算法有完整的直覺(jué)上的理解淌山，得益于這么多次搬家都沒(méi)有扔掉的數(shù)學(xué)分析和數(shù)值分析課本，從[2]和[3]可以得到基礎(chǔ)概念的理解顾瞻，微分方程/泰勒展開(kāi)/雅可比行列式/Hessian矩陣泼疑；
[4]的Chapter 10，講述了非線性方程組的數(shù)值解法荷荤，里面涉及到的一些算法和思路從一元方程的求解（Chapter 2）出發(fā)會(huì)更好理解退渗，而且總體的思路上一元方程的求解和非線性方程組的解法一脈相承移稳；這里面我覺(jué)得最核心的思想是不動(dòng)點(diǎn)迭代
有前面的基礎(chǔ)后基本能理解L-M的算法思路，[5]里面大量討論了迭代算法的原理和細(xì)節(jié)会油，linear search/雅可比矩陣的替代B矩陣/割線替代等
下面是個(gè)人一些理解的總結(jié)
一秒裕、面對(duì)的問(wèn)題
1 非線性方程組求解
f1(x1,x2,...,xn) = 0
f2(x1,x2,...,xn) = 0
...
fn(x1,x2,...,xn) = 0
2 最小化問(wèn)題
最小化問(wèn)題最典型的就是最小二乘問(wèn)題
3 問(wèn)題的統(tǒng)一
非線性方程組的求解可以轉(zhuǎn)換為最小化問(wèn)題；如1所示的方程組的解可以通過(guò)求解下面方程G的最小值來(lái)得到钞啸；
G(x1,x2,...,xn) = f1(x1,x2,...,xn) ^2 + f1(x1,x2,...,xn) ^2 + ... + fn(x1,x2,...,xn) ^2
這個(gè)問(wèn)題的統(tǒng)一涉及到對(duì)多元函數(shù)梯度和雅可比行列式的區(qū)別的理解几蜻，后續(xù)會(huì)講到
數(shù)值迭代方法
1 數(shù)值求解
對(duì)一元以及多元非線性方程（組）直接求解是很困難的，印象里有個(gè)數(shù)學(xué)家證明過(guò)体斩，對(duì)于某些高元非線性方程是無(wú)法直接求解的梭稚，只能通過(guò)數(shù)值求解；即通過(guò)計(jì)算的方法來(lái)迭代求解絮吵；
對(duì)于任意給定的函數(shù)求最值或者極值或者方程（組）求解弧烤，我們已知的信息就是函數(shù)或者方程本身；我們想要得到的是一組(x1,x2,...xn)值蹬敲，使得函數(shù)（方程）在該值上具備某些特性（等于0暇昂，具備最值或者極值），那么我們就要嘗試構(gòu)造函數(shù)或者方程跟(x1,x2,...xn)之間的關(guān)系伴嗡；
那么不動(dòng)點(diǎn)迭代的出現(xiàn)急波，其基本思想就非常清晰了
2 不動(dòng)點(diǎn)迭代
p=G(p)
如果給定點(diǎn)p，滿足上式瘪校，即p為函數(shù)G(x)的不動(dòng)點(diǎn)澄暮；這個(gè)概念很容易推廣到多元函數(shù)上
觀察這個(gè)式子，左邊是我們想要求解的項(xiàng)阱扬，右邊是函數(shù)本身泣懊，該式子直接構(gòu)造了求解項(xiàng)和已知項(xiàng)的聯(lián)系；
并且麻惶，該式能很容易地跟方程求解問(wèn)題關(guān)聯(lián)馍刮，G(x)的不動(dòng)點(diǎn)就是函數(shù) F(x)=x - G(x)的解，那么如果我們需要求解F(x)窃蹋，只需要構(gòu)造一個(gè)G(x)使得 F(x)=x - G(x)卡啰；就可以將F(x)的求解問(wèn)題轉(zhuǎn)換為G(x)求不動(dòng)點(diǎn)的問(wèn)題
x=G(x)如何通過(guò)數(shù)值方法求解
構(gòu)造不動(dòng)點(diǎn)方程的最大優(yōu)勢(shì)是，通過(guò)數(shù)值方法求解不動(dòng)點(diǎn)幾乎是天然的脐彩，觀察上式它直接給出了不動(dòng)點(diǎn)和函數(shù)直接的關(guān)系碎乃，假設(shè)給定一個(gè)x0，如果G(x0)=x0則結(jié)束惠奸，如果不等梅誓，則x1= G(x0)；通過(guò)這樣的更替，我們天然地能夠得到一個(gè)x的迭代序列
這就是不動(dòng)點(diǎn)迭代
3 不動(dòng)點(diǎn)迭代的理論基礎(chǔ)
x= G(x)
上式是否一定有不動(dòng)點(diǎn)以及對(duì)于任意給定的F(x)梗掰，我們可以構(gòu)造的不動(dòng)點(diǎn)函數(shù)G(x)有很多個(gè)（參考[4] Chapter 2 EXAMPLE 3）那是否任意構(gòu)造的G(x)都能夠收斂到真正的不動(dòng)點(diǎn)嵌言？答案是否定的
那[4] Chapter 2 Theorem 2.2給出了不動(dòng)點(diǎn)一定存在的理論前提
[4] Chapter 2 Theorem 2.3給出了如何考察構(gòu)造出來(lái)的G(x)是否能真正收斂到不動(dòng)點(diǎn)的理論基礎(chǔ)；
這部分偏理論及穗，不展開(kāi)講
4 泰勒展開(kāi)
上式就是泰勒展開(kāi)式摧茴，會(huì)發(fā)現(xiàn)一階泰勒展開(kāi)非常適合構(gòu)造不動(dòng)點(diǎn)迭代，并且很容易推導(dǎo)出來(lái)埂陆，一階泰勒展開(kāi)具備收斂到不動(dòng)點(diǎn)的性質(zhì)苛白。而且其誤差能進(jìn)行定量的分析，即求解F(x) = 0的問(wèn)題可以簡(jiǎn)化為求解如下的不動(dòng)點(diǎn)問(wèn)題：
x = F(x) / F'(x)
迭代公式：
p(n) = p(n-1) - F(p(n-1))/ F'(p(n-1))
這就是牛頓法
5 牛頓法
牛頓法需要知道原函數(shù)的一階導(dǎo)數(shù)焚虱，在數(shù)值方法里面购裙，求解導(dǎo)數(shù)很多時(shí)候是不方便的（也不通用，這意味著需要針對(duì)每一個(gè)函數(shù)分析其導(dǎo)函數(shù)）因此后續(xù)多采用的還是割線法以及 method of False Position等變種算法鹃栽，其通過(guò)提供兩個(gè)初始點(diǎn)來(lái)近似F'(x)躏率；可參考[4] Figure 2.10，有直觀的圖像表示
上述是一元函數(shù)的牛頓法論述民鼓，在多元函數(shù)中薇芝，討論高階導(dǎo)數(shù)變的困難(閱讀 [3] 第二十三章，對(duì)n維空間/向量函數(shù)/雅可比/黑塞矩陣等有具體的定義)丰嘉，一般最多討論到二階導(dǎo)數(shù)夯到，通過(guò)偏導(dǎo)數(shù)，我們得到雅可比矩陣和黑塞矩陣
而因?yàn)橛懻搶?duì)象從一元x變換成多元 (x,y,z,....)供嚎，其迭代公式中需要將(x1,y1,z1,...)迭代到下一個(gè) (x2,y2,z2,...)黄娘；需要的要么是(dx,dy,dz,....)，要么就是變換矩陣A
這兩個(gè)對(duì)應(yīng)的多元函數(shù)中的梯度法和牛頓法
6 梯度法
記得在前面提到的“問(wèn)題的統(tǒng)一”么克滴，來(lái)談多元函數(shù)的梯度和雅可比行列式
在一元函數(shù)中導(dǎo)數(shù)確定的方向就是梯度方向；
那么多元函數(shù)涉及的問(wèn)題會(huì)比較復(fù)雜优床，一般研究的對(duì)象會(huì)有兩種：
1 F(x)的最小化, 典型的像[5] 一直研究的就是Least Squares Problems
2 多元函數(shù)線性方程組
f1(x1,x2,...xn) = 0
f2(x1,x2,...xn) = 0
....
fn(x1,x2,...xn) = 0
仔細(xì)看這兩種問(wèn)題劝赔，一個(gè)是求平方和的最小，一個(gè)是方程組的解胆敞；會(huì)發(fā)現(xiàn)這兩者其實(shí)是一個(gè)問(wèn)題着帽；假如方程組有解，那么當(dāng)一組(x1,x2,...xn)為方程組的解時(shí)移层，F(xiàn)(x)取最小值仍翰；如果一組(x1,x2,...xn)使得F(x)取得最小值，即使該最小值大于0观话，那么這組(x1,x2,...xn)也是最接近方程組的解的一組值予借。
所以在討論多元函數(shù)相關(guān)概念的時(shí)候需要區(qū)別是針對(duì)方程組還是針對(duì)F(x)，一般地在多元函數(shù)的討論中，我們說(shuō)梯度灵迫，針對(duì)的是F(x)秦叛，而當(dāng)我們討論雅可比行列式的時(shí)候，直接針對(duì)的是方程組瀑粥；而當(dāng)我們討論黑塞矩陣挣跋，我們針對(duì)的是F(x)；
所以梯度下降直接利用F(x)函數(shù)對(duì)(x1,x2,...,xn)里面的各個(gè)分量的偏導(dǎo)數(shù)得到梯度方向狞换；而雅可比從組成F(x)的函數(shù)分量對(duì)對(duì)(x1,x2,...,xn)里面的各個(gè)分量的偏導(dǎo)數(shù)得到
梯度下降是線性收斂避咆，而牛頓法是Quadratic convergence （見(jiàn) [5] 2.3式以及2.2節(jié)）
[4] Chapter 10(簡(jiǎn)寫為 [4]-C10) 和[5] 都介紹牛頓法，[4]-C10直接引入的問(wèn)題是非線性方程組的解修噪，而[5]直接引入的問(wèn)題是最小二乘的最小值查库；
在思路上，[4]-C10引入的還是不動(dòng)點(diǎn)迭代割按， EXAMPLE2 是一個(gè)具體的構(gòu)造 P = G(P)的例子膨报；然后引入不動(dòng)點(diǎn)存在以及收斂的理論基礎(chǔ)(Theorem 10.4 以及 Theorem 10.6)，最后介紹牛頓法時(shí)水到渠成适荣；
[5] 在思路上從下降法開(kāi)始介紹现柠，下降法思路總體分為兩部，尋找下降方向弛矛，確定下降步長(zhǎng)够吩，然后迭代
7 在L-M之前
[5] 中對(duì)L-M的介紹是，“Levenberg (1944) and later Marquardt (1963) suggested to use a damped Gauss-Newton method”丈氓；所以理解L-M需要理解什么是 Gauss-Newton method 以及什么是damped method周循；
7.1 牛頓法和高斯-牛頓法的區(qū)分
牛頓法的思路很直接，利用函數(shù)F(x)如果在x處取得最小值万俗，則F'(x)=0的性質(zhì)來(lái)確定下降方向湾笛；
F'(X + h) = F'(X) + F''(X)h + 高階極小項(xiàng) = 0
當(dāng)h足夠小的時(shí)候，忽略高階極小項(xiàng)闰歪；這樣直接得到下降方向
h = -F''(X) / F'(X)
如果F''(X)是正定矩陣嚎研，很容易證明h一定是下降方向（下降方向的定義 [5] 中 Definition 2.6）；
高斯牛頓法的思路是展開(kāi)F(X)的各個(gè)組成子方程的泰勒展開(kāi)库倘；研究對(duì)象是最小二乘法的最小值定義形式的F(X):
F(X) = 0.5 * ||f(x)||^2
f(x+h) = f(x) + J(x)h + 高階極小項(xiàng)
當(dāng)h足夠小临扮，忽略高階極小項(xiàng)；然后將該式子帶入F(X)后計(jì)算得到([5] 3.7式)教翩；
F(X + h) = L(h) = F(X) + h(t) * J(t) * f + 0.5 * h(t) * J(t) * J * h
J(t)是J的轉(zhuǎn)置杆勇，h(t)是 h的轉(zhuǎn)置
研究L(h)函數(shù)， L'(h) = J(t) * f + J(t) * J * h L''(h) = J(t) * J
可以看到L''(h)的值跟h沒(méi)有關(guān)系饱亿，而且如果J是滿秩蚜退，L''(h)恒為正定矩陣闰靴，這說(shuō)明L(h)有唯一的最小值，并且可以通過(guò) L'(h) = 0 來(lái)確定下降方向关霸；
h = - J(t) * f / ( J(t) * J)
高斯牛頓法和牛頓法研究的對(duì)象是不同的传黄，但是思路是一致的，在高斯牛頓法中队寇，可以認(rèn)為高斯牛頓法將F(X)轉(zhuǎn)換L(h)后膘掰，對(duì)L(h)的研究用的就是牛頓法；
高斯牛頓法和牛頓法的區(qū)別還在于下降速度上佳遣，牛頓法是二次收斂速度识埋，高斯牛頓法在特定情況下可以達(dá)到二次收斂速度，以及一些特定情況下可以達(dá)到超線性收斂零渐，但是一般地窒舟，只能得到線性收斂速度（詳細(xì)見(jiàn) [5] 3.12式及后面的段落討論）
7.2 damped method 阻尼法
“Levenberg (1944) and later Marquardt (1963) suggested to use a damped Gauss-Newton method” 高斯牛頓法前面討論了，那么阻尼法是什么诵盼？具體的公式見(jiàn)于 [5] 2.4章節(jié)
此處記錄下個(gè)人的理解：
之前提到惠豺，[5] 在思路上從下降法開(kāi)始介紹，下降法思路總體分為兩部风宁，尋找下降方向洁墙，確定下降步長(zhǎng)，然后迭代
那么不管是梯度法戒财，牛頓法热监，高斯牛頓法都是確定下降方向的方法，對(duì)于步長(zhǎng)沒(méi)有討論饮寞，那么2.4提到 Trust Region 和 damped method兩種方法孝扛，其核心思想是同時(shí)考慮下降方向和步長(zhǎng)
如何做呢？通過(guò)研究F(X)的二階泰勒展開(kāi)式的近似函數(shù)的性質(zhì)：
L(h) = F(X) + h(t) * c + 0.5 * h(t) * B * h
L(h)是一個(gè)關(guān)于h的函數(shù)幽崩，然后研究L(h)在一定步長(zhǎng)情況下何時(shí)取得極小值苦始；
那么damped method就是研究 M(h) = L(h) + 0.5 * u * h(t) * h , u >= 0 何時(shí)取得極小值
當(dāng)其取得極小值時(shí)，如果 F(X + h) < F(X) 則說(shuō)明h是下降方向慌申，則迭代F(x) 否則迭代u
那么L(h)何時(shí)取得極小值盈简？ M'(h) = L'(h) + uh = 0 時(shí)取得；從而得到下降方向
h = -c / (B + u * I )
對(duì)于u的考量可以通過(guò) p = (F(X) - F(X + h) ) / ( L(0) - L(h) ) 來(lái)考量太示，詳細(xì)討論見(jiàn)[5] 中 2.18,2.19,2.20式子的討論
而從該數(shù)學(xué)公式上看，p越接近1說(shuō)明L函數(shù)越能近似F函數(shù)香浩，即在h方向上类缤，L函數(shù)能很好地近似F函數(shù)，當(dāng)p越接近0邻吭，甚至為負(fù)值時(shí)餐弱，則說(shuō)明L函數(shù)不能很好地近似F函數(shù)；在[5] 3.2節(jié)3.14式子后繼續(xù)討論了gain ratio
對(duì)u的考量就是討論當(dāng)h不是下降方向的時(shí)候迭代u的方法以及該方法的數(shù)學(xué)理論基礎(chǔ)，此處不贅述
8 L-M
[5] 中對(duì)L-M的介紹是膏蚓，“Levenberg (1944) and later Marquardt (1963) suggested to use a damped Gauss-Newton method”瓢谢；
所以L-M其實(shí)是一種 damped Gauss-Newton method；所以如果理解了Gauss-Newton method以及 Damped Method驮瞧，那么L-M其實(shí)很好理解氓扛，L-M定義的下降方向如下：
(J(t) * J + u * I) * h = -J(t) * f
討論下L-M的優(yōu)點(diǎn)，首先對(duì)所有 u > 0 论笔，系數(shù)矩陣 (J(t) * J + u * I) 是正定矩陣采郎，這確保了L-M方法確定的h方向一定是下降方向；另外就是當(dāng)u較大的時(shí)候狂魔，L-M接近梯度下降方向蒜埋，u較小的時(shí)候接近高斯-牛頓方向
9 割線法
牛頓法和L-M都有割線法；所有的割線法解決的問(wèn)題都是替代雅可比矩陣最楷；雅可比矩陣在實(shí)際問(wèn)題中不好計(jì)算整份，因?yàn)槲覀兠鎸?duì)的方程是多樣的（各種模型組成的函數(shù)差異），如果都需要其偏導(dǎo)函數(shù)籽孙，那么無(wú)疑增加來(lái)解決問(wèn)題的難度以及喪失了通用性烈评，割線法只用到了函數(shù)本身，使得問(wèn)題的解決能夠通用并能減少計(jì)算量
9.1 牛頓法的割線替代
[4] 2.3講到Newton‘s Method的兩種割線方法： Secant Method 和 Method of False Method蚯撩， 思路類似础倍，使用兩個(gè)初始點(diǎn)，然后用F(xn) - F(xn-1) / (xn - (xn-1) ) 來(lái)近似導(dǎo)數(shù)胎挎；此處需要注意沟启，隨著xn序列的更新，實(shí)際上導(dǎo)數(shù)的近似 F(xn) - F(xn-1) / (xn - (xn-1) ) 的值也在跟著更新
9.2 L-M 的割線替代B矩陣
如下所示的L-M方法中最核心的計(jì)算就是雅可比矩陣犹菇，那么我們同理假設(shè)有一個(gè)矩陣B來(lái)近似J德迹，那么核心就是如何得到初始的B0以及如何根據(jù)xn的序列來(lái)更新B
(J(t) * J + u * I) * h = -J(t) * f
10 OpenCV LMSolver 解析
L-M在OpenCV中的實(shí)現(xiàn)有兩個(gè)，一個(gè)是CvLevMarq揭芍，一個(gè)是LMSolver胳搞； 建議使用LMSolver，根據(jù)注釋称杨，LMSolver 是Matlab的LMSolve包轉(zhuǎn)換成C++語(yǔ)言肌毅，“translation to C++ from the Matlab's LMSolve package by Miroslav Balda”；使用LMSolver的時(shí)候只需要實(shí)現(xiàn)LMSolver::Callback姑原；計(jì)算每次迭代的雅可比矩陣以及error悬而；
11 Bundle Adjust的 LMSolver 實(shí)現(xiàn)
OpenCV的BundleAdjusterBase使用 CvLevMarq 來(lái)進(jìn)行L-M求解，而且雅可比矩陣計(jì)算時(shí)锭汛，每個(gè)方程使用了7參數(shù)笨奠；我們嘗試使用LMSolver來(lái)進(jìn)行重寫袭蝗，并且簡(jiǎn)化方程參數(shù)；關(guān)于映射模型般婆，更詳細(xì)的介紹參見(jiàn)：
Opencv2.4.9源碼分析——Stitching（二）
Opencv2.4.9源碼分析——Stitching（三）
[參考資料]
[1] Automatic Panoramic Image Stitching using Invariant Features
[2] 數(shù)學(xué)分析（上）第三版 華東師范大學(xué)數(shù)學(xué)系 第五 導(dǎo)數(shù)和微分到腥、第六章 微分中值定理及其應(yīng)用
[3] 數(shù)學(xué)分析（下）第三版 華東師范大學(xué)數(shù)學(xué)系 第二十三章 流形上微積分學(xué)初階
[4] NUMERICAL ANALYSIS (Seventh Edition) Chapter 2 Solution of Equations in one Variable, Chapter 10 Numerical Solutions of Nonlinear Systems of Equations
[5] METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS 2nd Edition, April 2004,Informatics and Mathematical Modeling Technical University of Denmark
[6] https://blog.csdn.net/zhaocj/article/details/78809143
[7] https://blog.csdn.net/zhaocj/article/details/78799194
