0. 內(nèi)容

1. 時間戳同步問題及意義

時間戳同步的原因：如果不同步，由于IMU頻率高，可能由于時間戳不同步而導(dǎo)致在兩幀camera之間的時間內(nèi)用多了或者用少了IMU的數(shù)據(jù)，且時間不同步會導(dǎo)致我們首尾camera和IMU數(shù)據(jù)時間不同，會使估計存在誤差，使我們的系統(tǒng)精度下降甚至出現(xiàn)錯誤的預(yù)測。如果以IMU時間為準(zhǔn)確的，同步之后，我們可以用時間戳偏移對系統(tǒng)估計的$T_{wb}$進(jìn)行補(bǔ)償（思路1），或者能夠得到與首尾IMU時間相同的時間對應(yīng)的兩幀camera的觀測（思路2），提升系統(tǒng)精度。

時間戳同步有兩種方式：硬同步和軟同步。

• 硬同步即硬件同步：即IMU（或其他傳感器）產(chǎn)生數(shù)據(jù)的同時給一個信號，可用于出發(fā)MCU中斷，然后觸發(fā)camera曝光，使camera輸出圖像的時間戳本身就和IMU同步，從而實現(xiàn)硬件同步。
• 軟同步即軟件同步：在軟件上對收到的IMU和camera數(shù)據(jù)進(jìn)行運(yùn)算，解算出時間戳。具體來說，camera和IMU從產(chǎn)生到到達(dá)MCU是需要時間的，這段時間分別叫$t_d^{cam}$和$t_d^{imu}$，產(chǎn)生時間分別加上這個時延就的到了MCU采樣的時間戳，二者相減就是真正的時間戳，這里我們只需要這個時間戳的相對量，所以我們可以假設(shè)IMU是準(zhǔn)確的，$t_d^{imu}?t_d^{cam}$就是我們MCU所知的時間戳偏移$t_d$。

1.1 思路1：補(bǔ)償camera pose的估計（軌跡勻速模型）

位姿補(bǔ)償公式思路：假設(shè)相機(jī)在兩幀間是勻速運(yùn)動。VIO輸出的是系統(tǒng)在world系下的pose，即$T_{wb}$，我們需要補(bǔ)償?shù)氖?span id="n5n3t3z" class="katex--inline">$T_{wc}$，于是就結(jié)合VIO輸出的角速度，速度反向推到cam時刻（補(bǔ)償了$t_d$之后的時刻），然后再用外參$T_{bc}$轉(zhuǎn)到cam系下即可：

在VIO殘差中進(jìn)行補(bǔ)償：

1. 同樣，我們VIO輸出的是$T_{wb}$所以我們要先將landmark轉(zhuǎn)換到body，再轉(zhuǎn)換到camera系：

2. 這里實際上是在歸一化平面，并沒在像素平面（重投影誤差的uv實際上是歸一化平面的坐標(biāo)，通過特征提取或者光流匹配而得），忽略了相機(jī)內(nèi)參。

3. 將式（3）帶入可的式（8），其中，$[R_{w{b}_i}exp(?w_{b_i}{t}_d){]}^T=exp(w_{b_i}{t}_d)R_{w{b}_i}^T$，即$[exp(?w_{b_i}{t}_d){]}^T=exp(w_{b_i}{t}_d)$，里代數(shù)取反。

重投影誤差對時間戳延時$t_d$的Javobian：

式（9）推導(dǎo)，只看中間最復(fù)雜的部分，$exp({\omega }_{b_i}{t}_d+{\omega }_{b_i}\delta t_d)$，這里好理解點的話實際上角速度$\omega t$可以寫成$(\omega t{)}^{\wedge }$，因為$\omega t$可以代表一個三維的旋轉(zhuǎn)矩陣，按照旋轉(zhuǎn)的表達(dá)方式是軸角表示，即 旋轉(zhuǎn)角度*旋轉(zhuǎn)軸，可以看之前的一篇知乎

所以上述部分可以表示成$exp[({\omega }_{b_i}{t}_d+{\omega }_{b_i}\delta t_d{)}^{\wedge }]$，這只是便于理解，正常還是按照$\omega t$表示，不加反對稱符號。
所以，用到兩個公式：

1. 按照BCH公式的SO3版本：
   
2. 還有$e^x$的無窮級數(shù)展開（跟太了展開差個無窮小），$e^x=1+x+\frac{x_2}{2!}+\frac{x_3}{3!}+\dots$，下面因為${\omega }_{b_i}\delta t_d$較小，所以二階以上直接忽略
   所以
   ? e x p ( ω b i t d + ω b i δ t d ) = e x p ( ω b i t d ) ? e x p { [ J r ( ω b i t d ) ω b i δ t d ] ∧ } = e x p ( ω b i t d ) ? { I + [ J r ( ω b i t d ) ω b i δ t d ] ∧ } \begin{align*}\ exp(\omega_{b_i}t_d+\omega_{b_i}\delta t_d) &=exp(\omega_{b_i}t_d) * exp\{[J_r(\omega_{b_i}t_d)\omega_{b_i}\delta t_d]^\wedge\}\\ &=exp(\omega_{b_i}t_d) * \{I + [J_r(\omega_{b_i} t_d)\omega_{b_i} \delta t_d]^\wedge \}\\ \end{align*} ?exp(ωbi??td?+ωbi??δtd?)?=exp(ωbi??td?)?exp{[Jr?(ωbi??td?)ωbi??δtd?]∧}=exp(ωbi??td?)?{I+[Jr?(ωbi??td?)ωbi??δtd?]∧}?
   將上式帶入式（9）就得BCH近似后的結(jié)果，因為是求關(guān)于時間戳$t_d$的Jacobian，整理得：
   f l c i ( t d + δ t d ) ? ≈ R c b e x p ( ω b i t d ) ? I ? R ω b i T ? C + p c b + R c b e x p ( ω b i t d ) ? { I ? R ω b i T v b i w δ t d + [ J r ( ω b i t d ) ω b i δ t d ] ∧ R ω b i T ? C + [ J r ( ω b i t d ) ω b i δ t d ] ∧ R ω b i T ? v b i w δ t d } \begin{align} f_l^{c_i}(t_d+\delta t_d)\ & \approx R_{cb}exp(\omega_{b_i}t_d)*I*R_{\omega b_i}^T*C+p_{cb} \tag{9a} \\ &\quad+R_{cb}exp(\omega_{b_i}t_d)* \{ \notag \\ &\quad I*R_{\omega b_i}^Tv_{b_i}^w\delta t_d \tag{9b}\\ &\quad+[J_r(\omega_{b_i} t_d)\omega_{b_i} \delta t_d]^\wedge R_{\omega b_i}^T *C \tag{9c}\\ &\quad + [J_r(\omega_{b_i} t_d)\omega_{b_i} \delta t_d]^\wedge R_{\omega b_i}^T * v_{b_i}^w\delta t_d \tag{9d}\\ & \quad\} \notag \end{align} flci??(td?+δtd?)??≈Rcb?exp(ωbi??td?)?I?Rωbi?T??C+pcb?+Rcb?exp(ωbi??td?)?{I?Rωbi?T?vbi?w?δtd?+[Jr?(ωbi??td?)ωbi??δtd?]∧Rωbi?T??C+[Jr?(ωbi??td?)ωbi??δtd?]∧Rωbi?T??vbi?w?δtd?}?(9a)(9b)(9c)(9d)?

上式中$C$是(8)中的簡記，(9a)即為式(8)，剩下的(9d)可能是反對稱約掉了為0？或者是跟$\delta t_d$不是直接關(guān)系，所以就扔掉了，(9a,9b,9c)整理即得完整式(9)，對$\delta t_d$求偏導(dǎo)可得(10)。

由于補(bǔ)償camera pose的估計改變了姿態(tài)，所以Jacobian的求取有些復(fù)雜，QinTong(2018，在待讀文獻(xiàn)2中)有一個想法是直接補(bǔ)償我們特征點的測量值，計算出補(bǔ)償值，應(yīng)用于真實觀測值上得到一個虛擬的觀測值。

1.2 思路2：補(bǔ)償觀測值坐標(biāo)（特征勻速模型）

具體思路是：根據(jù)特征匹配可以知道特征點uv的變化量，假設(shè)特征點是勻速運(yùn)動，可以算出特征點的速度，知道了時間戳偏移$t_d$之后即可求出補(bǔ)償后的虛擬的觀測值坐標(biāo)。

補(bǔ)償觀測值坐標(biāo)由于只改變了觀測的坐標(biāo)，所以求Jacobian較簡單，看（13）中與$t_d$有關(guān)的只有$?u,?v$，所以求導(dǎo)就是$?V$

QinTong的paper論文實驗結(jié)果和結(jié)論：

• 時間戳補(bǔ)償算法有用;
• 時間戳延遲越大，系統(tǒng)精度下降越多。
  

1.3 兩種方法對比

• 特征勻速模型的假設(shè)更強(qiáng)，是假設(shè)特征是勻速運(yùn)動的，而實際上是非線性運(yùn)動，并非勻速，且補(bǔ)償方法也是線性的。
• 而軌跡勻速模型的假設(shè)更接近實際，假設(shè)兩幀間camera是勻速運(yùn)動，補(bǔ)償方法也是非線性的，所以精度比前者高。
  
  實際中對時間戳偏差估計的收斂速度，軌跡勻速法更快。

2. 疑惑

這個是先采集一段數(shù)據(jù)先標(biāo)定出來IMU和Camera的時間戳，然后按照上面的任意一種方法補(bǔ)償?shù)较到y(tǒng)中，還是說邊估計邊補(bǔ)償？

看了些論文的標(biāo)題，應(yīng)該是先標(biāo)定出來的，后面看了論文再來填這個坑。

3. 時間戳同步算法擴(kuò)展

前面有系統(tǒng)初始化完成之后來估計$t_d$的，當(dāng)系統(tǒng)初始化沒有完成時$t_d$如何估計呢？
可以通過VO/SFM求出KF pose，利用$T_{bc}$外參轉(zhuǎn)到body系下，求出i，j時刻的相對位姿，跟IMU預(yù)計分量對比構(gòu)建誤差，多時刻觀測量可進(jìn)行BA，優(yōu)化出時間戳（也可優(yōu)化出gyro bias，速度等，但是優(yōu)化的量多了之后，優(yōu)化精度可能會下降）

4. 總結(jié)

本章主要討論了在VIO系統(tǒng)中對camera和IMU時間戳進(jìn)行對齊的問題，著重討論了將Camera對齊IMU的兩種方法：軌跡勻速模型和特征勻速模型。并對其他的VI時間戳同步算法進(jìn)行了擴(kuò)展。

1. 針對軌跡勻速模型和特征勻速模型：

• 前者假設(shè)兩個KF間的camera之間是勻速運(yùn)動，估計出時間戳延時$t_d$之后，將其補(bǔ)償在估計的camera位姿上，該方法對VIO數(shù)據(jù)的pose進(jìn)行了改動，對于系統(tǒng)Jacobian的改動較大，但假設(shè)相對后者較弱，精度比后者高。
• 后者假設(shè)兩幀KF間的特征點是勻速運(yùn)動，估計出$t_d$后，將其補(bǔ)償在特征點的觀測上，計算出補(bǔ)償之后的觀測，該方法對Jacobian改動較小，而且為線性改動，但缺點也較為明顯，因為該方法假設(shè)較強(qiáng)（特征較難滿足勻速運(yùn)動），所以精度比前者低，時間戳收斂速度比前者慢，但代碼改動小，計算速度簡單。

2. 另外對時間戳同步算法進(jìn)行了擴(kuò)展：

• 待讀4改為對IMU進(jìn)行時間戳補(bǔ)償，精度更高;
• 待讀5用camera的pose計算出traj，進(jìn)而得出$\omega ,a$，與IMU的進(jìn)行align得出$t_d$（這個不清楚，需要讀原文）
• 待讀6（提出Kalibr，行業(yè)標(biāo)桿）使用標(biāo)定板出一段camera pose，再使用B樣條（B Spline）進(jìn)行擬合，一階，二階導(dǎo)可以得出$\omega ,a$，和IMU數(shù)據(jù)進(jìn)行align，優(yōu)化出時間戳，不過使用的是autodiff 數(shù)值Jacobian。
• 待讀7推導(dǎo)出6的解析李群Jacobian。

A：VINS-MONO中可以選擇在線優(yōu)化，優(yōu)化完成之后會得到一個t_d的優(yōu)化結(jié)果，其他框架暫時還不知道。

5. 作業(yè)

6. 待讀文獻(xiàn)

1. 北大的，提出軌跡勻速模型

Weibo Huang, Hong Liu, and Weiwei Wan. “Online initialization and extrinsic spatial-temporal calibration for monocular visual-inertial odometry”. In: arXiv preprint arXiv:2004.05534 (2020).

2. QinTong提出特征勻速模型

Tong Qin and Shaojie Shen. “Online temporal calibration for monocular visual-inertial systems”. In: RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE. 2018, pp. 3662–3669.

3. PTAM的特征勻速模型思路

Georg Klein and David Murray. “Parallel tracking and mapping on a camera phone”. In: 2009 8th IEEE International Symposium on Mixed and Augmented Reality. IEEE. 2009, pp. 83–86.
這篇是2009年的PTAM，里面的思想上面QinTong這篇一樣。

4. 認(rèn)為camera是準(zhǔn)的，補(bǔ)償IMU時間戳

Yonggen Ling et al. “Modeling varying camera-imu time offset in optimization-based visual-inertial odometry”. In: Proceedings of the European Conference on Computer Vision (ECCV). 2018, pp. 484–500.
這篇相對于第1節(jié)的算法，假設(shè)更弱，雖然軌跡勻速是假設(shè)camera間是勻速運(yùn)動，但是如果是Slinding Window中的KF相隔時間久，可能存在較大時間差，勻速假設(shè)不易成立。
這篇改為補(bǔ)償IMU，由于IMU頻率高，數(shù)據(jù)之間的勻速假設(shè)更接近真實值，所以精度會更高，但是每次估計出新的時間戳都需要重新進(jìn)行IMU預(yù)先積分，導(dǎo)致計算量增大。（沒看過原文，IMU預(yù)積分不能找出來和時間戳的關(guān)系嗎，這樣就不用每次重新計算了）。

5. 用camera估出來的 ω , a \omega,a ω,a和IMU測量值進(jìn)行align，得到時間戳偏移

Janne Mustaniemi et al. “Inertial-based scale estimation for structure from motion on mobile devices”. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE. 2017, pp. 4394–4401.

6. ETH大組，Kalibr論文（行業(yè)標(biāo)桿，必讀）

Paul Furgale, Joern Rehder, and Roland Siegwart. “Unified temporal and spatial calibration for multi-sensor systems”. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE. 2013, pp. 1280–1286.
Roland Siegwart好象是組的負(fù)責(zé)人。
使用標(biāo)定板出一段camera pose，再使用B樣條（B Spline）進(jìn)行擬合，一階，二階導(dǎo)可以得出$\omega ,a$，和IMU數(shù)據(jù)進(jìn)行align，優(yōu)化出時間戳。但是解析解太復(fù)雜，這篇論文使用的autodiff數(shù)值Jacobian

7. 推導(dǎo)出待讀6的解析Jacobian（DSO那個組，公式較多）

Christiane Sommer et al. “Efficient derivative computation for cumulative B-splines on Lie groups”. In: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2020, pp. 11148–11156.
丹尼爾 crimse這個組文章來源地址http://www.zghlxwxcb.cn/news/detail-751641.html

到了這里，關(guān)于【深藍(lán)學(xué)院】手寫VIO第8章--相機(jī)與IMU時間戳同步--筆記的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！