從零開始一塊兒學習SLAM | 用四元數插值來對齊IMU和圖像幀

視覺 Vs. IMU

小白：師兄，很久沒見到你了啊，我最近在看IMU（Inertial Measurement Unit，慣性導航單元）相關的東西，正好有問題求助啊

師兄：又遇到啥問題啦？

小白：是這樣的，如今VIO（Visual-Inertial Odometry，視覺慣性里程計）很火，我就想試試把IMU測量的信息和圖像進行簡單的融合，這樣利用IMU測量的先驗信息，能夠給圖像一個比較好的初值。。。

師兄：嗯嗯，這個思路沒問題的啊，圖像信息和 IMU 確實存在必定互補性，二者各有所長，取長補短。

小白：是滴，我也是這樣想的，不過我採集了圖像和IMU的數據後，發現IMU輸出頻率好高啊，遠遠大於圖像幀率！

師兄：沒錯，IMU自己就是慣性傳感器，用來測量角速度和加速度，對短時快速運動很敏感，所以幀率很高才能測量到，因此通常是100Hz以上。而咱們圖像傳感器輸出幀率通常比較低，15 - 60Hz 居多~

小白：那就有問題了啊，我想要把IMU測量的值和圖像估計的值進行對齊，這樣我就能根據當前IMU輸出的旋轉量來做爲圖像預測的初值了，如今幀率差這麼多，這個怎麼對齊呢？

師兄：你是用什麼來表達的旋轉？

小白：四元數，我看網上都說用四元數好，不過不知道爲啥（/尷尬）

四元數的優點

師兄：四元數確實在對姿態的描述具備獨特的優點，很是適合用來表示空間中的旋轉。這主要是由於幾個緣由：

一、四元數解決了其餘3維空間旋轉算法會遇到的惱人的問題，好比使用歐拉角來表示旋轉操做時會遇到的萬向節鎖問題(Gimbal lock)。見下圖

二、計算效率比旋轉矩陣方法高，由於表達四元數只須要4個數，旋轉矩陣須要9個。

三、其簡單的數學表達方式能夠被用來規劃出高階連續姿態運動以及在多姿態間插值。這裏的插值就能夠解決你說的對齊問題啦

小白：原來如此，看來我選擇四元數表示是很是正確的！不過我有個疑問，師兄，什麼是插值啊？

什麼是插值？

師兄：插值對應的英文是interpolation ，是數學上的一個經常使用術語。下面是維基百科的專業解釋

數學的數值分析領域中，插值是一種經過已知的、離散的數據點，在必定範圍內推求新數據點的過程或方法。求解科學和工程的問題時，一般有許多數據點藉由採樣、實驗等方法得到，這些數據可能表明了有限個數值函數，其中自變量的值。而根據這些數據，咱們每每但願獲得一個連續的函數（也就是曲線）；或者更密集的離散方程與已知數據互相吻合，這個過程叫作擬合。插值是曲線必須經過已知點的擬合。

小白：師兄，你說的每一個字我都認識，可是連在一塊兒徹底不知道啥意思啊！

師兄：不要緊，爲了嚴謹定義通常都比較晦澀難懂。插值，講一個通俗但不嚴謹的例子，好比如今有10只大雁（對應已有的樣本）排成必定的陣列在飛，讓你在第五、6只大雁中間（原來沒有樣本的插值點）再插入一隻大雁，可是要保證插隊後的大雁在總體中不能太突兀，要顯得比較「合羣」（對應擬合曲線），若是其餘大雁飛人字形，插入的大雁儘可能要保持總體還是人字形；若是其餘大雁飛一字形，插入的大雁儘可能要保持總體還是一字形。

小白：師兄，你早這麼說，我不就明白啦！那通常怎樣插值呢？

師兄：嗯，之後多舉例子。插值方法有不少種，好比最簡單的最鄰近插值(nearest interpolation)、線性插值(linear interpolation)；經常使用的雙線性插值（Bilinear interpolation）,還有保護圖像細節效果較好的雙三次插值(bicubic interpolation)、三次樣條插值(cubic Spline Interpolation)等。

千言萬語匯成一個圖，以下圖是一維和二維插值的比較。黑色表示待計算的插值點，其餘顏色的點表示樣本點。

小白：看暈了都。那這麼多插值方法，咱們用哪一種呢？

師兄：在圖像處理和計算機視覺領域，應用比較多的雙線性插值。雙線性插值的效果不是最好的，但相較最鄰近插值和線性插值的簡單粗暴，其得到圖像的效果仍是更使人滿意的，並且雙線性插值的計算量和易於理解程度會優於雙三次插值和三次樣條插值等高階插值方法。所以雙線性插值仍是最受廣大圖像研究者喜好的。

小白：師兄，能夠舉個具體的例子嗎？仍是不太明白插值的具體應用呢！

師兄：嗯，那就再舉個栗子吧，好比咱們常見的針孔相機成像就是一種射影變換，下圖中一個矩形src通過相機拍攝後成像變爲了dst，此時咱們拿到了dst圖像中的像素點，若是想要用獲得的dst圖像來恢復原始的src圖像，就須要用到射影變換和插值。

當咱們要對圖像進行插值操做的時候，一般須要遍歷dst中的每個像素點，假設dst中某像素點爲p(x0,y0)，對像素點p進行相應變換，使其對應到原圖src中的p’(x0’,y0’)點。在咱們遍歷dst像素的時候， p點的像素值(x0,y0)都是整數，然而變換後對應到原圖src中的p’點的像素值(x0’,y0’)就不必定是整數了。

小白：圖中的p'點就是插值點吧？

師兄：對！將src中p’附近的內容放大，咱們能夠發現p’ (x0’,y0’)點落在了(x1,y1), (x1+1,y1), (x1+1,y1+1), (x1,y1+1)四個相鄰點中間。咱們要作的，就是要利用(x1,y1), (x1+1,y1), (x1+1,y1+1), (x1,y1+1)這幾個整數點的像素值來計算p’ (x0’,y0’)這個非整數點的像素值，再用src中p’ (x0’,y0’)的像素值表示dst中p(x0,y0)的像素值。這個就是插值啦！關於這部份內容網上不少資料，也不是今天的重點，這裏就不詳細介紹了，今天重點是介紹四元數插值~

小白：嗯嗯，我回頭去查查看雙線性插值。有點跑偏了，咱們仍是回到四元數插值的討論吧~

四元數有哪些插值方法？

師兄：好，其實四元數插值的思路也和上面相似，常見的有線性插值、球面線性插值等。咱們從簡單的提及。

先說說最簡單的線性插值（Linear Interpolation，簡稱Lerp）

假設有兩個四元數 q0,q1，想要在位置 t 處求插值 qt，用線性插值能夠這樣計算，是否是很熟悉？

小白：是啊，感受這個很是簡單啊，那我就用這個插值好了！

師兄：四元數的線性插值是很是簡單，可是是有代價的。以下圖所示，四元數表示旋轉時是單位四元數，這種插值方式，至關於咱們是沿着一條直線（也就是圓上的一個弦）進行插值的，這樣插值出來的四元數不是單位四元數，並且還有其餘問題（後面會說）。

小白：那我歸一化一下就好了吧？

師兄：你說的就是歸一化線性插值（Normalized LinearInterpolation，簡稱Nlerp)，前面說過Lerp這樣插值出來的並非單位四元數，但如你所說，只要將 qt 除以它的模 ||qt||就可以 將其轉化爲一個單位四元數了：

小白：嗯，那就這樣進行四元數插值吧，看起來也不是很複雜哈！

師兄：且慢！仍是有其餘問題的。以下圖所示，在同等時間內， vt 掃過的⻆度是不一樣的, vt 掃過的速度（或者說⻆速度）首先會不斷地增長，到t = 0.50以後會開始減速，因此Nlerp插值不能保證均勻的⻆速度。

小白：那怎麼辦呢？

師兄：爲了解決這個問題，咱們能夠轉而對⻆度進行線性插值。這就要使用更復雜一些的插值方法了，好比經常使用的球面線性插值(Spherical Linear Interpolation)，簡稱Slerp。Slerp插值能夠解決前面的均勻角速度問題，它可以保證 每兩個四元數之間的⻆速度是固定的，這就從原理上保證了插值的效果。以下圖所示，若是 v1 和 v2 之間的夾⻆爲 θ，那麼：

小白：那這個四元數怎麼計算呢？

師兄：計算也不復雜，主要是利用三角形、三角函數性質。證實過程咱們就不推導了，直接給出如下結論。固然若是你對結果有疑問，也能夠本身推導一遍~

小白：不，不，我相信這個結論，推導的事情前人已經作過了，我就不重複造輪子了，哈哈，用過的時候直接套公式就好了吧！

師兄：理論上是這樣的，不過，在編程實現Slerp插值的時候仍是有幾個問題須要注意一下。

一、若是單位四元數之間的夾角θ很是小，那麼sin(θ)可能會因爲浮點數的偏差被近似爲0.0，從而致使除以0的錯誤．因此，咱們在實施 Slerp 以前，須要檢查兩個四元數的夾角是否太小（或者徹底相同）。一旦發現這種問題，咱們就必須改用 Nlerp 對兩個四元數進行插值，這時候 Nlerp 的偏差很是小，因此基本不會與真正的 Slerp 有什麼區別。

二、在對兩個單位四元數進行插值以前，咱們須要先檢測q0與q1之間是不是鈍角，即檢測它們點積的結果q0⋅q1 是否爲負數。若是 q0⋅q1<0，那麼咱們就反轉其中的一個四元數，好比說將q1改成−q1 ，並使用q0與−q1之間新的夾角來進行插值，這樣才能保證插值的路徑是最短的．

小白：哇塞，太中肯的建議了！能夠少踩好多坑，謝謝師兄，我要去編程啦！

師兄：哈哈，彆着急，這個方法可行，可是編程稍微複雜點，計算量也大，還有一種實現四元數的球面插值計算方式，要簡單不少，留給你當作做業練習啦，搞定做業，你就能夠直接用來作Slerp插值啦！

編程練習

做業練習1：前面四元數球面線性插值方法比較複雜，下面是它的簡化版求解方法，請證實。

假設v0, v1是兩個四元數，其夾角爲θ，假設在它們中間進行四元數插值結果爲v'，v'和v1之間夾角爲θ‘ < θ，記v⊥是垂直於v1的四元數向量，證實：

v'=v1cosθ' + v⊥sinθ'

做業練習2：編程實現四元數球面線性插值。

咱們用智能手機採集了圖像序列和IMU數據，因爲IMU幀率遠大於圖像幀率，須要你用Slerp方法進行四元數插值，使得插值後的IMU和圖像幀對齊。

已知某幀圖像的時間戳爲：t =700901880170406

離該圖像幀最近的先後兩個時刻IMU時間戳爲：t1 = 700901879318945，t2 = 700901884127851

IMU在t1, t2時刻測量得的旋轉四元數爲：

q1x=0.509339, q1y=0.019188, q1z=0.049596, q1w=0.858921

q2x=0.509443, q2y=0.018806, q2z=0.048944,q2w=0.858905

根據上述信息求IMU對齊到圖像幀的插值後的四元數。

參考結果已經給出。

舒適提示：

代碼框架、數據及預期結果已經爲你準備好了，公衆號「計算機視覺life」後臺回覆：插值，便可得到。

歡迎留言討論，更多學習視頻、文檔資料、參考答案等關注計算機視覺life公衆號，，菜單欄點擊「知識星球」查看「從零開始學習SLAM」星球介紹，快來和其餘小夥伴一塊兒學習交流~

關注公衆號，點擊「學習圈子」，「SLAM入門「」，從零開始學習三維視覺核心技術SLAM，3天內無條件退款。早就是優點，學習切忌單打獨鬥，這裏有教程資料、練習做業、答疑解惑等，優質學習圈幫你少走彎路，快速入門！

本文參考：

高翔《視覺SLAM十四講》

https://zhuanlan.zhihu.com/p/47396001

推薦閱讀

如何從零開始系統化學習視覺SLAM？ 從零開始一塊兒學習SLAM | 爲何要學SLAM？ 從零開始一塊兒學習SLAM | 學習SLAM到底須要學什麼？ 從零開始一塊兒學習SLAM | SLAM有什麼用？ 從零開始一塊兒學習SLAM | C++新特性要不要學？ 從零開始一塊兒學習SLAM | 爲何要用齊次座標？ 從零開始一塊兒學習SLAM | 三維空間剛體的旋轉 從零開始一塊兒學習SLAM | 爲啥須要李羣與李代數？ 從零開始一塊兒學習SLAM | 相機成像模型 從零開始一塊兒學習SLAM | 不推公式，如何真正理解對極約束? 從零開始一塊兒學習SLAM | 神奇的單應矩陣 從零開始一塊兒學習SLAM | 你好，點雲 從零開始一塊兒學習SLAM | 給點雲加個濾網 從零開始一塊兒學習SLAM | 點雲平滑法線估計 從零開始一塊兒學習SLAM | 點雲到網格的進化 從零開始一塊兒學習SLAM | 理解圖優化，一步步帶你看懂g2o代碼 從零開始一塊兒學習SLAM | 掌握g2o頂點編程套路 從零開始一塊兒學習SLAM | 掌握g2o邊的代碼套路 從零開始一塊兒學習SLAM | 用四元數插值來對齊IMU和圖像幀 零基礎小白，如何入門計算機視覺？ SLAM領域牛人、牛實驗室、牛研究成果梳理 我用MATLAB擼了一個2D LiDAR SLAM 可視化理解四元數，願你再也不掉頭髮 最近一年語義SLAM有哪些表明性工做？ 視覺SLAM技術綜述 彙總 | VIO、激光SLAM相關論文分類集錦 研究SLAM，對編程的要求有多高？ 2018年SLAM、三維視覺方向求職經驗分享 2018年SLAM、三維視覺方向求職經驗分享 深度學習遇到SLAM | 如何評價基於深度學習的DeepVO，VINet，VidLoc？ AI資源對接需求彙總：第1期 AI資源對接需求彙總：第2期