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

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

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

相機(jī)投影模型

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

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

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

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

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

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

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

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

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

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

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

B-spline

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

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

但是這樣存在一個(gè)問(wèn)題，為了讓擬合的曲線和離散點(diǎn)盡可能吻合，需要不斷增加這個(gè)多項(xiàng)式的階數(shù)，這樣可能帶來(lái)計(jì)算耗時(shí)增加。而且有時(shí)候調(diào)整曲線上某一段，可能會(huì)動(dòng)上整個(gè)曲線。

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

每一段采用一個(gè)不同的多項(xiàng)式，而這個(gè)多項(xiàng)式系數(shù)采用了一種遞歸的方式來(lái)計(jì)算：

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

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

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

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

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

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

1、粗略估計(jì)camera與imu之間時(shí)間延時(shí)。

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

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

粗略估計(jì)camera與imu之間時(shí)間延時(shí)

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

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

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

這樣用camera和imu測(cè)量出來(lái)角速度隨時(shí)間原始曲線：

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

對(duì)齊后曲線則是下面這樣：

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

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

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

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

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

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

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

誤差項(xiàng)包括所有標(biāo)定板角點(diǎn)重投影誤差、imu加速度計(jì)與陀螺儀測(cè)量誤差、偏置的隨機(jī)游走噪聲(相對(duì)特殊點(diǎn))。

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

特征角點(diǎn)的重投影誤差：

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

陀螺儀、加速度計(jì)測(cè)量誤差：

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

偏置隨機(jī)游走噪聲：

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

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

在非線性最小二乘中，不同于角點(diǎn)重投影誤差、imu的測(cè)量誤差，在整個(gè)cost fuction中，只需要對(duì)單個(gè)測(cè)量誤差項(xiàng)求和：

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

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

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

2、重力加速度

3、各個(gè)時(shí)間節(jié)點(diǎn)的??、?

4、各個(gè)時(shí)間節(jié)點(diǎn)的??、?

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

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

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

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

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

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

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

編輯：黃飛