VINS技术路线与代码详解

VINS技术路线

   写在前面：本文整和了各大佬以及自己的思路，希望对学习VINS或者VIO的同学有所帮助，如果你觉得文章写的对你的理解有一点帮助，可以推荐给周围的小伙伴们，顺便给点个赞哦，当然，如果你有任何问题想要交流，欢迎随时探讨，我的QQ：605096939。话不多说，下面上正文。

   VINS代码主要包含在两个文件中，分别是feature_tracker和vins_estimate，feature_tracker就像文件的名字一样，总体的作用是接收图像，使用KLT光流算法踪；vins_estimate包含相机和IMU数据的前端预处理（也就是预积分过程）、单目惯性联合初始化（在线的标定过程）、基于滑动窗口的BA联合优化、全局的图优化和回环检测等。要想真正的理解一个SLAM框架，必须真正搞懂其对应的算法模型，然后才能研究其代码逻辑，最后做到相得益彰的效果，因此本次讲解主要是结合论文中的理论知识这和两个文件中的代码进行详细的探讨。整体的框架都比较熟悉，如下图所示，第一部分是Measuremen Preprocessing：观测值数据预处理，包含图像数据跟踪IMU数据预积分；第二部分是Initialization：初始化，包含单纯的视觉初始化和视觉惯性联合初始化；第三部分Local Visual-Inertia BA and Relocalization：局部BA联合优化和重定位，包含一个基于滑动窗口的BA优化模型；第四部分Global Pose Graph Optimization：全局图优化，只对全局的位姿进行优化；第五部分Loop detection：回环检测。

 

一．Feature_tracker文件夹中

        首先讲第一部分，也就是纯粹的图像处理部分内容，在论文中的第IV点观测值预处理的A部分视觉前端处理，为了更好的理解代码，有必要将论文中的相关内容和大家讨论一番。

         论文内容：每当进入新的图像，都会使用KLT稀疏光流法进行跟踪，同时提取100-300个角点信息，我的理解是角点是用来建立图像，光流跟踪是用来快速定位。同时在这里还进行了关键帧的选取，主要是两个剔除关键帧的策略，分别是平均视差法和跟踪质量法。平均视差法：如果当前帧的和上一个关键帧跟踪点的平均视差超出了一个设定的阈值，就将当前帧设为关键帧。这里有一个问题，就是旋转和平移都会产生视差（不只是平移哦），当出现纯旋转的时候特征点无法被三角化，无法计算出旋转值，也就无法计算跟踪点间的平均视差，为了解决这一问题，采用短时的陀螺仪观测值来补偿旋转，从而计算出视差，这一过程只应用到平均视差的计算，不会影响真实的旋转结果。

         具体代码实现：主要负责图像角点提取和光流跟踪，只有一个主线程。主要是三个源程序，分别是feature_tracker、feature_tracker_node以及parameters。feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。

1.      feature_tracker_node.cpp系统入口

（1）      main()函数

步骤1：readParameters(n);读取参数，是config->euroc->euroc_config.yaml中的一些配置参数。

步骤2: trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);在这里NUM_OF_CAM设置成常量1，只有一个摄像头（单目），读取相机内参。

步骤3:判断是否加入鱼眼mask来去除边缘噪声

步骤4: ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100,img_callback); 订阅话题和发布话题，监听IMAGE_TOPIC（/cam0/image_raw），有图像发布到这个话题上的时候，执行回调函数，这里直接进入到img_callback函数中接收图像，前端视觉的算法基本在这个回调函数中。

1）  img_callback(const sensor_msgs::ImageConstPtr &img_msg)接收图像

步骤1: 频率控制，保证每秒钟处理的image不多于FREQ，这里将平率控制在10hz以内。

步骤2: 处理单目相机

步骤2.1: trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW *(i + 1)));读取到的图像数据存储到trackerData中，读取完之后如果图像太亮或太黑（EQUALIZE=1），使用createCLAHE对图像进行自适应直方图均衡化，如果图像正常，设置成当前图像。在读取图像的时候进行光流跟踪和特征点的提取。FeatureTracker类中处理的主要函数就是readImage()，这里涉及到3个img(prev_img, cur_img, forw_img)和pts(prev_pts,cur_pts, forw_pts)，两者是相似的。刚开始看不是太好理解，cur和forw分别是LK光流跟踪的前后两帧，forw才是真正的“当前”帧，cur实际上是上一帧，而prev是上一次发布的帧，它实际上是光流跟踪以后，prev和forw根据Fundamental Matrix做RANSAC剔除outlier用的，也就是rejectWithF()函数. readImage()的处理流程为:

①先调用cv::CLAHE对图像做直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗)

②调用calcOpticalFlowPyrLK()跟踪cur_pts到forw_pts,根据status,把跟踪失败的点剔除(注意:prev, cur,forw, ids, track_cnt都要剔除),这里还加了个inBorder判断,把跟踪到图像边缘的点也剔除掉.

③如果不需要发布特征点,则到这步就完了,把当前帧forw赋给上一帧cur, 然后退出.如果需要发布特征点(PUB_THIS_FRAME=1), 则执行下面的步骤

④先调用rejectWithF()对prev_pts和forw_pts做ransac剔除outlier.(实际就是调用了findFundamentalMat函数), 在光流追踪成功就记被追踪+1，数值代表被追踪的次数，数值越大，说明被追踪的就越久

⑤调用setMask(), 先对跟踪点forw_pts按跟踪次数降排序, 然后依次选点, 选一个点, 在mask中将该点周围一定半径的区域设为0, 后面不再选取该区域内的点. 有点类似与non-max suppression, 但区别是这里保留track_cnt最高的点.

⑥在mask中不为0的区域,调用goodFeaturesToTrack提取新的角点n_pts, 通过addPoints()函数push到forw_pts中, id初始化-1,track_cnt初始化为1.

整体来说需要注意的是：光流跟踪在②中完成，角点提取在⑥中完成

 

步骤2.2:判断是否需要显示畸变。

步骤2.3:将特征点矫正（相机模型camodocal）后归一化平面的3D点（此时没有尺度信息，3D点p.z=1），像素2D点，以及特征的id，封装成ros的sensor_msgs::PointCloud消息类型的feature_points实例中；将图像封装到cv_bridge::CvImageConstPtr类型的ptr实例中

步骤3: 发布消息的数据

pub_img.publish(feature_points);

pub_match.publish(ptr->toImageMsg())

将处理完的图像信息用PointCloud实例feature_points和Image的实例ptr消息类型，发布到"feature"和"feature_img"的topic（此步骤在main函数中完成）

 

至此，已经将图像数据包装成特征点数据和图像数据发布出来了，下面就是在开一个线程，发布一个话题，接收这两种消息，也就是下面的vins_esitimate文件中做的事。

下面是具体的流程图：

二．Vins_estimate文件夹中

 

       下面到第二部分了，也是整个框架的重点和难点，估计要花很长的时间学习和总计理论知识和代码逻辑，这一部分还是从论文的理论知识讲起，然后再附上代码消化理解。     

         论文内容：

1.      论文第IV点的B部分IMU预积分，

IMU预积分的作用是计算出IMU数据的观测值以及其协方差矩阵，为后面的联合初始化提供初值以及后端优化提供IMU的约束关系。原始陀螺仪和加速度计的观测值数据：

第一个式子等式左边是加速度测量值（你可以从加速度计中读到的值），等式右边是加速度真实值（其实就是准确的值，我们需要得到的是这个真实值）加上加速度计的偏置、重力加速度和加速度噪声项。第二个式子等式左边是陀螺仪测量值，等式右边是陀螺仪真实值加上陀螺仪偏置和陀螺仪噪声项。这里的值都是IMU（body）帧坐标系下的。这里假设噪声是服从高斯正态分布，而偏置服从随机游走模型。

         有上面最原始的式子积分就可以计算出下一时刻的p、v和q:

这里等式左边的值都是世界坐标系下（W）bk+1帧的值。从整个式子可以看出来这里的状态传播需要bk帧下的旋转，平移和速度，当这些开始的状态发生改变的时候，就需要重新传播IMU观测值，也就是说状态传播方程要重新计算和修改，我们想要一次性就求出bk和bk+1之间的状态传播，因此选用预积分模型（其实这里我也没有完全搞明白，但是有一点是明白的，这里是在世界坐标系下求解状态，但是由条件里需要世界坐标系下的旋转，明显冲突啊，因此可以使用预积分将世界坐标系下的状态转换到IMU的bk帧坐标系下，最初提出预积分的外国大牛市将世界坐标系转换到求状态的变化量，其实两者的原理都一样，预积分求的值都是变化量），两边同时乘以世界到bk帧坐标系的转换，如下图所示，然后提出等式右边只与加速度和角速度有关的量进行积分，如公式6：

到这里其实只要求出公式6中的积分值，真的预积分的值就得到了，这里bk是参考帧，从式6中可以看出，在bk到bk+1帧间，这里要求的三个临时状态量只与IMU的偏置有关系，而与其他状态无关，也就是说每个式子相当于一个二元一次方程（f(x,y)=ax+by+c,x相当于加速度计偏置，y相当于陀螺仪偏置），这里就是为了求解这个二元一次方程，当这里的偏置变化特别小的时候，我们可以使用一阶线性展开来调整临时状态量，如下公式12 所示：

所以要想求出这个临时的状态量，就必须求出等式右边的两部分值，第一个部分还是原来的积分形式（就像公式6那样），是预积分的主体，论文中使用的是最简单的欧拉积分法进行展开（取第i时刻值的斜率乘以时间差加上i时刻的初值，就得到i+1时刻的值），但是在代码中作者也提出了采用的是中值积分（顾名思义这里的斜率取得是i和i+1中点（2i+1）/2的时刻斜率）.公式7是采用欧拉积分的结果。这里前面有一定的说明，一开始abkbk，bbkbk等是零，旋转是单位旋转，注意整个过程把噪声设为0。

         第二部分其实就是对应的一阶偏导（对加速度计偏置和陀螺仪偏置的），一阶偏导的求法在下面进行介绍，到这里我们已经求出了临时状态量的测量值，也就可以求出状态量测量值。

论文到这一步预积分其实已经做了一半了，也就是完成了测量值的求解，还差什么呢？当然是协方差矩阵了，下面重点求解协方差矩阵，顺便把上面没有求出的陀螺仪和加速度计偏置的雅各比矩阵求出来。

         如何求协方差矩阵呢？怎么从数学的定义里去求呢？这里要用到SLAM中的神作state estimation for robotic，建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。也就是说还是需要前面求解状态测量值的公式6。注意代码中真正求解公式6使用的是中值法，所以为了和代码中相一致，下面的求解过程我也才用中值法的方式，为求解需求我们先补充点干货：

首先需要将上面的四自由度的旋转转换成三个维度的状态量，这是由于四自由度的旋转存在过参数数化的情况，因此将误差看成是一个扰动定义式8：

然后有下面的两张图定义出来离散状态下的预积分过程：

然后有下面的两张图定义出来离散状态下的预积分过程：

        最后得到图中的线性误差状态传播模型，由此得到协方差矩阵和雅各比矩阵，雅各比矩阵可以直接代入到公式12中计算出更加精确的传播状态值，而协方差矩阵自然是在后端优化中使用。到这里恭喜你已经完成了数据前端处理的所有步骤，下面直接进入初始化的过程吧！

2.1首先初始化设置节点vins_estimator，同时读取参数和设置相应的参数，为节点发布相应的话题，同时为节点订阅三个话题，分别用来接收和保存IMU数据、图像特征数据和原始图像数据，分别是在三个回调函数中imu_callback、feature_callback和raw_image_callback。imu_callback函数中首先将IMU数据保存到imu_buf中，同时唤醒作用于process线程中的获取观测值数据的程序，然后预测未考虑观测噪声的p、v、q值，同时将发布最新的IMU测量值消息（pvq值）。feature_callback和raw_image_callback函数中主要是将特征数据和原始图像数据分别保存到feature_buf和image_buf中。

 

2.2开启process线程函数，首先等待得到数据的代码在imu_callback中被唤醒，当被唤醒后，得到包含“一帧图像的特征和帧间的一组IMU观测数据”的measurements数据。得到数据这里调用了getMeasurements方法，首先是判断IMU数据和图像数据是否对齐，确保的到的IMU数据是两个图像帧之间的。

         接着分别处理IMU数据和图像数据。首先处理IMU数据，调用send_imu将单个IMU数据的dt，线加速度值和角加速度值计算出来送给优化器处理，优化器调用estimator.processIMU方法，在这个方法中调用IMU预积分tmp_pre_integration->push_back(dt, linear_acceleration,angular_velocity); propagation ,计算对应的状态量、协方差和雅可比矩阵，在push_back内部有调用方法propagate(dt, acc, gyr)，在预积分传播方程propagate中使用中点积分方法midPointIntegration计算预积分的测量值，中点积分法中主要包含两个部分，分别是得到残差值result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新协方差矩阵和雅可比矩阵。计算残差使用的是由前面的状态数据计算当前的状态数据，在一开始的时候由构造函数给相应的初始值，然后通过指点积分法得到当前时刻的状态变量。计算跟新雅可比矩阵，这里的值域论文中提供的不同，相应的请参考博客http://www.cnblogs.com/buxiaoyi/p/7353353.html#%5B2%5D的解释。最后残差和雅可比矩阵、协方差矩阵保存在pre_integrations中。

         然后处理图像特征数据，一开始根据视差判断这一帧是不是关键帧，如果是关键帧就丢弃最老的一帧，如果不是关键帧就丢弃视觉观测值保留IMU观测值，首先将数据和时间保存到图像帧的对象imageframe中（对象中包含特征点，时间，位姿R，t，预积分对象pre_integration）,同时将临时的预积分值保存到此对象中，然后将图像帧的对象imageframe保存到all_image_frame对象中，跟新临时预积分初始值。接着如果没有外部参数就标定外部参数，参数传递有的话就跳过这一步（默认有）。然后初始化系统同时进行local BA优化，当求解器处于可初始化状态时，判断当前frame_count是否达到WINDOW_SIZE，确保有足够的frame参与初始化，这里的frame_count是图像帧的数量，一开始被初始化为0。在初始化中主要是调用了initialStructure()方法，在initialStructure()初始化系统中，首先初始化Vision-only SFM，然后初始化Visual-Inertial Alignment，构成整个初始化过程。一开始通过线加速度的标准差（离散程度）判断保证IMU充分运动，加速度标准差大于0.25则代表imu充分激励，足够初始化。纯视觉初始化，对Sliding Window中的图像帧和相机姿态求解sfm问题，这里解决的是关键帧的位姿和特征点坐标。首先构建SFMFeature对象sfm_f，SFMFeature数组中包含了特征点状态（是否被三角化），id，2d点，3d坐标以及深度，将特征管理器中的特征信息保存到SFMFeature对象sfm_f中sfm_f.push_back(tmp_feature)。接着由对极约束中的F矩阵恢复出R、t，主要调用方法relativePose(relative_R,relative_T, l)。relativePose方法中首先通过FeatureManeger获取（滑动窗口中）第i帧和最后一帧的特征匹配corres，当corres匹配足够大时，考察最新的keyFrame和sliding window中某个keyFrame之间有足够feature匹配和足够大的视差（id为l=i），满足这两个条件，然后这两帧之间通过五点法恢复出R，t并且三角化出3D的特征点feature point，这里是使用solveRelativeRT(corres, relative_R, relative_T)，solveRelativeRT方法定义在solv_5pts.cpp类中，由对极约束中的F矩阵恢复出R、t，直接调用opencv中的方法，没什么好说的，这里值得注意的是，这种relativePose得到的位姿是第ｌ帧的，第ｌ帧的筛选是从第１帧开始到滑动窗口所有帧中一开始满足平均视差足够大的帧，这里的第ｌ帧会作为参考帧到下面的全局SFM使用。到这里就已经得到图像的特征点2d坐标的提取，相机第l帧和最后一帧之间的旋转和平移（注意暂时还没有得到特征的3d点坐标），有了这些信息就可以构建全局的SFM类GlobalSFM sfm，在这里调用sfm.construct(frame_count + 1, Q, T, l,relative_R, relative_T,sfm_f,sfm_tracked_points)，这里以第l帧作为参考帧，在进行PNP求解之前，需要判断当前帧数要大于第l帧，这保证了第l帧直接跳过PNP步骤，首先执行下面的第l帧和最后一帧的三角化，得到共视的特征点，供下面第l+1帧和最后一帧求解PNP，然后利用pnp求解l+1帧到最后一帧的位姿R_initial,P_initial，最后的位姿都保存在Pose中，一次循环，得到l+1，l+2…n-1帧的位姿。跳出步骤2 的循环后，至此得到了l+1，l+2…n-1帧的位姿以及l+1，l+2…帧与n-1帧的特征点三角化。然后再三角化l帧和i帧（在第l帧和最后一帧之间的帧）之间的3d坐标，（这里不明白为什么要做两次，是可以三角化出更多的特征点吗？？？？），接着PNP求解l-1，l-2…0帧和l帧之间的位姿已经三角化相应的特征点坐标，最后三角化其他所有的特征点。至此得到了滑动窗口中所有相机的位姿以及特征点的3d坐标。第6部就是进行BA优化，使用的是ceres优化位姿和特征点，这里可以参考视觉SLAM第十讲中的内容，优化方式相同。

         步骤3：接着使用PNP求解所有all_image_frame的相机位姿。这里所有的相机是指接收到所有的图像帧。所以一开始的时候要判断读到的图像帧是不是关键帧，是关键帧的话就不需要进行PNP求解（前面已经求解成功），直接将位姿转换到IMU坐标系下。如果是非关键帧的话，进行PNP求解，将位姿保存到all_image_frame中，至此，所有的位姿都已经保存到all_image_frame->ImageFrame中。

         步骤4：imu与视觉对齐，获取绝对尺度，在里面对Visual-Inertial Alignment，调用的方法是visualInitialAlign()。求解陀螺仪零偏metricscale，陀螺仪偏置和加速度，这里的metric scale指的是imu和sfm结果进行对齐需要的比例，内部调用solveGyroscopeBias(all_image_frame, Bgs)求解陀螺仪偏置，调用LinearAlignment(all_image_frame,g, x)求解加速度和尺度。首先介绍陀螺仪偏置的求解。这里构造超定方程的最小二乘解法AT*A*x=AT*b，这里A->tmp_A，b->tmp_b，得到delta_bg。得到了delta_bg之后，在进行预积分的计算的都更加准确的预积分值。然后进行重力加速度和尺度的计算，主要还是构造AT*A*x=AT*b问题，但值得注意的是这里的维度问题。在得到重力加速度的方向和值时，重新计算中两个I加速度的方向和尺度信息RefineGravity(all_image_frame, g, x)，值得注意的是这里最后要计算得到的是重力加速度的方向，其中具体的数值以及在参数中固定好了（9.81007），以及尺度数据。首先归一化前面计算的重力加速的的方向以及乘以重力的数值（9.81007），然后得到正切平面的lxly，有正切平面算出其对应的重力加速度dg，归一化前面计算的重力加速度和正切平面结合的重力加速度，乘以重力的数值，得到新的重力加速度g0，迭代5次，基本趋于平稳。（最后一部分不是很理解，暂时放着）

         在处理图像数据最后一步就是BA优化，调用solveOdometry()方法内部有BA优化的方法optimization()。在optimization()方法中，步骤1初始化ceres。步骤2加入待优化的参数项，首先为顶点添加pose、speed、加速度计和陀螺仪的偏置，这里涉及到过参数化的问题，因此引入LocalParameterization参数化来减小维度，我的理解是旋转四元数是4维的量，但是可以是用dsita的三维角度来表示，因此原来7维位姿可以使用6维的位姿来表示，而速度和偏置参数不需要参数化处理，同时添加外部提供的位姿参数，并设置其为常量这里的AddParameterBlock功能其实和AddResidualBlock的功能相似，都是构建最小二乘的优化问题。这时候调用vector2double();将前面计算得到的状态变量全部写入到参数para_Pose等中。步骤3 是加入误差项，这里是使用了自行推导解析形式，（一般都使用自动求导方式），依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法, 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式. 分别对应marginalization_factor.h，imu_factor.h,projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类。其实就是调用AddResidualBlock方法，marginal项和IMU没有使用到costfunction，而feature使用了costfunction。