VINS_Fusion 前端源码解析

VINS_estimator

VINS_estimator是VINS_Fusion的节点，其不包含回环检测部分，该节点可以单独对相机进行位姿估计。

在阅读源码之前，可以先了解一下VINS特征追踪策略:VINS_Fusion 特征追踪策略

文件树目录

guoben@guoben-WRT-WX9:~/Project/VIO/Source/VINS_VR/vins_estimator$ tree
.
├── cmake
│   └── FindEigen.cmake
├── CMakeLists.txt
├── launch
│   └── vins_rviz.launch
├── package.xml
└── src
    ├── estimator
    │   ├── estimator.cpp
    │   ├── estimator.h
    │   ├── feature_manager.cpp
    │   ├── feature_manager.h
    │   ├── parameters.cpp
    │   └── parameters.h
    ├── factor
    │   ├── imu_factor.h
    │   ├── initial_bias_factor.h
    │   ├── initial_pose_factor.h
    │   ├── integration_base.h
    │   ├── marginalization_factor.cpp
    │   ├── marginalization_factor.h
    │   ├── pose_local_parameterization.cpp
    │   ├── pose_local_parameterization.h
    │   ├── projection_factor.cpp
    │   ├── projection_factor.h
    │   ├── projectionOneFrameTwoCamFactor.cpp
    │   ├── projectionOneFrameTwoCamFactor.h
    │   ├── projectionTwoFrameOneCamFactor.cpp
    │   ├── projectionTwoFrameOneCamFactor.h
    │   ├── projectionTwoFrameTwoCamFactor.cpp
    │   └── projectionTwoFrameTwoCamFactor.h
    ├── featureTracker
    │   ├── feature_tracker.cpp
    │   └── feature_tracker.h
    ├── initial
    │   ├── initial_aligment.cpp
    │   ├── initial_alignment.h
    │   ├── initial_ex_rotation.cpp
    │   ├── initial_ex_rotation.h
    │   ├── initial_sfm.cpp
    │   ├── initial_sfm.h
    │   ├── solve_5pts.cpp
    │   └── solve_5pts.h
    ├── rosNodeTest.cpp
    └── utility
        ├── CameraPoseVisualization.cpp
        ├── CameraPoseVisualization.h
        ├── tic_toc.h
        ├── utility.cpp
        ├── utility.h
        ├── visualization.cpp
        └── visualization.h

8 directories, 44 files

rosNodeTest.cpp

rosNodeTest.cpp是vins_estimator节点的程序入口。主要实现以下函数。

主程序基本流程

1. 读取配置文件参数 readParameter()

2. 订阅了四个话题，分别是imu、双目相机图像以及feature_tracker所提供的跟踪光流点，收到各个话题的消息后执行回调函数，对各个数据进行相应的处理

3. 开启一个新线程sync_process。

   该线程的作用：若图像buffer里面有数据,读入数据并且添加到estimator中。利用图片携带的时间戳信息能够检测两图片是否同步，若两图片的时间戳差距在一定范围内，则添加到estimator中中，否则丢弃两帧图片。

主要成员

Estimator estimator; 						//位姿估计器
queue<sensor_msgs::ImuConstPtr> imu_buf; 	//IMU缓存buff
queue<sensor_msgs::PointCloudConstPtr> feature_buf; //特征点缓存buff
queue<sensor_msgs::ImageConstPtr> img0_buf; //图像缓存buff
queue<sensor_msgs::ImageConstPtr> img1_buf; //图像缓存buff
std::mutex m_buf; 							//互斥访问信号

主要函数

	函数	功能
1	void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)	存储左目图片信息
2	void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)	存储右目图片信息
3	cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)	从话题数据中提取图像信息，返回图像矩阵(拷贝)
4	void sync_process()	从具有相同时间戳的话题中提取图片
5	void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)	从IMU话题信息中提取数据保存到acc向量和gyr向量中，然后把{t,acc,gyr}保存到估计器中。
6	void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)	获取前端得到的点云数据，
7	void restart_callback(const std_msgs::BoolConstPtr &restart_msg)	如果有重启信号输入，则重新初始化进程；estimator.clearState();estimator.setParameter();
8	void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg)	用来切换状态：是否使用IMU数据
9	void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg)	用来切换图片信息是单目还是双目的

注意事项：

1. 超参数：双目图片之间的时间差设置在0.003s

estimator节点

VIO系统的整个程序从Estimator estimator开启。

1. estimator

estimator类的定义由estimator.h和estimator.cpp两个文件完成。

VINS_VR/vins_estimator/src/estimator$ tree.
├── estimator.cpp
├── estimator.h
├── feature_manager.cpp
├── feature_manager.h
├── parameters.cpp
└── parameters.h

主要成员

以下是该类中包含的几组成员

	类的主要成员	功能
1	queue<pair<double, Eigen::Vector3d>> accBuf; queue<pair<double, Eigen::Vector3d>> gyrBuf; queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf;	加速度计数据缓冲区； 陀螺仪数据缓冲区 特征点（7维）数据缓冲区
2	std::mutex mProcess; std::mutex mBuf; std::mutex mPropagate	进程互斥信号 缓存空间互斥信号 传播的互斥访问信号
3	double prevTime, curTime;	前一帧时间，当前帧时间
4	bool openExEstimation;
5	std::thread trackThread; std::thread processThread;	追踪线程 处理线程
6	FeatureTracker featureTracker;	特征追踪器 用来对原始图像进行畸变校正，特征点采集，光流跟踪 * vins-mono中该部分作为一个独立Node存在
7	SolverFlag solver_flag; MarginalizationFlag marginalization_flag;
8	Vector3d g;	重力加速度在各个方向上的分量； 最开始由参数设置函数确定，在初始化过程中还要进行后续优化
9	Matrix3d ric[2]; Vector3d tic[2];
10	Vector3d Ps[(WINDOW_SIZE + 1)]; Vector3d Vs[(WINDOW_SIZE + 1)]; Matrix3d Rs[(WINDOW_SIZE + 1)]; Vector3d Bas[(WINDOW_SIZE + 1)]; Vector3d Bgs[(WINDOW_SIZE + 1)];	滑动窗口中的数据； 位置、速度、旋转矩阵、加速度偏差、角速度偏差
11	double dt;	时间变化量
12	Matrix3d back_R0, last_R, last_R0; Vector3d back_P0, last_P, last_P0;
13	double Headers[(WINDOW_SIZE + 1)];	窗口内所有帧的时间戳
14	IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];	滑动窗口里边存放的imu预积分
15	Vector3d acc_0, gyr_0;	IMU的加速度，陀螺仪初始值
16	vector dt_buf[(WINDOW_SIZE + 1)]; vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; vector angular_velocity_buf[(WINDOW_SIZE + 1)];	滑动窗口 成员：dt（时间）、加速度数据、角速度数据
17	int frame_count; int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; int inputImageCnt;	窗口内第几帧，最大值位WINDOW+SIZE+1 外点个数，后点个数，前点个数，无效点个数 输入图片数目
18	FeatureManager f_manager	特征点管理器 用来对滑动窗口内所有特征点的管理。
19	MotionEstimator m_estimator;	运动估计器
20	InitialEXRotation initial_ex_rotation;	定义一个估计外部参数校准的对象
21	bool first_imu; bool is_valid, is_key; bool failure_occur;	该图像之后的第一个imu FIXME: 没有用到 检测是否发生了错误,在failureDetection中
22	vector point_cloud; vector margin_cloud;	点云 边缘云
23	vector key_poses; double initial_timestamp;	关键姿态 初始时间戳
24	double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; double para_Feature[NUM_OF_F][SIZE_FEATURE]; double para_Ex_Pose[2][SIZE_POSE]; double para_Retrive_Pose[SIZE_POSE]; double para_Td[1][1]; double para_Tr[1][1];	参数:11*7, 滑窗内帧的位姿 滑窗内速度偏差 外参 pose
25	int loop_window_index;
26	MarginalizationInfo *last_marginalization_info;	上一时刻的先验信息,也就是上一个H矩阵matg掉一部分后剩下的内容
27	vector<double *> last_marginalization_parameter_blocks;	最新的边缘化参数块
28	map<double, ImageFrame> all_image_frame;	图像帧的哈希表<时间戳，图像帧>；
29	IntegrationBase *tmp_pre_integration;	输入到图像中的预积分值
30	Eigen::Vector3d initP; Eigen::Matrix3d initR;	初始化的位置 初始化的姿态
31	double latest_time; Eigen::Vector3d latest_P, latest_V, latest_Ba, latest_Bg, latest_acc_0, latest_gyr_0;	最新的系统状态
32	bool initFirstPoseFlag; bool initThreadFlag;	IMU初始位姿的flag FIXME: 在哪里进行初始化的?线程是否进行初始化了

超参数

超参数	值
WINDOW_SIZE	10
初始的G	{0 0 ,9.8}

estimator类中包含两类成员函数：

类的必备函数

函数	功能
Estimator();	构造函数; 将initThreadFlag设为flase；然后将估计器状态清空（初始化）。
~Estimator();	析构函数 调用processsThread.join(),一直等待该线程死亡
void setParameter();	参数设置函数

1. 接口函数

包括以下8个函数：

	接口函数	功能
1	initFirstPose()	初始化初始位姿；
2	inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)	输入IMU数据 1.把IMU数据与时间戳一起输入到加速度缓冲区和角速度缓冲区中 2.如果solver_flag是非线性的，执行快速预测IMU，输出最新里程计数据；
3	inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)	输入双目图片数据到缓冲区;
4	inputFeature(double t, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &featureFrame)	输入特征到缓冲区;
5	ProcessIMU()	处理IMU数据，对IMU进行预积分； 如果是第一个IMU数据，则设置acc初始值、gyr初始值 如果帧数不是0的话，进行预计分计算，更新该帧的系统状态。
6	ProcessImage()	处理相机数据； 1. 基于特征点的视差来判断当前帧是否属于关键帧； 2. 判断相机到IMU的外参是否有校正，若无则用手眼标定法进行标定,具体在CalibrationExRotation里，此处只标定旋转矩阵，未标定平移矩阵，原因是系统对旋转矩阵较敏感，系统易因为小幅度的角度偏差而崩溃； 3. 判断是否有进行初始化;若已完成初始化，则调用optimization( )，用ceres_solver对滑窗进行非线性优化的求解，优化项主要有四项：边缘化残差、 imu残差、相机重投影残差以及相机与Imu间同步时间差的残差项。否则进行相应的初始化过程。 4. 本函数中包含一个failureDetection()函数,用于判断系统在一定条件下是否崩溃，比如非线性求解器中的解有大跳动，求解出相机IMU的外参矩阵或IMU偏移等等，系统挂掉就清空状态，重新初始化。
7	ProcessMeasurements()	处理测量值； 处理各buffer里的数据，当featureBuf不等于空时，开始进行以下处理（为什么是featureBuf，因为当有图像buffer数据的时候，才会有featuretracker.push(make_pair(t,featureFrame))，即有图像数据后，程序才发给跟踪器叫他产生feature，因此当featureBuf不等于空，所有的buffer，包括imu,图像，都不为空）：
8	void Estimator::changeSensorType(int use_imu, int use_stereo)	改变传感器类型，用于确定是否使用IMU，使用单目相机还是双目相机.

2. 内部函数

类的初始化函数 Estimator( ) ，由于Estimator类成员内部有两个比较重要的自定义类成员：
（1）Feature_Tracker featuretracker;
（2）FeatureManager f_manager;

简单设置了一些参数后，系统进入main()。

接着main()与Estimator estimator两者开始发生联系：
main()中estimator.setParameter()开启了滑动窗口估计的一个新线程
由于我们在配置文件中 多线程MULTIPLE_THREAD设置为1，因此当setParameter()时候，就开启了一个Estimator类内的新线程：processMeasurements();

pub VIO的各种话题，包括里程计信息，tf变换，相机姿态，点云信息，并且发布关键帧。

	内部函数	功能
1	void clearState()	清空估计器状态 主要需要做的操作有：清空buff缓冲区；设置时间、初始化位姿；把滑动窗口中的数据都清空（删除预积分指针）；清空tic、ric；特征管理器。
2	bool initialStructure()	初始化结构，SFM 利用SFM进行初始化； 视觉的结构初始化，首先得到纯视觉的,所有图像在IMU坐标系下的,一个初始化结果,也就是RT然后进行视觉imu对其,陀螺仪偏执估计等等 如果IMU的方差小于0.25，则输出IMU激励过小（返回失败）；
3	bool visualInitialAlign()	视觉初始化对齐 1. 求取scale尺度 2. 把所有图像帧都设置为关键帧 3.三角化
4	bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)	相对位姿; 查找到与最新帧中包含足够的对应点对和视差的关键帧
5	void slideWindow()	滑动窗口函数
6	void slideWindowNew();	滑到倒数第二帧,作用主要是删除特征点
7	void slideWindowOld();	滑掉最老的那一帧,,作用主要是删除特征点
8	void optimization()	后端优化函数 基于滑动窗口的紧耦合的非线性优化，残差项的构造和求解
9	void vector2double();	把滑动窗口向量转为double数据； 主要包括：1. 位置；2、姿态；3、IMU速度4、加速度偏差；5、角速度偏差；
10	void double2vector();	double转向量
11	bool failureDetection()	系统失败检测；发生以下情况则认定为失败 1. 检测数目<2; 2. IMU的加速度偏差>2.5； 3. IMU的角速度偏差大于1.0； 4. 最新帧与上一帧的平移量P的模>5； 5. 最新帧与上一帧z值变化>1; 6.
12	bool getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, vector<pair<double, Eigen::Vector3d>> &gyrVector)	对imu的时间进行判断，将队列里的imu数据放入到accVector和gyrVector中，完成之后返回true
13	void getPoseInWorldFrame ( Eigen::Matrix4d &T)； void getPoseInWorldFrame(int index, Eigen::Matrix4d &T)	获得世界坐标系下的位置
14
15	void predictPtsInNextFrame()
16	void outliersRejection(set &removeIndex)	外点移除函数
17	double reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici,	计算重投影误差； 输入：第i与第j帧之间的像素点位置，第i帧像素点的深度，相机模型转换矩阵；用head提取前两个元素以计算残差；
18	void updateLatestStates()	更新估计器的最新状态 主要是利用滑动窗口中的最近帧的数据来更新最新的time,P,Q,V,Ba,Bg,acc_0,gyr_0；tmp_accBuf,tmp_gyrBuf,acc,gyr;再次调用IMU的数据快速预测状态；
19	void fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)	快速预测IMU； 利用IMU数据更新Latest_P，Latest_V，Latest_acc_0,Latest_gyr_0;
20	bool IMUAvailable(double t)
21	void initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)

2. Initial初始化

文件树目录

├── initial_aligment.cpp
├── initial_alignment.h
├── initial_ex_rotation.cpp
├── initial_ex_rotation.h
├── initial_sfm.cpp
├── initial_sfm.h
├── solve_5pts.cpp
└── solve_5pts.h

初始化流程图

从上面的流程图中可以看出，初始化共分为两大步，第一是纯视觉初始化，第二是视觉惯性联合初始化。另外，如果没有提供相机和IMU之间的外参，VINS-Mono还提供了相应的标定程序。因此我们梳理代码就按照这三部分来梳理。

首先看文件结构，初始化流程相关的代码在vins_estimator/src/estimator.cpp中，具体讲就是initialStructure这个函数，它实现初始化所需要的所有既定步骤。其实就是下面这张图里的的功能和函数调用

文件	作用
initial_alignment.h	IMU与视觉信息的联合初始化
initial_ex_rotation.h	外参旋转初始化
initial_sfm.h	初始化SFM
solve_5pts.h	5点法求解

各文件中的函数

	函数	功能
1	void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs);
1	bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x);

3. FeatureTracker

主要作用跟踪特征并三角化恢复深度以后发给后端去优化。

FeatureTracker 里边存放的是用于特征跟踪的类

文件树目录

├── feature_tracker.cpp
└── feature_tracker.h

特征追踪流程图

类成员	功能
int row, col;	图片的宽、长；
cv::Mat imTrack;	当前追踪的图像；
cv::Mat mask;	用于标记点的图像；
cv::Mat fisheye_mask;	鱼眼mask；
cv::Mat prev_img, cur_img;	先前和现在的图像；双目中，特指左目
vector<cv::Point2f> n_pts;	从图片上返回的特征，shi-tomasi角点（Harris角点）
vector<cv::Point2f> predict_pts;
vector<cv::Point2f> predict_pts_debug;	etPrediction生成的，暂时不知道作用
vector<cv::Point2f> prev_pts, cur_pts, cur_right_pts;	cur_pts当前帧上的特征点，双目中的左目，并且应该像素坐标
vector<cv::Point2f> pts_velocity, right_pts_velocity;	像素移动速度
vector ids, ids_right;	ids这个好像就是当前帧特征点数目的索引
vector track_cnt;	保存了当前追踪到的角点一共被多少帧图像追踪到
map<int, cv::Point2f> cur_un_pts_map, prev_un_pts_map;	cur_un_pts_map中存放ids[i]和cur_un_pts[i]构成的键值对。
map<int, cv::Point2f> cur_un_right_pts_map, prev_un_right_pts_map;	当前右目上的点
map<int, cv::Point2f> prevLeftPtsMap;	上一帧的左目中的点
vector<camodocal::CameraPtr> m_camera;	相机类，双目的话有两个
double cur_time; double prev_time;	当前帧的时间； 上一帧的时间；
bool stereo_cam;	是否是双目相机
int n_id;
bool hasPrediction;

函数	功能
FeatureTracker();	构造函数
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat());	追踪图片 对图片进行了一系列操作,返回特征点featureFrame.执行的操作包括： 图像处理、区域mask、检测特征点、计算像素速度等
void setMask()	对跟踪点进行排序并依次选点，设置mask：去掉密集点，使特征点分布均匀。
void readIntrinsicParameter(const vector &calib_file);	读取内参。生成相机类m_camera
void showUndistortion(const string &name);
void rejectWithF();	使用F矩阵进行拒绝，删除一些点 涉及到FM_RANSAC, 拒绝的是一些光流跟踪错误的点, /TODO: 这里可能涉及到动态检测 首先将将图像坐标畸变矫正后转换为深度归一化坐标，通过cv::findFundamentalMat() 计算F矩阵，利用得到的status通过reduceVector()去除outliers。
void undistortedPoints();	将像素座标系下的座标，转换为归一化相机座标系下的座标 即un_pts为归一化相机座标系下的座标。
vector<cv::Point2f> undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam);	将像素座标系下的坐标，转换为归一化相机座标系下的坐标 即un_pts为归一化相机座标系下的座标。
vector<cv::Point2f> ptsVelocity(vector &ids, vector<cv::Point2f> &pts, map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts);	其为当前帧相对于前一帧 特征点沿x,y方向的像素移动速度
void showTwoImage(const cv::Mat &img1, const cv::Mat &img2, vector<cv::Point2f> pts1, vector<cv::Point2f> pts2);
void drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight, vector &curLeftIds, vector<cv::Point2f> &curLeftPts, vector<cv::Point2f> &curRightPts,map<int, cv::Point2f> &prevLeftPtsMap);	//在imTrack图像上画出特征点
void setPrediction(map<int, Eigen::Vector3d> &predictPts);	把上一帧3d点预测到归一化平面， 预测方法：：好像就是直接把3D点投影下来。 调用了四个函数m_camera[0]->spaceToPlane(itPredict->second, tmp_uv);
double distance(cv::Point2f &pt1, cv::Point2f &pt2);	计算两点之间的距离
void removeOutliers(set &removePtsIds);	移除外点，代码很容易懂
cv::Mat getTrackImage();	直接返回ImTrack，imTrack是在drawTrack函数中画出来的
bool inBorder(const cv::Point2f &pt);	返回是否在图像边界内

重要的函数

readImage( const cv::Mat &_img, //图像double _cur_time) //时间戳

它位于feature_tracker.cpp文件中，是整个特征跟踪功能的全部，其他函数主要是为了帮助他实现功能，是它的子函数

它的主要流程包括：

1）createCLAHE() 判断并对图像进行自适应直方图均衡化；

2）calcOpticalFlowPyrLK() 从cur_pts到forw_pts做LK金字塔光流法；

3）根据status，把跟踪失败的和位于图像边界外的点剔除，剔除时不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts、cur_pts，记录特征点id的ids，和记录特征点被跟踪次数的track_cnt中剔除；

4）setMask() 对跟踪点进行排序并依次选点，设置mask：去掉密集点，使特征点分布均匀

5）rejectWithF() 通过基本矩阵F剔除outliers

6）goodFeaturesToTrack() 寻找新的特征点(shi-tomasi角点)，添加(MAX_CNT - forw_pts.size())个点以确保每帧都有足够的特征点

7）addPoints()向forw_pts添加新的追踪点，id初始化-1，track_cnt初始化为1

8）undistortedPoints() 对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度

4. factor

├── imu_factor.h
├── initial_bias_factor.h
├── initial_pose_factor.h
├── integration_base.h
├── marginalization_factor.cpp
├── marginalization_factor.h
├── pose_local_parameterization.cpp
├── pose_local_parameterization.h
├── projection_factor.cpp
├── projection_factor.h
├── projectionOneFrameTwoCamFactor.cpp
├── projectionOneFrameTwoCamFactor.h
├── projectionTwoFrameOneCamFactor.cpp
├── projectionTwoFrameOneCamFactor.h
├── projectionTwoFrameTwoCamFactor.cpp
└── projectionTwoFrameTwoCamFactor.h

5. utility

文件树目录

guoben@guoben-WRT-WX9: ~/vins_estimator/src/utility$ tree
.
├── CameraPoseVisualization.cpp
├── CameraPoseVisualization.h
├── tic_toc.h
├── utility.cpp
├── utility.h
├── visualization.cpp
└── visualization.h

0 directories, 7 files

文件	功能
CameraPoseVisualization.h	包含了相机位资可视化函数
TicToc.h	该库主要用于计算时间
utility.h	重新定义了Eigen里面四元数的一些计算函数
visualization.h	把各种数据发送到ROS系统中以进行可视化