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
最後，把剩下的這些特征點，把圖像點投影回深度歸一化平面上，再畸變校正，再投影到球面上，再延伸到深度歸一化平面上，得到校正後的位置。把校正後的位置發送出去。
特征點跟蹤和匹配，就是前一幀到這一幀的，一幀幀繼承下去。或者生成新的特征點。
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.參考文獻
- Qin T, Li P, Shen S. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[J]. 2017.
- Lin Y, Gao F, Qin T, et al. Autonomous aerial navigation using monocular visual‐inertial fusion[J]. Journal of Field Robotics, 2018, 35(4).
- 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.
VINS-mono詳細解讀