VINS-mono详细解读与实现
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.流程圖
?
VIN-SLAM實現轉載自博客：https://www.cnblogs.com/Jessica-jie/p/6950604.html方便自己使用參考學習
香港科技大學的VINS_MONO初試
?
期刊分享SLAMVINS-Mono：一種穩健的單目視覺慣性狀態估計器 上
期刊分享SLAMVINS-Mono：一種穩健的單目視覺慣性狀態估計器 下
第一部分是測量處理前端，負責提取并跟蹤每個新圖像幀的特征，及幀間IMU數據預積分。
第二部分是初始化過程，提供了激活非線性系統的必要初始值，如姿態、速度、重力向量、陀螺儀偏差及3D特征位置。
第三部分為非線性圖優化，通過聯合優化所有的視覺、慣性信息求解滑窗內的狀態。
另一部分運行另一個線程，負責閉環檢測及姿態圖優化。VINS-mono詳細解讀
?
VINS-Mono is a real-time SLAM framework for?Monocular Visual-Inertial Systems. It uses an optimization-based sliding window formulation for providing high-accuracy visual-inertial odometry. It features efficient IMU pre-integration with bias correction, automatic estimator initialization, online extrinsic calibration, failure detection and recovery, loop detection, and global pose graph optimization. VINS-Mono is primarily designed for state estimation and feedback control of autonomous drones, but it is also capable of providing accurate localization for AR applications. This code runs on?Linux, and is fully integrated with?ROS. For?iOS?mobile implementation, please go to?VINS-Mobile.
https://github.com/HKUST-Aerial-Robotics/VINS-Mono
1.運行環境
?Ubuntu 14.04, ROS Indigo, OpenCV 3.1.0, Eigen 3.2.0,Ceres Solver
2. Build VINS-Mono on ROS
在此之前，由于我的opencv有兩個版本-OpenCV 2.4.9和OpenCV 3.1.0,引起沖突，讓我很是苦惱...但最終還是解決啦啦啦啦。。。
記錄在此http://www.cnblogs.com/Jessica-jie/p/6959309.html
Clone the repository and catkin_make:
cd ~/catkin_ws/src
git clone git@github.com:HKUST-Aerial-Robotics/VINS-Mono.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash結果：?? 這5個package均有可執行文件生成，在/home/wj/catkin_ws/devel/lib/里
3. Performance on EuRoC dataset
3.1Download?EuRoC MAV Dataset.Although it contains stereo cameras, we only use one camera.
3.2?Open three terminals, launch the vins_estimator , rviz and play the bag file respectively. Take MH_05 as example:
[1]roslaunch vins_estimator euroc.launch ///estimationroslaunch benchmark_publisher publish.launch ///ground-truth
[2]roslaunch vins_estimator vins_rviz.launch 等同于---rosrun rviz rviz -d /home/wj/catkin_ws/src/VINS-Mono/config/vins_rviz_config.rviz
[3]rosbag play YOUR_PATH_TO_DATASET/MH_05_difficult.bag ---rosbag play /home/wj/Downloads/dataset/EuRoC/ROS_bag/MH_05_difficult.bag 注意：***.launch要在其工作空間執行.
wj@wj-Inspiron-5437:~$ cd catkin_ws
wj@wj-Inspiron-5437:~/catkin_ws$ source ~/catkin_ws/devel/setup.bash
wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch vins_estimator euroc.launch wj@wj-Inspiron-5437:~$ cd catkin_ws
wj@wj-Inspiron-5437:~/catkin_ws$ source ~/catkin_ws/devel/setup.bash
wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch benchmark_publisher publish.launch?
?結果圖：
?
解析：
wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch vins_estimator euroc.launchSUMMARY
========PARAMETERS* /feature_tracker/config_file: /home/wj/catkin_w...* /feature_tracker/vins_folder: /home/wj/catkin_w...* /rosdistro: indigo* /rosversion: 1.11.20* /vins_estimator/config_file: /home/wj/catkin_w...* /vins_estimator/vins_folder: /home/wj/catkin_w...NODES/feature_tracker (feature_tracker/feature_tracker)vins_estimator (vins_estimator/vins_estimator)auto-starting new master
process[master]: started with pid [4194]
ROS_MASTER_URI=http://localhost:11311setting /run_id to d4e07290-fea5-11e7-92da-bc8556c5ff8b
process[rosout-1]: started with pid [4207]
started core service [/rosout]
process[feature_tracker-2]: started with pid [4211]
process[vins_estimator-3]: started with pid [4222]
[ INFO] [1516537385.770250050]: init begins
[ INFO] [1516537385.783868675]: Loaded config_file: /home/wj/catkin_ws/src/VINS-Mono/feature_tracker/../config/euroc/euroc_config.yaml
[ INFO] [1516537385.786188731]: Loaded vins_folder: /home/wj/catkin_ws/src/VINS-Mono/feature_tracker/../config/../
[ WARN] [1516537385.786329200]: Optimize extrinsic param around initial guess!
[ INFO] [1516537385.786765223]: Extrinsic_R :
2.22045e-16 -1 01 2.22045e-16 00 0 1
[ INFO] [1516537385.786810735]: Extrinsic_T :
-0.02 -0.06 0.01
[ WARN] [1516537385.786861452]: waiting for image and imu...
[ WARN] [1516537385.830039004]: LOOP_CLOSURE true
voc file: /home/wj/catkin_ws/src/VINS-Mono/feature_tracker/../config/..//support_files/brief_k10L6.bin
loop start load vocabularyloop closure init finish?
?
wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch benchmark_publisher publish.launchSUMMARY
========PARAMETERS* /benchmark_publisher/data_name: /home/wj/catkin_w...* /rosdistro: indigo* /rosversion: 1.11.21NODES/benchmark_publisher (benchmark_publisher/benchmark_publisher)ROS_MASTER_URI=http://localhost:11311core service [/rosout] found
process[benchmark_publisher-1]: started with pid [6334]
[ INFO] [1520410994.189896520]: Loaded data_name: /home/wj/catkin_ws/src/VINS-Mono/benchmark_publisher/config/MH_05_difficult/data.csv
load ground truth /home/wj/catkin_ws/src/VINS-Mono/benchmark_publisher/config/MH_05_difficult/data.csv
[ INFO] [1520410994.284550074]: Data loaded: 22212?
?
4.AR Demo
4.1 Download the?bag file, which is collected from HKUST Robotic Institute. For friends in mainland China, download from?bag file.
4.2 Open three terminals, launch the ar_demo, rviz and play the bag file respectively.We put one 0.8m x 0.8m x 0.8m virtual box in front of your view.
roslaunch ar_demo 3dm_bag.launchroslaunch ar_demo ar_rviz.launchrosbag play /home/wj/Downloads/dataset/ar_box.bag ---YOUR_PATH_TO_DATASET結果圖：
參考資料：
VINS-Mono代碼注釋以及公式推導：https://github.com/QingSimon/VINS-Mono-code-annotation
不依賴ROS的VINS：https://github.com/heguixiang/Remove_ROS_VINS
https://blog.csdn.net/wangshuailpp/article/details/78461171
https://blog.csdn.net/houlianfeng/article/details/79525854
https://blog.csdn.net/houlianfeng/article/details/79626657
https://blog.csdn.net/houlianfeng/article/details/79713333
https://www.cnblogs.com/ilekoaiq/p/8836970.html
?
總結
以上是生活随笔為你收集整理的VINS-mono详细解读与实现的全部內容，希望文章能夠幫你解決所遇到的問題。
