本部落客要是參考論文:<Inertial-Only Optimization for Visual-Inertial Initialization> , 也是ORBSLAM3的作者寫的關于如何計算IMU預積分,如果想要更好地了解IMU的代碼,可以看看這篇論文,很通順很享受;
(在很多的SLAM問題解決中,都會用到一個詞,叫做Manifold, 關于Manifold的解釋可以參考這篇部落格：Manifold是什麼)
基于"單目SLAM的軌迹誤差與IMU所帶來的誤差相比是可以忽略的" 這樣的假設,本文提出來一種有效的隻計算慣性導航資料的MAP估計法, 并用其初始化整個visual-inertial SLAM系統;
1. 本論文主要包括以下幾個貢獻:
- 下面的關于IMU優化的計算公式,都将IMU噪聲的機率模型納入考慮;
- 在優化時,對所有的相關變量進行一次性優化,而不是想之前一樣分次求解,導緻變量之間的關系解耦;
- 公式相機或機器的初始速度或者高度不進行任何的假設,這也就使得該公式的适用範圍更廣;
- IMU的偏差bias并不為0, 并且加入到後面的優化過程中;
2. 最大後驗初始化(MAXIMUM-A-POSTERIORI)
差別于傳統的VI優化 (可以參考2017年的基于ORBSLAM2推出的ORB-VI的優化過程) , 将所有的變量一次性地在bundleAdjustment中進行優化,下面簡稱 VI-BA, 這篇論文提出一種新的初始化和優化方法;
基于下面幾點:
- 基于單目SLAM已經是相當的成熟及魯邦,本文基本假設其隻是差一個準确的尺度而已;
- 視覺slam的誤差與IMU資料帶來的誤差相比, 是很小的,是以,在進行IMU的inertial-only估計時,将視覺的位姿保持為常量;
- 将scale作為參數加入了優化的過程;
- 将所有的IMU相關量一次性優化,是以采用的也是位置position,線速度 linear velocity, 角速度angulat velocity的所有的變量的交叉協方差(cross-covariance)
具體的初始化過程分為:
1). 純視覺的MAP估計
2). 純IMU資料MAP估計
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);
具體的介紹就是如此, 後續會追加代碼的相關介紹,以及更詳細的雅克比矩陣的推導,歡迎交流.
如有疑問,歡迎交流: wx: baobaohaha_ 歡迎對SLAM有興趣的小夥伴一起交流學習~~