【泡泡读者来稿】VINS 论文推导及代码解析（二）

崔华坤，SLAM算法工程师，主要研究方向是VIO。2011年硕士毕业于北京工业大学，毕业后从事六年的三维显示研发，从最初的裸眼3D显示到现在的AR显示，期间做出一款国内尺寸最大的裸眼3D LED屏。对作者感兴趣的可以加微信:cuihuakun。

---

后端非线性优化

状态向量

状态向量共包括滑动窗口内的n+1个所有相机的状态（包括位置、朝向、速度、加速度计bias和陀螺仪bias）、Camera到IMU的外参、m+1个3D点的逆深度：

目标函数

其中三个残差项即误差项分别为边缘化的先验信息、IMU测量残差、视觉的重投影残差。三种残差都是用马氏距离表示。

根据《十四讲》中高斯-牛顿法，若要计算目标函数的最小值，可以理解为，当优化变量有一个增量后，目标函数值最小，以IMU残差为例，可写成如下所示：

其中 J_{b_{k+1}}^b_k 为误差项关于所有状态向量（即优化变量）X的Jacobian，将上式展开并令关于的导数为0，可得增量的计算公式：

那么，对于公式(19)的整体目标函数的整体增量方程可写成：

上式中，P_{b_{k+1}}^{b_k}为IMU预积分噪声项的协方差，P_l^{C_j}为visual观测的噪声协方差。当IMU的噪声协方差P_{b_{k+1}}^{b_k}越大时，其信息矩阵{P_{b_{k+1}}^{b_k}}^-1将越小，意味着该IMU观测越不可信，换句话说，因IMU噪声较大，越不可信IMU预积分数据，而更加相信visual观测。注意，这里的IMU和visual协方差的绝对值没有意义，因为考虑得是两者的相对性。

可将上式继续简化为：

其中Λ_p，Λ_B，Λ_C和为Hessian矩阵，上述方程称之为增量方程。

值得说明的是，上面中的Jacobian虽然是误差项r关于状态向量X的一阶导，但在具体求解时，通常采用扰动的方式进行计算，如下所示，注意其中δX的是状态向量的微小扰动，并非上面求解的增量∆X：

IMU约束

1）残差：两帧之间的PVQ和bias的变化量的差

其中各增量关于bias的Jacobian可从公式(16)的大Jacobian中的相应位置获得。上面与代码中IntegrationBase::evaluate()对应。

2）优化变量：

3）Jacobian：

计算Jacobian时，残差对应的求偏导对象为上面的优化变量，但是计算时采用扰动方式计算，即扰动为

推导可参考附录10.4。

推导可参考附录10.5。

推导可参考附录10.6

上面公式在代码中对应：

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 对于Evaluate输入double const *const *parameters, parameters[0], parameters[1], parameters[2], parameters[3]分别对应4个输入参数, 它们的长度依次是7,9,7,9，分别对应4个优化变量的参数块。

代码IMUFactor::Evaluate()中residual还乘以一个信息矩阵sqrt_info，这是因为真正的优化项其实是Mahalanobis 距离：d=r^TP^{-1}r，P是协方差，又因为Ceres只接受最小二乘优化，也就是, 所以把P^{-1}做LLT分解，即LL^{T}=P^{-1}，则有：

令r'=L^Tr作为新的优化误差，这样就能用Ceres求解了。Mahalanobis距离其实相当于一个残差加权，协方差大的加权小, 协方差小的加权大, 着重优化那些比较确定的残差. 若写成“sqrt_info.setIdentity()”相当于不加权。

4）协方差

上面提到的IMU协方差P为公式（17）推导的IMU预积分中迭代出来的IMU增量误差的协方差。

视觉约束

1) 残差

视觉残差是重投影误差，对于第l个路标点P，将P从第一次观看到它的第i个相机坐标系，转换到当前的第j个相机坐标系下的像素坐标，可定义视觉误差项为：

其中，P ̅_l^(c_j )为第l个路标点在第j个相机归一化相机坐标系中的观察到的坐标：

另外，P_l^(c_j )是估计第l个路标点在第j个相机归一化相机坐标系中的可能坐标，推导可参附录10.7：

因为视觉残差的自由度为2，因此将视觉残差投影到正切平面上，b1,b2为正切平面上的任意两个正交基。

图4 在单位球面上的视觉残差

2) 优化变量

3) Jacobian

根据视觉残差公式，我们可以得到相对于各优化变量的Jacobian，推导可参考附录10.7：

4）协方差

视觉约束的噪声协方差与标定相机内参时的重投影误差，也就是偏离几个像素有关，代码对应为ProjectionTdFactor::sqrt_info，这里取的1.5个像素，对应到归一化相机平面上的协方差矩阵需除以焦距f，则信息矩阵等于协方差矩阵的逆，为：

5）如果考虑Cam和IMU的时间戳偏移量td，则需要增加优化变量，后续再补。

前端视觉处理

特征点检测

Frame 1: goodFeaturesToTrack检测MAX_CNT个特征点，设置forw_pts如下：

表1 第一帧的特征点

特征点跟踪

Frame 2: calcOpticalFlowPyrLK跟踪，将跟踪失败的点删除，跟踪成功的点点跟踪计数+1，并调用goodFeaturesToTrack检测出MAX_CNT - forw_pts.size()个特征点，并添加到forw_pts中，并调用updateID更新ids，最后得到的forw_pts如下：

表2 第二帧的特征点

代码FeatureTracker::undistortedPoints()中cur_un_pts为归一化相机坐标系下的坐标，pts_velocity为当前帧相对前一帧特征点沿x,y方向的像素移动速度。

初始化

初始化采用视觉和IMU的松耦合方案，首先用SFM求解滑窗内所有帧的位姿，和所有路标点的3D位置，然后跟IMU预积分的值对齐，求解重力方向、尺度因子、陀螺仪bias及每一帧对应的速度。初始化的流程图如下所示： 

                                    图5 初始化流程图

首先，我们对代码中几个易混变量进行说明：

1)  传到processImage的image表示当前帧跟踪到上一帧中的特征点集合，也就是当前帧观测到的所有的路标点（不包括在当前帧新提取的点），如表2中track_cnt≥2的feature，image类型为：

2)     FeatureManager中几个变量：

   图6 FeatureManager中的几个易混变量

relativePose

在滑窗内寻找与当前帧的匹配特征点数较多的关键帧作为参考帧，并通过求基础矩阵cv::findFundamentalMat计算出当前帧到参考帧的T；

GlobalSFM.construct

首先，将图4中feature队列，放到vector<SFMFeature> sfm_f中，其中SFMFeature中的observation 存放的是观测到该路标点的FrameId及特征点坐标，如下所示：

图7 SFM中的几个易混变量

GlobalSFM的流程如下，其中，参考帧为上一步选出来的最共视帧，最后得到的sfm_tracked_points为三角化出来的3D路标点。

图8 SFM流程图

solvePnP

对滑窗内每一帧图像，都跟上一步SFM得到的所有3D路标点，进行cv::solvePnP求解位姿。

纯视觉初始化时，我们采用第一帧c0作为基准坐标系，若要转化为从body坐标系到c0坐标系，可以进行如下变换：

上式推导如下，这里采用个人较熟悉的写法：

visualInitialAlign

图9 IMU预积分与视觉结构对齐

1) 陀螺仪bias校正：对应代码中solveGyroscopeBias()函数

目标函数为：visual给出的相邻帧间的旋转应等于IMU预积分的旋转值

上述目标函数的最小值为单位四元数，所以可以将目标函数进一步写为：

我们只考虑虚部，则有：

将左边转为正定阵，这样就可以直接用Cholesky进行分解了：

求解出陀螺仪的bias后，需要对IMU预积分值进行重新计算。

2)初始化速度、重力和尺度因子：对应代码中LinearAlignment ()函数

优化变量为：

其中，为在第0帧Camera相机坐标系下的重力向量。

残差可定义为相邻两帧之间IMU预积分出的增量α_(b_(k+1))^(b_k )，β_(b_(k+1))^(b_k )，与预测值之间的误差，可写成：

将公式(30)代入上式中的δα_(b_(k+1))^(b_k )可得：

我们想办法将上式转为Hx=b的形式，这样便于直接利用Cholesky进行求解：

转成矩阵形式可写成：

同样的也将转为矩阵形式，综合上式可写成：

即：

这样，可以用Cholosky分解下面方程求解X1：

3)     修正重力矢量：对应代码中RefineGravity()函数

因此重力矢量的模固定，因此将为2个自由度，可写成：

图10 两自由度的重力矢量参数化

将上式代入(36)，重新整理可得：

这样，可以用Cholosky分解下面方程求解X1：

这样我们就得到了在C0系下的重力向量g^{C_0}，通过将旋转至惯性坐标系中的z轴方向，可以计算相机系到惯性系的旋转矩阵q_(C_0)^w，这样就可以将所有变量调整至惯性世界系中。

未完待续

欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。

有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！

泡泡网站：www.paopaorobot.org

泡泡论坛：http://paopaorobot.org/bbs/

 

泡泡机器人SLAM的原创内容均由泡泡机器人的成员花费大量心血制作而成，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

商业合作及转载请联系liufuqiang_robot@hotmail.com