本博客主要是參考論文:<Inertial-Only Optimization for Visual-Inertial Initialization> , 也是ORBSLAM3的做者寫的關於如何計算IMU預積分,若是想要更好地理解IMU的代碼,能夠看看這篇論文,很通順很享受;html
(在不少的SLAM問題解決中,都會用到一個詞,叫作Manifold, 關於Manifold的解釋能夠參考這篇博客：Manifold是什麼)dom
基於"單目SLAM的軌跡偏差與IMU所帶來的偏差相比是能夠忽略的" 這樣的假設,本文提出來一種有效的只計算慣性導航數據的MAP估計法, 並用其初始化整個visual-inertial SLAM系統;優化
1. 本論文主要包括如下幾個貢獻:spa
- 下面的關於IMU優化的計算公式,都將IMU噪聲的機率模型歸入考慮;
- 在優化時,對全部的相關變量進行一次性優化,而不是想以前同樣分次求解,致使變量之間的關係解耦;
- 公式相機或機器的初始速度或者高度不進行任何的假設,這也就使得該公式的適用範圍更廣;
- IMU的誤差bias並不爲0, 而且加入到後面的優化過程當中;
2. 最大後驗初始化(MAXIMUM-A-POSTERIORI).net
區別於傳統的VI優化 (能夠參考2017年的基於ORBSLAM2推出的ORB-VI的優化過程) , 將全部的變量一次性地在bundleAdjustment中進行優化,下面簡稱 VI-BA, 這篇論文提出一種新的初始化和優化方法;3d
基於下面幾點:htm
- 基於單目SLAM已是至關的成熟及魯邦,本文基本假設其只是差一個準確的尺度而已;
- 視覺slam的偏差與IMU數據帶來的偏差相比, 是很小的,所以,在進行IMU的inertial-only估計時,將視覺的位姿保持爲常量;
- 將scale做爲參數加入了優化的過程;
- 將全部的IMU相關量一次性優化,所以採用的也是位置position,線速度 linear velocity, 角速度angulat velocity的全部的變量的交叉協方差(cross-covariance)
具體的初始化過程分爲:blog
1). 純視覺的MAP估計ci
2). 純IMU數據MAP估計get
3). 視覺+慣導的MAP優化;
-
純視覺優化
視覺部分的初始化過程和跟蹤過程與傳統的ORBSLAM2一致,惟一的區別在於 關鍵幀的插入更加頻繁(約爲4HZ~10HZ) 用於下降IMU偏差帶來的不肯定性; 優化過程也是將關鍵幀以及其所包含的有效地圖點送去參與BA優化;
其中利用咱們的標定外參Tbc, 每一幀的KEYFRAME的pose都被轉換到IMU的座標系下, 也就是所謂的body reference 中, 所以每一幀對應在bodyframe中的位姿表示爲
其中Ri表示的從body frame到world reference的旋轉矩陣, pi表示的是第i幀的一個無尺度的位置;
-
純IMU數據的優化
這一步的主要目標是經過上一步中視覺計算出的位姿,得到IMU相關參數的優化結果; 如前面所說的full VI-BA須要一個很好的初值,可是咱們目前並無一個很好的guess, 所以這裏不進行full VI-BA, 也就是不將 視覺部分加入的優化的過程. 所以折中的辦法是將地圖點邊緣化(僅僅使用其帶來的變量,不參與優化過程,或者構建最小化重投影偏差的方程), 得到軌跡的先驗信息,以及先關的信息矩陣, 用這些信息進行IMU數據的優化;
所以, 文章提出了一種方法,也就是將全部軌跡,也就是關鍵幀的位姿固定,而僅僅優化IMU的相關參數, 打包 以下:
其中, s 也就是整個地圖的尺度 ;
是重力加速度方向, 表示爲
表示IMU的加速度和角速度誤差;
是全部的關鍵幀對應的無尺度的速度;
相對於真值s*v表示的速度, 使用 up-to-scale的速度,使得優化過程更加容易; 同時, 假設在初始化的過程當中bias的值是固定不變的, 同時隨機遊走random walk幾乎沒有什麼影響; 同時,從一開始就將重力加速度歸入到優化公式中;
所以全部的測量值都是IMU的變量, 同時用表示從第i幀到第j幀的關鍵幀的預積分; 表示連續的從第0幀到第k幀的IMU積分; 所以在最大後驗機率的公式以下:
其中表示在給定第k幀IMU數據的狀況下, 從0到k幀的可能的IMU測量值分佈可能性;
考慮到各個變量測量值之間的獨立性, 則 又能夠表示爲:
若找到知足最大化後驗機率的值， 也就是找到值Ｘｋ知足：
下面展現了最大化乘積是怎麼轉換爲最小化和的過程:
由於假設ＩＭＵ的預積分都是服從高斯分佈的， 則ＭＡＰ問題轉換成公式：
其中和分別表示prior值的殘差, 以及IMU數據測量值的殘差; 其中能夠表示爲:
這其中旋轉矩陣，速度，以及位移的殘差分別表示以下：
具體公式的推到能夠參考個人博客ＩＭＵ預積分， 可是須要注意的是在ＯＲＢＳＬＡＭ３的IMU相關公式中,並無隨機遊走, 且在兩個關鍵幀之間, 誤差bias爲常數;
接下來就是具體的求解過程當中的一些介紹, 在代碼中也能夠看到; 由於要求解非線性優化方程, 所以咱們須要設定一個良好的初值; 在代碼中,將bais的初值設置爲0, 而重力加速度的初值設置爲加速度測量值的均值; 在優化scale factor的時候,也對初值進行了假設,分別設置爲1, 4, 16 米來進行優化;
在優化的最後部分, 全部位姿以及地圖點都會乘上優化後的尺度因子, 而且根據估計出的重力加速度方向, 再乘上一個旋轉矩陣;
視覺慣性MAP估計
這裏的估計和以前推出的那版Visual-Inertial中的估計沒有什麼區別,主要是最小化重投影偏差以及IMU的位姿偏差 .
這一部分的優化中不會對重力加速度和尺度進行直接優化,可是他們已經包含在關鍵幀的位姿中,做爲隱性參數出現; 可是由於上一步求解的是線性系統, 而這一步糾結的是MAP問題, 所以, 所使用的初值會有所不一樣;
當全部的優化完成之後, SLAM也就從視覺SLAM(visual SLAM) 切換到了 視覺慣導SLAM(visual-inertial SLAM);
具體的介紹就是如此, 後續會追加代碼的相關介紹,以及更詳細的雅克比矩陣的推導,歡迎交流.
本文同步分享在 博客「莫奈的三輪車」（CSDN）。
若有侵權，請聯繫 support@oschina.cn 刪除。
本文參與「OSC源創計劃」，歡迎正在閱讀的你也加入，一塊兒分享。