orb_slam中lm演算法的分析
劉康2023年 1月
在orb_slam的poseoptimization()函式中的lm演算法，可以分解成幾個部分，首先是線性方程的建立和求解，然後是位姿矩陣的迭代更新；對於求解線性方程，就是求解方程 ，然後應用∆x迭代更新當前的位姿 ，其中 ， 分別表示第k次，k+1次的位姿；⨁符號表示的是加法意義的運算，當前的系統下表示矩陣的乘法。
在lm的演算法中 ， , j是雅克比矩陣，e表示的是對應的誤差項,增加了乙個 正定因子；
該演算法中的需要求解的方面如下：
1，j矩陣的求解方法；
2，資訊矩陣 的求取方法；
3，正定因子 的初始化，更新方式；
4，向量b的求解方法；
5，線性方程h*∆x= b 的求解；
6，利用線性方程求解的值∆x更新位姿；
7，得到新的位姿之後在次進入步驟1到6的迴圈；
8，當位姿達到收斂，或者精度達到要求時，停止迭代。
計算步驟的大體的框圖如下：
1，求解j矩陣：
在optimization()優化函式中，給出的誤差項是對應的匹配點的重投影誤差，也就是二維的畫素的差值，要求的是當前的幀的位姿，位姿的變化矩陣可以由六個李代數引數表示，也就是乙個六維的向量表示，所以j矩陣是乙個 的矩陣，具體的公式如下：
j= 2，資訊矩陣 的求取
在求解lm演算法中資訊矩陣反應的是提取特徵點時所在的影象金字塔的層數的因子這一特性，金字塔一共八層，每一層的特徵乘子為1.2，所以對應層的特徵因子為 ，其中n為對應的層數，資訊矩陣的求解公式如下：
3，正定因子的初始化
lm演算法中正定因子的作用主要是保證h矩陣的正定性，線性方程有解。本次演算法中正定因子初始化的過程如下：
首先由1,2步可以得到矩陣 ，求解正定因子 的公式如下：
其中 表示給定的乙個引數值，這裡給出 ； 表示的是矩陣a的所有的對角元素， 表示對角元素中的最大值。
4，向量b的求取
可以知道在lm演算法中 , j是雅克比矩陣，e表示的是對應的誤差項，也就是對應的畫素值的差。
5，線性方程h*∆x= b 的求解
這裡求解的應用的是eigen庫中的ldlt方法求解方程：
∆x =h.ldlt().solve(b)
6，利用線性方程求解的值∆x更新位姿
我們更新的方法為： ，其中 ， 分別表示第k次，k+1次的位姿；⨁符號表示的是加法意義的運算，當前的系統下表示矩陣的乘法。
現在詳細的講解一下具體的求解，首先我們的位姿 ， 是乙個 的矩陣，所以李代數∆x向量需要轉化為乙個 的矩陣，轉化的方法如下：
設轉化後的矩陣為 ，所以有 其中exp()就是一種把李代數轉化為矩陣的函式，下面就是直接的矩陣的乘法問題：
更新位姿時，要左乘以增量矩陣。
7，得到新的位姿之後的判斷
7.1 判斷條件然後決定是否把這次得到的位姿作為本次更新的位姿
在開始的時候首先計算得到位姿更新之前的所有誤差項的二範數的和，這裡用變數chi2表示,計算公式如下：
更新位姿以後現在的誤差項 發生了變化，為 ；所以所有誤差項的二範數的和為：
判斷乙個公式：
a， 當rh1的值大於0時，把當前的跟新的位姿作為本次更新的位姿，並且更新正定因子 的值：
更新後的值在下一次迭代的時候使用。
b，當rh1的值小於等於0時，增加 的值重新進行計算一次位姿，直到rh1的值大於0，或者計算10次以後仍然不滿足時，停止迭代；
增加 值的策略如下：
首先有初始的 的值為 ，同時定義乙個更新因子nu_=2;
當rh1<=0時， ，同時增加為nu_=2* nu_；
下一次迭代之後繼續有rh1<=0時，用新的 和nu_繼續更新引數的值。
7.2判斷條件然後決定是否進行下一次更新
判斷不進行進行更新的條件主要有兩項，第一項是設定的最多迭代次數，當迭代到達最大次數時自動結束，在本次演算法中設定的是10次；另一項判斷迭代結束的條件是當優化後的位姿達到了一定的精度之後，判斷的條件如下：
if((inichi-chi2)*1e3_nbad++;
else
_nbad=0;
if(_nbad>=3)
break; //若連續3次優化的誤差都達到了精度要求，則結束優化
其中inichi表示迭代之前的總的誤差項的二範數的和，chi2表示當前的誤差項的二範數的和；上面的程式表示當連續優化三次誤差的變化都不大的時候，停止優化。
8，核函式
本次演算法中為了使得不受錯誤的匹配得到的大的誤差的干擾，應用了核函式來調整誤差很大的誤差項的權重值，使得能夠很大程度上降低錯誤匹配的影響。
LM演算法的C 實現
這是乙個資料擬合的例子，並沒有採用物件導向的設計方法是使能更好的理解lm演算法的流程，簡約而不簡單。演算法詳細過程不多介紹。程式中用到opencv庫中的矩陣類mat。例 pragma once include include opencv2 core core.hpp pragma comment ...
ORBSlam2的位姿優化演算法
orbslam2中使用了對極約束，三角測量，pnp投影和bundleadjustment演算法 本文著重講解在orbslam2中使用到的bundleadjustment優化演算法。區域性ba優化的過程是，通過當前取得的關鍵幀找到附近的區域性關鍵幀，區域性地圖點和可以觀察到這些區域性地圖點但是不是區域...
視覺中光源的選擇
如果想要邊界非常清晰 而且沒有陰影 通常是背光 但是背光的最大影響就是光源在下面放置 容易怕壓 而且灰塵等汙漬對其影響也大 另一種方案是在物體的四周打光 這樣的話上面的問題就可以解決了 缺陷是物體周圍得做個支撐光源的件兒 有可能會不太方便 還有就是四周打光的高度問題 高度越高 距離越遠 光斑越大 陰...