VINS-Mono代码阅读
一、feature_tracker
1.首先初始化feature_tracker节点，函数readParameters(n)加载config参数配置文件。trackerData[i].readIntrinsicParameter(CAM_NAMES[i])给相机模型加载内参，这里是单目i=1；如果是鱼眼相机则加载mask。然后开始订阅相机的topic，调用回调函数读取图片 [1.1]，然后初始化3个需要发布的topic（特征点feature、图像feature_img、重启restart）。
回调函数img_callback：
1.1若是第一张图片则获取时间戳直接返回。后续判断两帧图像时间间距太大或者顺序不对则重启系统，重新读取第一张图片。否则获取当前图像时间戳，判断当前的发布频率是否小于设定的上线FREQ，若小于则发布当前图像PUB_THIS_FRAME 为true否则为false。把收到的图像转化为灰度图，trackerData[i].readImage读取图像[1.1.1],进行光流跟踪和特征补充。得到当前帧的所有特征点之后，目前未编号的特征点的ids都是-1，下一步对未编号的特征按顺序递增进行编号，其中程序中n_id的值表示从程序开始到现在编号的所有的特征数。当前帧未编号的特征编完后返回true则与completed标志位进行抑或取1，则break for循环完成编号(程序node103行）。然后判断该帧是否需要发布，若发布则发布数加1，定义需要发布的变量（都是sensor_msgs中自带的消息类型），然后把所有空间点归一化坐标赋值给feature_points->points，特征点id赋值给id_of_point.values，像素坐标赋值给u_of_point.values，v_of_point.values，特征点速度赋值给velocity_x_of_point.values，velocity_y_of_point.values。最后合并到feature_points->channels，一起发布出去。接下来就是把特征点画到图像上，而且根据跟踪次数不同显示不同的颜色，最后发布出去pub_match.publish(ptr->toImageMsg())。
1.1.1若图像太亮或太暗，使用限制对比度的直方图均衡。首先第一张图像通过cv::goodFeaturesToTrack（）函数进行提取特征点。随后通过光流金字塔calcOpticalFlowPyrLK进行特征点到下一帧的跟踪并剔除不好的跟踪点和边缘点。随后判断是否发布这一帧if (PUB_THIS_FRAME)，若发布的话进一步去除外点rejectWithF()，然后setMask()函数起到一个特征点均匀化的作用[1.1.1.1]，然后判断n_max_cnt决定是否还需要增加特征点，若需要补充的话就调用函数cv::goodFeaturesToTrack（）在mask剩余的空间中进行提取，其中n_pts存放的是新补充的特征点，然后addPoints()函数把新增的特征点加入到forw_pts中，然后把forw_pts赋给了cur_pts。最后特征点像素坐标通过相机内参投影到归一化平面，其中cur_un_pts保存的是当前帧特征点的归一化坐标，并计算当前帧跟踪次数大于等于1的特征点的归一化坐标的速度存入变量pts_velocity中。
1.1.1.1FeatureTracker::setMask()函数。首先判断是否为鱼眼相机，若是则加载鱼眼相机mask（一般是个和图像大小一样并内部填充为白色的圆形），否则加载的mask设置为与图像大小一样设为全白。cnt_pts_id变量用于把特征点forw_pt和对应的跟踪次数track_cnt放在一起，然后后面根据跟踪次数从大到小进行排序，并把forw_pt存储的特征点清除用于后续重新存储特征点，排完序之后再根据设定的特征最小间距进行筛选，筛选策略是先把跟踪次数最多的那个特征点放入forw_pt中，然后把mask以该特征点为中心半径为MIN_DIST的区域设成实心的黑色圆，以便于该特征中心半径为MIN_DIST的范围内的特征点直接筛选掉。然后不断进行提取重新放入到forw_pts中。
二、vins_estimator
2.首先初始化节点名vins_estimator，加载config参数（IMU内参，相机与IMU外参等），然后estimator.setParameter()把外参旋转、平移以及投影焦距和曝光时间参数td（暂不考虑）传给估计器estimator。接着registerPub（n）初始化需要发布的topic包括imu_propagate、path、relocalization_path、odometry、point_cloud、key_poses、camera_pose、camera_pose_visual、keyframe_pose、keyframe_point、extrinsic、relo_relative_pose，初始化相机位姿可视化以及关键帧可视化变量cameraposevisual、keyframebasevisual。订阅四个topic，分别是获取IMU数据[2.1]、获取图像特征点观测信息[2.2]、获取是否重启系统指令[2.3]、获取重定位时的匹配点[2.4]。最后开启主线程process[2.5]。
2.1imu_callback回调函数：首先消息进来后判断时间顺序是否正确，若不正确直接return，否则得到当前时间戳，锁住当前线程，获取IMU数据到imu_buf中，获取完毕后解锁线程并唤醒process线程。然后是predict(imu_msg)函数通过下面的中值积分法求解当前时刻的p,v,q。
2.2feature_callback回调函数：去掉第一波的特征点因为不包含光流速度信息，通过线程锁操作把特征信息传给feature_buf变量，唤醒process。
2.3restart_callback回调函数：收到重启系统指令后，把之前feature_buf和imu_buf里面的值清空，然后使用线程锁把估计器的所有状态重置estimator.clearState()，并重新加载外参、焦距和tdestimator.setParameter()。
2.4relocalization_callback回调函数：若收到重定位信号后，把把从/pose_graph/match_points话题中获取的匹配点存到relo_buf变量中。
2.5process主线程：首先定义观测变量measurements，线程锁等待获取观测值measurements = getMeasurements()首先保证存在IMU数据和图像特征数据，在回调函数中都有线程锁，当每次有数据过来后都会去尝试唤醒process线程，只有当图像数据和IMU数据都存在且最后一个IMU数据的时间戳大于图像数据的时间戳之后才会把当前帧的图像数据以及与上一帧之间的IMU数据组成一对儿存放在measurements中，即进行图像特征数据和IMU数据的对齐操作。这里使用的是队列数据结构（先进先出front是先进的数据，back是后进的数据）。满足数据对齐就可以数据从队列中按对齐的方式取出来。这里直到把缓存中满足条件对齐条件的图像特征数据及MU数据取完，才能够跳出此函数，返回数据。获取完数据后process线程中的con.wait()函数得到唤醒。接着立马再开启个线程锁m_estimator.lock()，首先一个大的for循环遍历前面处理的观测值（如果取得比较慢估计维数会比较大，如果后续的位姿估计频率大于10HZ的话估计维数一直可以保持为1），然后就是遍历观测值measurements，对成对儿的对齐后的IMU信息通过estimator.processIMU（）进行预积分，由于每一对数据中包含一个IMU信息的时间戳大于img的因此程序中也考虑到这种情况下把IMU数据也积分进去。具体数学公式如下所示。
2.5.1重定位：此处后续再进行补充。
2.5.2图像观测数据处理：首先通过函数f_manager.addFeatureCheckParallax（）首先,迭代 image 即当前帧检测到的每个路标点 ,判断之前的 feature 队列中是否包含,若不含,则添加到 feature 队列中;若包含,则添加到对应 id 的 FeaturePerFrame 队列。然后,compensatedParallax2 计算每个路标点在两幅图中的视差量,若视差量大于某个阈值或者当前帧跟踪的路标点小于某个阈值,则边缘化滑窗内最老的帧;否则,边缘化掉次新帧,即倒数第二帧。接着再回到processimage（）函数，将图像数据和时间戳存到imageframe中（ImageFrame对象中包含特征点，时间，位姿R，t，预积分对象pre_integration，是否是关键帧）,同时将相邻帧的预积分值保存到此对象中（这里的预积分值就是前面processIMU（）时计算的），然后将当前帧的对象imageframe保存到all_image_frame对象中（imageframe的容器）。然后判断外参估计标志ESTIMATE_EXTRINSIC是否为2，是的话就需要根据前后帧估计外参，然后标志ESTIMATE_EXTRINSIC设为1，表示后续继续优化初始估计的参数。接下来就是需要进行初始化操作，初始化的时候需要判断frame_count是否达到最大窗口大小WINDOW_SIZE,确保有足够的frame参与初始化,有外部参数同时时间间隔大于0.1秒，就进行初始化操作调用函数initialStructure()[2.5.2.1]。如果初始化成功就调用函数solveOdometry()[2.5.2.2],slideWindow()[2.5.2.3],removeFailures()[2.5.2.4]。如果初始化已经完成那么solveOdometry(),failureDetection(),clearState(),setParameter();
2.5.2.1系统初始化：首先遍历滑窗内的所有帧，根据相邻帧间的线速度变化量求解帧间平均加速度，然后求解窗口内加速度的标准差，判断是否大于0.25，保证IMU充分运动。然后创建SFMFeature类的sfm_f对象，遍历feature将所有feature的id和被所有帧的观测打包给sfm_f对象用于后续的SFM初始化过程。然后调用relativatePose函数求解与当前帧（也就是窗口内的最后一帧）匹配点对较多且平均视差满足一定预值的帧作为参考帧，并求出相对Ｒ，ｔ。然后construct函数求解之间参考帧与当前帧之间点进行三角化triangulateTwoFrames；然后有了3D点之后利用solveFrameByPnP求解参考帧与当前帧之间的帧的姿态，求解姿态之后再三角化中间帧与当前帧共视且未被三角化的特征点；然后再把中间帧与参考帧共视且未被三角化的点进行三角化；接着继续pnp求解第0帧与参考帧之间帧的姿态，并与参考帧进行三角化；最后再遍历所有特征点，把那些至少被2帧观测到但还未被三角化的进行三角化。然后利用ceres优化库对初始化窗口内的所有特征点和相机位姿进行优化，优化后的姿态存放在Q，T中。然后再利用PnP算法得到更加精确的关键帧位姿。接着调用visualInitialAlign（)[2.5.2.1.1]函数进行视觉和IMU的对齐操作。
2.5.2.1.1视觉惯导数据对齐：首先调用函数VisualIMUAlignment（）进行计算陀螺仪偏置solveGyroscopeBias（） [2.5.2.1.1.1]，计算尺度，重力加速度和速度LinearAlignment（）[2.5.2.1.1.2]。然后f_manager.triangulate（）函数，根据加速度bias重新对关键帧间的IMU数据进行预积分repropagate（）。最后得到具有真实尺度的状态量Ps,Rs,Vs。这一部分代码主要根据下面的理论推到相对应的（图片内容来源崔华坤大佬的VINS论文推导及代码解析）
2.5.2.2solveOdometry()：
(1) f_manager.triangulate(Ps, tic, ric);在后续的跟踪过程中对新加入的feature进行三角化，得到深度值。（**注意：**这里三角化主要是求解特征点在第一次观测帧坐标系下的逆深度，后续在构建视觉投影误差时会用到。和论文中讲的对应上了，之前看论文还一直不理解为啥要定义特征被第一次观测时的深度作为逆深度呢）
(2) optimization()优化部分
VINS-Mono在优化部分包括三项误差,分别是先验误差和IMU误差和视觉误差。其中后两者的误差项的设置相比边缘化误差理解起来简单的多（但是也看了很多天），下面着重讲解边缘化先验误差。
首先看边缘化的过程：看边缘化老的帧的情况代码如下
if (last_marginalization_info)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
drop_set.push_back(i);
}
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
如果上一次边缘化过（即存在上次的边缘化信息），需要把上次的边缘化参差也加入到本次的边缘化参差中，本次的边缘化还需要在上次边缘化后留下的参数块儿中找到本次边缘化中需要边缘化的参数块。其中上面的代码中drop_set保存的就是需要边缘化掉的参数在上次留下的参数块中的索引。可以看出VINS在本次去边缘化上次留下的先验信息时，并没有考虑特征点，只是边缘化了最老帧的pose和速度、加速度bias、角速度bias。是因为滑窗中其他特征点和先验误差是没关系的，每次边缘化都会把最老帧所主导的特征点都给边缘化掉了。
下面程序是添加滑窗内第0帧（也就是需要边缘化掉的最老帧）和第一帧之间的IMU预积分参差，和这个参差相关联的参数块分别是para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]，大小分别是6969，vector{0, 1}表示需要边缘化第0个和第1个参数块儿。
if (pre_integrations[1]->sum_dt < 10.0)
{
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
vector<int>{0, 1});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
下面程序是遍历第0帧看到的所有特征点，然后把在滑窗内其他帧中看到后产生的重投影误差添加进去。比如第一个特征点被其他10个帧都看到了那么addResidualBlockInfo就要被调用10次，如果有10个点都被其他10个帧看到，那么就要调用100次。其中每一个参差对应的参数块是para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]（不考虑Td），vector{0, 3}表示需要边缘化掉的是para_Pose[imu_i]和para_Feature[feature_index]。
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
if (imu_i != 0) //只选取老帧看到的点
continue;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
continue;
Vector3d pts_j = it_per_frame.point;
if (ESTIMATE_TD)
{
ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
else
{
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
2.5.2.2.1 marginalization_info->addResidualBlockInfo()函数
void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
factors.emplace_back(residual_block_info);
std::vector<double *> ¶meter_blocks = residual_block_info->parameter_blocks;
std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();
for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
{
double *addr = parameter_blocks[i];
int size = parameter_block_sizes[i];
parameter_block_size[reinterpret_cast<long>(addr)] = size;
}
for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
{
double *addr = parameter_blocks[residual_block_info->drop_set[i]];
parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
}
}
residual_block_info->parameter_blocks就是每次添加参差时对应的参数块儿，比如重投影误差的话就是para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]。对应的residual_block_info->cost_function->parameter_block_sizes()就是6，6，6，1。
然后把参数块数组的地址当做map类型变量parameter_block_size的键值索引，并对应的值是parameter_block_sizes的值。parameter_block_idx保存的是需要被边缘化掉的参数块的地址和对应的值都初始化为0，图示如下：
而parameter_block_idx对应的图示：
2.5.2.2.2marginalization_info->preMarginalize()函数
void MarginalizationInfo::preMarginalize()
{
for (auto it : factors)
{
it->Evaluate();
std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();
for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
{
long addr = reinterpret_cast<long>(it->parameter_blocks[i]);
int size = block_sizes[i];
if (parameter_block_data.find(addr) == parameter_block_data.end())
{
double *data = new double[size];
memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
parameter_block_data[addr] = data;
}
}
}
}
就是遍历每一个参差因子，调用Evaluate函数计算各自的参差和雅克比矩阵。然后把各自参数块的值放在一起存在parameter_block_data变量中，而且都带有各自的地址作为键值索引。
2.5.2.2.3marginalization_info->marginalize()函数
int pos = 0;
for (auto &it : parameter_block_idx)
{
it.second = pos;
pos += localSize(parameter_block_size[it.first]);
}
上面代码干的事儿就是把parameter_block_idx改成下面所示的样子
for (const auto &it : parameter_block_size)
{
if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
{
parameter_block_idx[it.first] = pos;
pos += localSize(it.second);
}
}
上面的代码把parameter_block_size中其他不需要边缘化的也加到parameter_block_idx中，此时pose表示整体的大小，m表示需要边缘化掉的变量的大小，n表示剩下的大小。
接下来的操作是多线程计整个先验项的参数块,雅可比矩阵和残差值
然后在下关键帧来的时候会先添加这一次边缘化后产生的参差，代码如下：
if (last_marginalization_info)
{
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks); //边缘化后的先验误差
}
其中边缘化参差因子构建如下代码所示
bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
//printf("internal addr,%d, %d\n", (int)parameter_block_sizes().size(), num_residuals());
//for (int i = 0; i < static_cast(keep_block_size.size()); i++)
//{
// //printf("unsigned %x\n", reinterpret_cast(parameters[i]));
// //printf("signed %x\n", reinterpret_cast(parameters[i]));
//printf("jacobian %x\n", reinterpret_cast(jacobians));
//printf("residual %x\n", reinterpret_cast(residuals));
//}
int n = marginalization_info->n;
int m = marginalization_info->m;
Eigen::VectorXd dx(n);
for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
{
int size = marginalization_info->keep_block_size[i];
int idx = marginalization_info->keep_block_idx[i] - m;
Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
if (size != 7)
dx.segment(idx, size) = x - x0;
else
{
dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))
{
dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
}
}
}
Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;
if (jacobians)
{
for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
{
if (jacobians[i])
{
int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);
int idx = marginalization_info->keep_block_idx[i] - m;
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);
jacobian.setZero();
jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);
}
}
}
return true;
}
2.5.2.３slideWindow()：滑动窗口更新。