在vio系統(tǒng)中，camera-imu間內(nèi)外參精確與否對整個定位精度起著重要的作用。所以良好的標(biāo)定結(jié)果是定位系統(tǒng)的前提工作。

目前標(biāo)定算法主要分為離線和在線標(biāo)定，離線標(biāo)定以kalibr為代表，能夠標(biāo)定camera內(nèi)參、camera-imu之間位移旋轉(zhuǎn)、時間延時以及imu自身的刻度系數(shù)、非正交性等。

本文基于kalibr[2]整體框架，介紹標(biāo)定算法原理。

相機投影模型

這里以常用的針孔相機投影為例，而畸變模型包括最常見rantan、equidistant兩種。對于其他投影、畸變模型可查閱相關(guān)資料，這里不詳細(xì)敘說。對于空間中3D點怎樣投影得到像素平面2D坐標(biāo)：

由上圖，可以看到空間三維點??由三角形相似，可以簡單計算出來像素平面坐標(biāo)??。實際投影成像過程不想上圖那么簡單，主要包含三步：

上圖第二步以rantan畸變?yōu)槔?，對于equidistant畸變模型則是另外一種形式：

投影過程首先是算出歸一化平面上點，然后再對歸一化平面上點加rantan、equidistant畸變，最后再作用于內(nèi)參焦距、主點，得到成像像素點坐標(biāo)。

camera內(nèi)參標(biāo)定

camera內(nèi)參標(biāo)定如上圖所示，可以簡單概括為四步：

1.???從圖像中提取標(biāo)定板角點。根據(jù)標(biāo)定板類型，常見三種：checkboard、april grid和circle grid，每一種都有各自提取角點的方法，具體可查詢相關(guān)資料。

2.???對于一個非線性優(yōu)化問題，往往需要有一個不錯的初始值。對于主點坐標(biāo)??、??可以簡單地初始化為圖像尺寸的1/2。焦距部分參考[4]，即真實世界平行的直線經(jīng)過成像投影之后，不再是平行的，相交于兩點(有的地方稱作vashing point)。所以對標(biāo)定板每一行的點，都可以在圖像上擬合一個圓，兩個圓交點的距離除以??即可得到焦距初始值。

3.???把標(biāo)定板作為固定參考坐標(biāo)系，因為標(biāo)定板尺寸已知，這樣就知道每一個角點在這個參考坐標(biāo)系下3D坐標(biāo)。知道3D-2D的對應(yīng)，即可求解出每一幀camera的pose，這個pose會作為后續(xù)優(yōu)化的變量。

4.???通過每一幀相機的pose把參考坐標(biāo)系下3D點轉(zhuǎn)換到相機坐標(biāo)系下，通過相機的投影模型，即可得到標(biāo)定板3D角點在圖像平面上的預(yù)測投影位置。通過優(yōu)化每一幀相機的Pose和相機內(nèi)參，來讓所有的重投影誤差最小。

這樣即完成相機的內(nèi)參標(biāo)定。

B-spline

已知一些離散的點，怎樣用一條連續(xù)光滑曲線來把它們串聯(lián)起來。比較容易想到是貝賽爾曲線，

用一個高階的多項式，附上多項式系數(shù)，即可算出每一時刻的曲線值：

但是這樣存在一個問題，為了讓擬合的曲線和離散點盡可能吻合，需要不斷增加這個多項式的階數(shù)，這樣可能帶來計算耗時增加。而且有時候調(diào)整曲線上某一段，可能會動上整個曲線。

為了結(jié)局這個問題，B-spline采用了一個更加聰明方法。把整個曲線分很多段

每一段采用一個不同的多項式，而這個多項式系數(shù)采用了一種遞歸的方式來計算：

這樣帶來一個問題，就是每一時刻曲線值只與這一段前p個控制系數(shù)有關(guān)，這個p是樣條的階數(shù)。

最左邊的是p個列向量組成的矩陣，每一列??是要優(yōu)化的樣條系數(shù)。中間??矩陣維度??，是個固定常量，只與整個b-spline knot時間分布有關(guān)。最右邊是p行列向量，其形式為：

只和位于這個曲線段時間點有關(guān)。這樣就很容易對每一時刻的樣條值求一階導(dǎo)、二階、多階導(dǎo)等等，這個對于后面camera-imu外參標(biāo)定非常重要。

camera-imu外參標(biāo)定

知道imu測試每一時刻加速度和角速度，對加速度、角速度進行積分可以得到速度、位置、旋轉(zhuǎn)。不同于SLAM中對離散imu數(shù)據(jù)進行積分得到狀態(tài)可能帶來較大的誤差，采用對時間連續(xù)的狀態(tài)求導(dǎo)來反推imu數(shù)據(jù)。把離散的狀態(tài)描述成連續(xù)的就需要B-spline。

camera-imu外參標(biāo)定大體上分為三步：

1、粗略估計camera與imu之間時間延時。

2、獲取imu-camera之間初始旋轉(zhuǎn)，還有一些必要的初始值：重力加速度、陀螺儀偏置。

3、大優(yōu)化，包括所有的角點重投影誤差、imu加速度計與陀螺儀測量誤差、偏置隨機游走噪聲。

粗略估計camera與imu之間時間延時

上面相機內(nèi)參標(biāo)定，可以先標(biāo)定出camera的內(nèi)參?，F(xiàn)在已知每一幀圖像的3D-2D對應(yīng)，可以算出每一幀camera的pose。用這些離散的pose構(gòu)造連續(xù)的B-spline，就可以獲取任意時刻pose。

注意這里對pose參數(shù)化采用六維的列向量，分別三維的位移??和旋轉(zhuǎn)矢量??。對位移和旋轉(zhuǎn)矢量分別求一階導(dǎo)、二階導(dǎo)可以得到速度與加速度：

利用camera的樣條曲線獲取任意時刻camera旋轉(zhuǎn)角速度，而陀螺儀又測量imu的角速度。忽略偏置和噪聲影響，兩者相差一個旋轉(zhuǎn)，且模長相等：

這樣用camera和imu測量出來角速度隨時間原始曲線：

現(xiàn)在利用兩個曲線的相關(guān)性，可以粗略估計imu和camera時間延時：

對齊后曲線則是下面這樣：

可以看到利用相關(guān)性，可以把時間延時誤差縮小到1-2個imu周期范圍內(nèi)。

獲取imu-camera之間初始旋轉(zhuǎn)，還有一些必要的初始值：重力加速度、陀螺儀偏置

同樣利用角速度測量關(guān)系，這次構(gòu)造一個優(yōu)化問題：

這樣就可以獲得camera-imu之間的旋轉(zhuǎn)，以及陀螺儀偏置初始值。

忽略加速度偏置與噪聲，假設(shè)整個標(biāo)定過程中平均加速度為零，所以也可以獲得重力加速度在參考坐標(biāo)系下的表示：

大優(yōu)化，包括所有的角點重投影誤差、imu加速度計與陀螺儀測量誤差、偏置隨機游走噪聲

前面兩步為最后大優(yōu)化提供一個不錯的初始值，接著大優(yōu)化就是調(diào)整所有要優(yōu)化的變量來讓所有的觀測誤差最小。

誤差項包括所有標(biāo)定板角點重投影誤差、imu加速度計與陀螺儀測量誤差、偏置的隨機游走噪聲(相對特殊點)。

為了簡化imu測量誤差的構(gòu)建，這里利用camera pose??乘上上面計算出來外參[??, 0]，得到imu的pose??曲線。當(dāng)然這個曲線可能誤差比較大，會在后續(xù)優(yōu)化過程中進行調(diào)整。

特征角點的重投影誤差：

這里帶??表示要優(yōu)化的參數(shù)，??即上面相機投影模型，??、??由imu狀態(tài)計算camera狀態(tài)時需要考慮camera-imu間時間延時。

陀螺儀、加速度計測量誤差：

?、??分別對樣條值旋轉(zhuǎn)??、位移??部分對時間求一階、二階導(dǎo)可得。??模長固定，只優(yōu)化方向。

偏置隨機游走噪聲：

如果假設(shè)上述偏置??、??隨時間變化，則需要添加偏置隨機游走噪聲：

所以像pose曲線一樣，需要分別對加速度偏置和陀螺儀偏置生成一個樣條曲線，只不過對于樣條上初始值加速度偏置設(shè)為0，陀螺儀偏置為第二步估計的初始值。

在非線性最小二乘中，不同于角點重投影誤差、imu的測量誤差，在整個cost fuction中，只需要對單個測量誤差項求和：

而偏置隨機游走誤差需要對一段時間內(nèi)進行積分[3]：

至此加入外參標(biāo)定過程中所有的誤差項。優(yōu)化的量包括：

1、camera-imu外參部分包括位移??、旋轉(zhuǎn)??、時間延時?

2、重力加速度

3、各個時間節(jié)點的??、?

4、各個時間節(jié)點的??、?

其中1、2為常量，3、4為隨時間變化的量。構(gòu)造整個優(yōu)化problem即可完成camera-imu的外參標(biāo)定。

imu內(nèi)參標(biāo)定

對于一些廉價的imu設(shè)備，加速度計、陀螺儀本身可能存在三個軸非正交，以及每個軸存在刻度系數(shù)誤差，實際的加速度計測量方程就是這樣的形式：

對于陀螺儀，除了存在非正交性、刻度系數(shù)誤差，還額外加速度計敏感項、以及加速度計與陀螺儀三軸之間角度誤差：

為了估計imu器件本身內(nèi)參，只需要在外參標(biāo)定過程中，替代其對應(yīng)的測量模型即可，參考[1]。

內(nèi)外參標(biāo)定工作思考

評估一個算法好壞大體上分兩個指標(biāo)：效率與精度。通過一些技巧能夠提高標(biāo)定的效率。但是對于精度部分，做標(biāo)定工作時候，往往不知道設(shè)備的準(zhǔn)確值，或者能拿到可能就設(shè)備結(jié)構(gòu)設(shè)計值來作為參考，所以評估算法標(biāo)定精度也是一件棘手的問題。

編輯：黃飛