vins中的外参标定
之前写过一篇博客介绍vins中进行初始化的操作，其实在这之前，vins会首先进行外参的标定。今天终于找到时间写一下。
外参标定
外参
此处的外参指的是相机坐标系到imu坐标系（body）的变换矩阵。下文中会使用\(T_{bc}\)来表示。
vins中的设置
在vins中可以通过在配置文件中设置estimate_extrinsic值来选择对外参进行如何处理
将这个值设为0时代表提供的外参已经是准确的了，vins之后不会再对其进行优化；
将这个值设为1时代表提供的外参只是一个估计值，vins之后还会将其作为节点初始值放入非线性优化中进行优化；
将这个值设为2时代表并不提供外参，此时vins会在初始化前在线标定外参。
需要说明的是，在vins进行外参的在线标定时，只进行了外参旋转部分的估计，而平移部分是直接初始化为0了。这个是因为一般情况下我们使用的都是相机与imu集成起来的设备，两者之间的距离十分小（几厘米），所以在此处可以直接初始化为0，后续可以通过优化来矫正（或者直接忽略也不会有太大影响）。
标定原理
首先，从第\(k+1\)帧相机坐标系到第\(k\)帧body坐标系的旋转变换\(R_{b_k c_{k+1}}\)可以使用两种如下方式求得
\[ R_{b_k c_{k+1}} = R_{b_k b_{k+1}} * R_{bc} = R_{bc} * R_{c_k c_{k+1}} \]
上面等式用四元数重写为如下形式
\[ q_{b_k b_{k+1}} * q_{bc} = q_{bc} * q_{c_k c_{k+1}} \]
上面的第一个等式我们会发现\(R_{bc}\)在等式两边都出现了，只是一边是左乘了一个矩阵，一边是右乘了一个矩阵，我们可以将等式右边的右乘矩阵\(R_{c_k c_{k+1}}\)变换成左乘形式\(R_{c_k c_{k+1}}^{right}\)然后移动到右边，然后上式子就变成了
\[[ R_{b_k b_{k+1}} - R_{c_k c_{k+1}}^{right} ] * R_{bc} = 0 \]
将多帧之间的等式关系联立我们可以构建一个超定方程\(Ax=0\)。对\(A\)进行\(svd\)分解，其中最小奇异值对应的右奇异向量便是结果\(R_{bc}\)。
vins中的实现
在vins的实现中流程如下：
取得连续两帧之间匹配到（跟踪到）的归一化特征点集合；
根据帧间匹配集合由随意抽样一致方法通过对极几何约束得到前后两帧的相机坐标系的相对旋转变换\(R_{c_k c_{k+1}}\)
根据imu预积分得到前后两帧BODY坐标系的相对旋转变换\(R_{b_k b_{k+1}}\)
连续积累滑动窗口数个上面的方程，解方程得到。
实现细节
在vins实现中，构建的左乘矩阵\(L\)是\(R_{c_k c_{k+1}}\)的左乘矩阵，构建的右乘矩阵\(R\)是\(R_{b_k b_{k+1}}\)，相当于其解方程的公式如下：
\[[ R_{c_k c_{k+1}} - R_{b_k b_{k+1}}^{right} ] * R_{cb} = 0 \]
所以解方程求得的是\(R_{cb}\)，因此代码的最后会对其进行求逆来获得\(R_{bc}\)
参考资料
- [1] https://github.com/HKUST-Aerial-Robotics/VINS-Fusion