在vio系統中，camera-imu間內外參精確與否對整個定位精度起著重要的作用。所以良好的標定結果是定位系統的前提工作。

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

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

相機投影模型

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

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

上圖第二步以rantan畸變為例，對于equidistant畸變模型則是另外一種形式：

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

camera內參標定

camera內參標定如上圖所示，可以簡單概括為四步：

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

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

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

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

這樣即完成相機的內參標定。

B-spline

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

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

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

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

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

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

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

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

camera-imu外參標定

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

camera-imu外參標定大體上分為三步：

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

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

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

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

上面相機內參標定，可以先標定出camera的內參?，F在已知每一幀圖像的3D-2D對應，可以算出每一幀camera的pose。用這些離散的pose構造連續的B-spline，就可以獲取任意時刻pose。

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

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

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

現在利用兩個曲線的相關性，可以粗略估計imu和camera時間延時：

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

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

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

同樣利用角速度測量關系，這次構造一個優化問題：

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

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

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

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

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

為了簡化imu測量誤差的構建，這里利用camera pose??乘上上面計算出來外參[??, 0]，得到imu的pose??曲線。當然這個曲線可能誤差比較大，會在后續優化過程中進行調整。

特征角點的重投影誤差：

這里帶??表示要優化的參數，??即上面相機投影模型，??、??由imu狀態計算camera狀態時需要考慮camera-imu間時間延時。

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

?、??分別對樣條值旋轉??、位移??部分對時間求一階、二階導可得。??模長固定，只優化方向。

偏置隨機游走噪聲：

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

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

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

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

至此加入外參標定過程中所有的誤差項。優化的量包括：

1、camera-imu外參部分包括位移??、旋轉??、時間延時?

2、重力加速度

3、各個時間節點的??、?

4、各個時間節點的??、?

其中1、2為常量，3、4為隨時間變化的量。構造整個優化problem即可完成camera-imu的外參標定。

imu內參標定

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

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

為了估計imu器件本身內參，只需要在外參標定過程中，替代其對應的測量模型即可，參考[1]。

內外參標定工作思考

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

編輯：黃飛