VINS-mono詳細解讀

VINS-mono詳細解讀

極品巧克力

前言

Vins-mono是香港科技大學開源的一個VIO算法，https://github.com/HKUST-Aerial-Robotics/VINS-Mono，是用緊耦合方法實現的，經過單目+IMU恢復出尺度，效果很是棒。

感謝他們開源，我從中學到了很是多的知識。源碼總共有15000多行，我在通讀完程序以後，結合參考文獻，把程序背後的算法理論都推導了一遍，總結成了本文，與各位分享。

本文目標讀者：對vins-mono有必定了解的SLAM算法工程師。因爲程序裏有很是多的實現細節，建議讀者在讀本文前，先讀一遍vins-mono的程序。

1.特徵點跟蹤

首先用cv::goodFeaturesToTrack在第一幀圖像上面找最強的150個特徵點，非極大值抑制半徑爲30。新的特徵點都有本身的新的對應的id。而後在下一幀過來時，對這些特徵點用光流法進行跟蹤，在下一幀上找匹配點。而後對先後幀中這些匹配點進行校訂。先對特徵點進行畸變校訂，再投影到以原點爲球心，半徑爲1的球面上，再延伸到深度歸一化平面上，得到最終校訂後的位置。對於每對匹配點，基於校訂後的位置，用F矩陣加ransac來篩選。而後再在匹配上的特徵點以外的區域，用cv::goodFeaturesToTrack搜索最強的新的特徵點，把特徵點數量補上150個。

最後，把剩下的這些特徵點，把圖像點投影回深度歸一化平面上，再畸變校訂，再投影到球面上，再延伸到深度歸一化平面上，獲得校訂後的位置。把校訂後的位置發送出去。

特徵點跟蹤和匹配，就是前一幀到這一幀的，一幀幀繼承下去。或者生成新的特徵點。

2.初始化

2.1外參中的旋轉

用機器人手眼標定的方法，計算出外參中的旋轉。

其中是陀螺儀預積分獲得的，是用8點法對先後幀對應的特徵點進行計算獲得的。詳細見《Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration》。

2.2 SFM

先在關鍵幀窗口裏面，找到第l幀，第l幀與最後一幀有足夠的像素位移，而且能用8點法算出旋轉和位移。以l幀的姿態爲世界座標系。先從l幀開始與最後一幀進行三角定位，再用pnp估計出下一幀的位姿，下一幀再與最後一幀三角定位得出更多的三維點。重複到倒數第二幀。從l幀開始往第一幀，逐漸幀pnp，再與第l幀進行三角定位獲得更多的三維點。每幀pnp時的位姿初值都用上一個關鍵幀的的位姿。剩下的那些尚未被三角定位的特徵點，經過它被觀察到的第一幀和最後一幀進行三角定位。

固定住l幀的位置和姿態，固定住最後一幀的位置。由於這時候的圖像位姿和點的位置都不太準，因此用ceres統一一塊兒優化圖像位姿和三維點位置，優化重投影偏差。優化的測量值是，特徵點在每幀中被觀察到的位置，能夠轉成重投影偏差約束。有關的自變量是，每幀圖像的位姿，特徵點的三維座標。

優化完成以後，即用ceres優化出這些關鍵幀的位姿和地圖點後，再用pnp算出在這段時間區域內的全部圖像的位姿。每一個圖像的計算都用下一個關鍵幀的位姿來當pnp的初值。

程序裏面沒有求雅克比，而是用自動求導的方法。

2.3 計算陀螺儀的偏移

在2.1中已經根據連續圖像的相對旋轉算出相機和IMU間的外參旋轉了，如今要再根據上一節2.2中的SFM算出來的各幀圖像的相對旋轉來計算出陀螺儀的偏移。

就是根據先後幀之間的根據陀螺儀預積分出來的旋轉與基於SFM圖像算出來的旋轉轉換到IMU座標系的相對旋轉 之間的向量差的兩倍。

在程序裏面，每次算出的圖像的姿態，都會轉換成。而後在計算相對IMU的姿態時，就用。

這裏是採用了近似計算的方法。其實就是把角度的殘差轉換成了角軸差的形式。詳見《從角軸到四元數微分方程》和《on-manifold詳細解讀》。

這個關於陀螺儀的偏移求導，獲得雅克比矩陣。而後再根據高斯牛頓法算出陀螺儀偏移。

算出陀螺儀的偏移之後，對於每一幅圖像，利用緩存的每個IMU數據，從新計算這一段時間的每幀圖像對應的預積分，雅克比矩陣和協方差矩陣。

雅克比矩陣的初值是單位陣，協方差矩陣的初值爲零。雅克比矩陣每次都乘以一個狀態轉移矩陣。協方差矩陣每次都左右乘狀態轉移矩陣，再加上一個噪聲矩陣，噪聲矩陣就是加速度和陀螺儀噪聲所造成的噪聲協方差矩陣。

2.4速度，重力和尺度對齊

從2.3以後，認爲陀螺儀是準確的，每幀圖像的姿態也都是準確的。

速度，重力和尺度對齊，其實就是，每一幀對後一幀的位置，速度的預測值，與當前值的偏差。當前值的位置，是SFM位置經過外參計算過來，給的。與這有關的自變量是每幀的速度，重力，尺度。速度，重力，尺度，給的初值都是零。

以第一幀和第二幀爲例。

由於速度，重力尺度的初值所有都是零，因此第一步，這裏殘差的計算能夠簡化成以下。

但雅克比的計算，仍是得用原來的表達式算。

雖然給的初值是零，但由於雅克比矩陣裏面的元素都是與自變量無關的常數，表示這是線性的，因此只用高斯牛頓法計算一次就能夠了。只要數據足夠，就能算出比較準確的值。

其中，當前值的位置，是SFM位置經過外參計算過來，給的。機器人手眼標定。

 

固然，程序裏面其實是把殘差轉換成了這樣的奇怪的座標系下，結果就會像《technical report》裏面的公式18那樣子，所有都乘以。可是，我以爲像我上面那樣的用機器人手眼標定的方法來表示，會比較方便容易易懂直觀明瞭，更不容易在寫表達式的時候出錯。

雖然給的初值是零，雖然只用高斯牛頓法計算一次，但只要數據足夠，也能算出比較準確的值。每增長一幀，就用高斯牛頓法所有計算一次，速度重力尺度的初值所有都是零。由於反正均可以用高斯牛頓法一次算出結果，因此就不用繼承以前的值來優化。

直到優化後的重力接近9.8，好比重力模在[8.8,10.8]區間內。這個方法挺好的，不用先驗知識就把重力優化到9.8，結果使人信服。

程序裏面，會把關於尺度求導獲得的雅克比除以100，這就意味着，尺度這個變量對殘差的影響力減弱了100倍。最終爲了能消去殘差，優化後的尺度會比實際的大100倍。獲得後，要再除以100。這麼作的目的，應該是要讓尺度的精度更高。

每次都直接把這一塊的雅克比矩陣和對應的殘差，轉換成H矩陣的形式，直接加到H矩陣上。爲何要這樣作呢？爲何不把所有的雅克比矩陣算好以後再一次性地轉換成H矩陣。由於雅克比矩陣太巨大了，並且很是稀疏，裏面不少元素都是零。因此，能夠直接根據雅克比矩陣的計算表達式知道哪些位置是非零的，而後非零的位置對應相乘加到入H矩陣中對應的位置，即節省存儲空間，又能加快計算。

2.5 進一步優化重力

在2.4計算出來一個相對準確的值的基礎上，還要再加個約束，即重力的模爲9.8。能夠有兩個方法。

第一個方法是，直接在2.4的殘差約束裏面增長一個，。

可是，若是這樣的話，雅克比矩陣裏面就有了與自變量有關的元素，就不是線性的了。這樣子，計算起來就會比較麻煩，可能須要迭代不少次。

因此，採用方法二。

對模的方向進行優化。2.4的優化僅僅是提供一個較好的重力的方向。

直接給重力一個初值，模的大小就是9.8，方向的初值是2.4中優化後的重力的方向。對這個重力，在切線上調整它的方向。

由於是線性的，因此自變量的初值直接所有設爲零。直接一步高斯牛頓法，算出最優值。用來更新。

重複上述步驟四次，即更新四次後，就認爲這些變量，速度，重力，尺度都已經優化到一個很優的值了。這時候，尺度應該是大於零的。

手動在切線上調整，而後再控制模。其實，就是由自動的非線性優化，改爲了手動的線性優化，用來控制迭代次數。

2.6優化地圖

用奇異值分解的方法，對每個特徵點都優化重投影偏差最小化，優化出它在被觀察到的第一幀的相機座標系下的位置，而後只把深度拿過來用。設它們在被觀察到的第一幀的相機座標系下的齊次座標爲，其它觀察幀相對觀察第一幀的位姿爲。已知它在每幀的投影點，最小化它在每幀的重投影偏差。至關因而尋找空間中的一點，與，每幀的光心從投影點發出的射線，距離最小。

與2.2SFM時算出的地圖點的區別在於，2.2SFM時算出的地圖點是ceres優化出來的關鍵幀及其對應的地圖點。而這裏，是固定這段時間的全部的關鍵幀，這些關鍵幀的姿態在2.3後期算出陀螺儀偏移後，已經調整過了。奇異值分解，最小化重投影偏差，算出地圖點的位置。只取在被觀察到的第一幀的深度。由於在這時候，各個圖像的位姿已經相對比較準確了，因此與2.2中的cere圖像位姿和地圖點所有優化不一樣，這裏只須要優化地圖點就能夠了。

在算奇異值分解時，還要增長 模爲1的約束。齊次座標爲一個四維的向量，這樣子，經過增長模爲1的限制，就能夠用奇異值分解來優化，這就是這裏用齊次座標的好處。（在程序實際運行的時候，上面方程的左邊，還除以了歸一化的模，但不會影響計算結果）

基於2.3中的陀螺儀的偏移，從新計算每相鄰兩個關鍵幀以前的相對姿態，算的是關鍵幀之間的預積分。用的是這一段時間緩存的。IMU數據，用中點插值的方法。

而後用2.5中優化出來的尺度，計算出窗口裏面的每一個關鍵幀的新的位置，它們相對於第一個關鍵幀的位姿，把第一個關鍵幀看成世界座標系。用2.5中優化出來的速度賦值給每個對應的關鍵幀。用尺度來調整每個特徵點的深度。

而後根據重力，計算出當前的世界座標系相對於水平座標系的旋轉，即第一個關鍵幀相對於水平座標系的旋轉。而後，把全部關鍵幀的位置，姿態和速度都轉到水平座標系上。

3.正常的跟蹤

每新進來一張圖片，上面有跟蹤出來的特徵點。

對於f_manager中的feature列表中的那些尚未深度的特徵點，用奇異值分解計算出一個它的座標，使得它在它被觀察到的每幀圖像上的重投影偏差最小。它的座標用在它被觀察到的第一幀圖像的相機座標系的深度表示。由於它還有它在被觀察到的每幀圖像上的歸一化座標。

而後用cere來優化。結合各個關鍵幀的位姿，各個相機的外參，邊緣化的信息，與預積分的偏差，每一個特徵點的重投影偏差，迴環閉環偏差。進行優化。

而後滑動窗口。判斷邊緣化的條件是，是否進來一個新的關鍵幀。

若是有邊緣化的話，則把窗口中最前面的一個關鍵幀滑掉。而後把第一次被這個關鍵幀觀察到的特徵點，都轉移到新的第0個關鍵幀上。若是沒有邊緣化的話，則把以前最新的這一幀用進來的最新的這一幀替換掉。

3.1判斷是不是關鍵幀

窗口的大小默認是10。

每當進來一個新的圖像幀的時候，首先判斷它與窗口裏面存儲的以前的那一幀的的相對位移，就是與第10幀的特徵點的相對位置，用特徵點的相對位移來表示。

若是特徵點的平均相對位移大於某一個閾值，就認爲是一個新的關鍵幀。就把這個新的關鍵幀壓入到窗口裏面，就是壓入到第10個位置，而後其它的關鍵幀都往前移動。第一個位置的關鍵幀被移出去，邊緣化。

若是不是新的關鍵幀，就把以前的第10幀邊緣化掉，這個新的一幀替換成爲第10幀。

總之，不管是哪一種狀況，這個新的一幀確定都會成爲窗口裏面的第10幀。

邊緣化，是在優化以後才進行的，並且最新的這幀上面觀察到的新的特徵點並不參與優化。因此，優化的時候，是包括最新的這一幀的11幀的姿態，以及前10幀的特徵點在每一幀的投影，包括它們在最新這幀的投影點。因此para_Pose之類的待優化變量的數組長度是11。

3.2建立地圖點

對於f_manager中的feature列表中的那些尚未深度的特徵點，若是它被以前2幀以上的關鍵幀觀察到過，用奇異值分解計算出一個它的座標，使得它在它被觀察到的每幀圖像上的重投影偏差最小。它的座標用在它被觀察到的第一幀圖像的相機座標系的深度表示。由於它還有它在被觀察到的每幀圖像上的歸一化座標。

若是之後要把VINS改用深度相機的話，能夠在這裏修改。1.信任深度相機，這個點被觀察到的第一次的位置就是準確值，直接加入地圖點。2.怕深度相機有偏差，因此加判斷，這個點在它被觀察到的連續兩幀裏面，在世界座標系中的三維座標不能相差太大，若是相差太大，就把它的第一幀的觀察記錄刪掉。若是相差不大，就取兩個三維座標的平均值，做爲該點的三維位置，加入地圖點。3.還能夠借鑑ORBSLAM裏面的篩選地圖點的方法，該點須要在連續幾幀中被觀察到，而且這連續幾幀，觀察到的它在世界座標系中的三維座標，不能相差太大，就認爲它是一個好的點，加入地圖點。以後的cere優化中，就再也不優化地圖點，能夠極大地加快優化速度。

3.3cere優化

要優化的目標是各幀的位置，姿態，速度，偏移，以及相機的外參，以及每個特徵點在它被觀察到的第一幀的深度。即，要優化的自變量是。

要擬合的目標是，以前邊緣化後的先驗值，先後幀之間的IMU的預積分值，再加上先後幀之間的偏移的差的約束，每個特徵點的重投影位置。

 

3.3.1與先驗值的殘差

其中，prior表明先驗值。表明先後幀之間的加速度計的二次積分，表明先後幀之間的加速度計的一次積分。表明先後幀的陀螺儀的一次積分。

另外，要再加上先後幀之間的偏移的差的約束。

因此，在當前的自變量的狀況下，與目標的殘差爲residual，表示以下。

其中，與先驗值的殘差，爲了計算方便，能夠再轉換到以前的舒爾補以後的殘差空間裏。用第一狀態雅克比。由於在上一次邊緣化以前，已經優化以後的殘差，舒爾補以後的residual，已是最小二乘結果，它反饋到自變量上面是接近於零（由於各個方向的量都抵消了）。因此，若是這時候，若是對自變量調整了dx，則與以前的舒爾補後的值的殘差會增長J*dx。

其實，在這裏，不只僅是要表示與以前的先驗值的差，而是要表示在以前的偏差的基礎上面疊加上來的偏差。由於以前的偏差是最小二乘結果，而不是全零。

//每一個關鍵幀的位姿與先驗值的不一樣，會形成殘差項增大

Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;

因此，第一部分的雅克比就是以前邊緣化後的雅克比，偏差就是以前的偏差再加上新的偏差。

第一部分，要擬合的約束目標是，每幀與以前邊緣化後的位姿、速度和偏移不能相差太大。殘差用，以前的邊緣化後的殘差，加上，如今的自變量與以前邊緣化後的自變量的相差的值乘以第一狀態雅克比。與之相關的自變量是，每幀的位姿，速度和偏移，相機與IMU的相對位姿。殘差關於要優化的自變量求導，獲得雅克比矩陣。

 

3.3.2 與預積分的殘差

爲了簡化表示，能夠把，裏面的A和b矩陣按行分紅一塊一塊的來表示。

從第二部分開始。要擬合的測量值目標是，IMU的加速度計和陀螺儀的預積分。能夠轉換成實際值與預測值的位姿差來約束。另外還要加上先後偏移接近的約束。與這有關的自變量是，先後幀的位姿，速度，偏移。

其實就是，前一幀基於IMU對後一幀的位置，姿態，速度，偏移，的預測值，與當前值的，偏差。

每次迭代以後，從新算偏差（須要人爲寫出算偏差的程序）。都會用最新的偏移與以前的偏移的差，從新計算兩幀之間的，由於先前預積分的時候已經算好了預積分的值關於偏移的導數。因此，這裏直接根據偏移差乘以導數，就能一下調節預積分的值。明明已經有了預積分關於偏移的雅克比了，爲何這裏要這樣子把偏移單獨拿出來算新的預積分，而不是偏移和預積分一塊兒直接優化呢？每迭代一次都要調整一次，爲何很少優化幾回，最後再累加呢？由於當新的偏移與舊的偏移相差較大的時候，就再也不使用以前預積分關於偏移的雅克比了，須要repropagate，從新計算新的預積分值和雅克比。但程序裏把，判斷偏移相差較大後從新傳播這一塊註釋了。預積分關於偏移的雅克比矩陣的計算爲，狀態向量轉移或相加，則對應的雅克比矩陣也轉移或相加，詳見《on-manifold詳細解讀》。

非線性迭代，每次迭代後，根據殘差和雅克比調整自變量的值，再根據調整後的自變量計算出新的殘差，再計算出新的雅克比，如此循環。這樣子，由於有殘差裏面的預積分關於偏移的導數，每次迭代後，調節自變量裏面的偏移的值後，再計算新的殘差時，就方便了。

3.3.3最小化重投影偏差

而後，第三部分，要擬合的測量值目標是特徵點在各幀圖像上的測量位置。能夠轉換成重投影偏差來約束。與這有關的自變量是，該特徵點被觀察到的第一幀和被觀察到的另一幀的位置和姿態，相機和IMU之間的相對位姿，特徵點在第一幀的深度。

其實就是，對每個地圖點的預測的投影點與實際的投影點的偏差。

全展開殘差項能夠表示爲，

其中，是當前的原點到測量單位球面點的向量，在球面上的切向單位向量。

它與偏差向量相乘，就至關於偏差向量在這兩個切向向量上的投影。由於。

化簡後，能夠表示爲，

其中，鏈式求導，一步步傳導下去。

由於在上述的雅克比矩陣裏面，都有自變量了，因此它是非線性的，只能經過優化的方法一步步迭代。

3.3.4邊緣化

H*x=JT*residuals，H=JT*J。

基於舒爾補來邊緣化。

用奇異值分解的技巧來獲得新的雅克比矩陣，做爲先驗雅克比。由b反過去算殘差，做爲先驗值，在下一次優化時使用。

其實，與《on-mainifold》裏面的舒爾補的方法，本質上是同樣的。

關於邊緣化，還能夠再參考http://blog.csdn.net/heyijia0327/article/details/52822104。

3.3.5 迴環優化

若是窗口裏存在有迴環幀的對應幀的話，則先找到對應幀與迴環幀的匹配點id和位置，特徵點匹配用的是在必定範圍內匹配描述子。特徵點匹配完以後，而後只保留這些匹配上的id和特徵點位置，給front_pose，id用對應幀的id，特徵點位置用迴環幀的位置。其實，就是把對應幀的id賦給匹配上的迴環幀的特徵點。而後給front_pose.loop_pose對應幀的姿態做爲初值，而後用窗口裏面的點的重投影偏差來優化front_pose.loop_pose，點的逆深度，點被觀察到的第一幀的位姿。

窗口優化以後，比較對應幀與front_pose.loop_pose的yaw角是否相差太大，若是相對yaw角大於30度或相對位置大於20的話，就認爲這是錯誤的迴環，刪除這個迴環。由於這時候，這個對應幀還在窗口裏面，尚未滑出去迴環閉環。因此，這樣也能夠及時把DBow找出來的錯誤的迴環，刪掉。

其實，就是把迴環幀放到窗口裏面來優化，優化出迴環幀的位姿，而後再算出迴環幀和對應幀的相對位姿。而後把這個相對位姿，做爲後面的迴環4自由度優化的測量值。具體是，優化出迴環幀在窗口裏面的位置後，算出對應幀相對於迴環幀的位姿。

新的關鍵幀在加入關鍵幀數據庫的時候，做爲對應幀，在關鍵幀數據庫裏面去找回環幀。若是用DBOW找到迴環幀，使用cur_kf->findConnectionWithOldFrame(old_kf, measurements_old, measurements_old_norm, PnP_T_old, PnP_R_old, m_camera);。先用searchByDes 去找匹配點，而後用cv::findFundamentalMat(un_measurements, un_measurements_old, cv::FM_RANSAC, 5.0, 0.99, status);篩選匹配點，再用對應幀自帶的地圖點結合solvePnPRansac，獲得迴環幀的位姿PnP_T_old。PnP_T_old做爲迴環幀位姿loop_pose的初值，結合繼承過來的匹配點measurements_old_norm，再傳回當前窗口中優化，problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);，獲得優化後的loop_pose。若是優化後的loop_pose相對對應幀的位姿relative_yaw小於閾值，則認爲迴環正確。而優化後的loop_pose相對迴環幀原來的位姿，爲relocalize_r和relocalize_t。

在vins-mono的新版本中，新增長了relocalize_r、relocalize_t，其做用是，在大回環起做用的間隙，用relocalize_r、relocalize_t來對位姿進行及時的修正，以更好地保證輸出位姿的準確性，以及關鍵幀輸入到關鍵幀數據庫裏時的位姿的準確性。由於之前是要等迴環幀的對應幀滑出窗口，大回環優化後，纔對這兩個位姿進行校訂的，而如今能夠更及時地修正這些位姿，若是有地方想要最快速地獲得準確的位姿的話。new KeyFrame(estimator.Headers[WINDOW_SIZE - 2], vio_T_w_i, vio_R_w_i, cur_T, cur_R, KeyFrame_image, pattern_file);這裏面的vio_T_w_i是迴環優化時的計算先後關鍵幀的相對位置時用的，因此用的仍是窗口中的位姿。而cur_T，也就是T_w_i，是表示校訂後的位姿，在輸入的時候，會根據relocalize_r校訂，大回環優化後，還再校訂關鍵幀數據庫裏的全部的關鍵幀位姿。relocalize_r、relocalize_t也是很巧妙的方法，由於它們是根據迴環幀和對應幀的圖像的相對位姿算出來的迴環偏移，其實大回環優化以後獲得的迴環偏移correct_t，和這個迴環偏移relocalize_t，應該相差不大。大回環主要是優化環中間的那些關鍵幀的位姿，大回環的主要目的在於修正關鍵幀圖PoseGraph。因此，relocalize_r、relocalize_t一開始就很接近最終大回環優化後的迴環偏移correct_t。relocalize_t是把迴環幀放到當前窗口下優化，算出來的與原來的偏移；correct_t是大回環優化後的對應幀位姿與它原來的位姿的偏移。correct_t準確但計算慢，relocalize_t計算快速且較準確。

4.迴環檢測

這裏的迴環檢測，是每3個關鍵幀檢測一幀，至關因而跳兩幀。這跟迴環檢測的速度，和實際關鍵幀生成的速度，對比有關。由於迴環檢測的速度老是慢於關鍵幀生成的速度，因此爲了保持迴環檢測的關鍵幀不落後於時間，只能跳幀檢測。ORBSLAM裏面也是這樣，但ORBSLAM裏面的迴環檢測判斷標準是，一段時間內的關鍵幀都能匹配上回環，因此ORBSLAM的策略是拿一段時間的關鍵幀來進行檢測。ORBSLAM的迴環檢測程序，Sleep一段時間，在Sleep的這段時間，收集關鍵幀，而後開始工做，只針對收集的這些關鍵幀。工做時，不收集新的關鍵幀，都跳過。處理完這些關鍵幀後，又Sleep。

由於迴環檢測的速度老是慢於關鍵幀生成的速度，因此爲了保持迴環檢測的關鍵幀不落後於時間，只能跳幀檢測。

窗口裏，每3個關鍵幀，送一個到關鍵幀數據庫裏。關鍵幀數據庫裏面的每一幀都跟以前的進行檢測，看是否有迴環。迴環檢測用的是DBow，這個關鍵幀上面的用FAST找出來的新的特徵點和它在以前被光流跟蹤到的特徵點，提取描述子，與歷史描述子匹配。這幀的描述子以及對應的特徵點，跟歷史上的描述子以及對應的特徵點進行匹配，獲得匹配上的特徵點。

若是有迴環，就等這個對應幀從窗口裏滑出，再回環閉環。

找出迴環最先開始的幀，而後把這幀的位姿設爲固定。

迴環閉環的約束條件是，與優化前的相對位姿不能差太大，每幀與前5幀的相對位姿。迴環幀和對應幀的相對位姿的權重是5。迴環閉環裏面，優化的都是4自由度的姿態，迴環幀與閉環幀，每幀與它的前5幀的相對姿態。

優化完後，在最後一個對應幀那裏，再把世界座標系也轉換一下，而後把剩下的關鍵幀都轉換一下。

迴環閉環優化部分的測量值是，迴環幀與對應幀的基於圖像算出的相對4自由度姿態，就是relative_t和relative_yaw，就是迴環幀的loop_pose在窗口中優化後，相對窗口中的對應幀的位姿。每幀圖像與它前5幀的相對4自由度姿態。約束爲，預測值和測量值之間的差。與此有關的自變量是，每幀圖像的4自由度位姿。就像一個項鍊同樣，一串地拉過來。

注意的是，迴環幀與對應幀的基於圖像算出的相對4自由度姿態的權重是5，爲了平衡每幀圖像與它前5幀的相對4自由度姿態。對應幀的先後對它的拉力要相同。

假設是第0幀和第m幀迴環閉環了。

在這裏，到底是使用仍是，我認爲是同樣的。前者是把下一幀相機的原點轉換到上一幀相機的yaw角的水平座標系下，以此爲測量值，預測值與測量值的殘差，關於yaw角求導。後者是把把下一幀的相機的原點轉換到上一幀的座標系下，預測值與測量值的殘差，關於yaw角求導。但這裏的yaw角都是指在水平世界座標系裏面的yaw角。最終優化的結果應該是同樣的。像這種，只是把殘差從一個直角座標系轉換到另一個直角座標系，結果應該是同樣的。可是，若是是另一些轉換，那結果就可能不同了，好比非直角座標系，各維度的有不一樣的縮放尺度，或者維度變換，那樣子的話，優化結果就會不同。

這裏使用程序裏面的表示，。程序裏面沒有求雅克比，而是用自動求導的方法。

迴環檢測以後的處理，很是有創新點。通常的，迴環檢測以後，是要根據迴環檢測結果，把窗口關鍵幀的位姿都轉換掉。但實際上，是能夠無論窗口裏面的關鍵幀的，能夠認爲窗口裏面的關鍵幀的世界座標系相對真實的世界座標系發生了偏移和轉換。只須要在輸出位姿的時候，乘以這個轉換就能夠了，窗口裏面的位姿仍是原來的位姿。這樣子，就能夠避免對窗口的位姿進行大幅度調整，也不須要從新計算雅克比，協方差矩陣。輸出的結果是應用所須要的，修正過的結果。

爲了輸出最新的位姿，須要把新緩存進來的IMU數據也都積分起來。更新實時的最新的位姿。而後，每新進來一個IMU數據，就在以前的基礎上預測一下。由於IMU數據都是在載體座標系的，因此，能夠先在窗口的座標系裏積分，而後最後再轉換到真實的世界座標系，也能夠直接把座標轉換到真實的世界座標系，而後再積分。最後都是獲得在真實的世界座標系裏面的位姿，而後每新進來一個IMU數據，就在以前的基礎上預測一下。這兩種方法是同樣的。

5.流程圖

6.求讚揚

您以爲，本文值多少？ 

7.參考文獻

1. Qin T, Li P, Shen S. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[J]. 2017.
2. Lin Y, Gao F, Qin T, et al. Autonomous aerial navigation using monocular visual‐inertial fusion[J]. Journal of Field Robotics, 2018, 35(4).
3. Yang Z, Shen S. Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration[J]. IEEE Transactions on Automation Science & Engineering, 2017, 14(1):39-51.