基本上VINS里面绝大部分功能都在这个package下面，包括IMU数据的处理(前端)，初始化(我觉得可能属于是前端)，滑动窗口(后端)，非线性优化(后端)，关键帧的选取(部分内容)(前端)。我第一次看的时候，总是抱有一个疑问，就是为什么把这么多内容全都放在这一个node里面。为了回答这个问题，那么首先先搞清楚vins_estimator里面分别具体都是什么，为什么要有这些数据结构/函数，这些函数是怎样工作的。
这个package下面主要以下文件：
factor——主要用于非线性优化对各个参数块和残差块的定义，VINS采用的是ceres，所以这部分需要对一些状态量和因子进行继承和重写。
initial——主要用于初始化，VINS采用的初始化策略是先SfM进行视觉初始化，再与IMU进行松耦合。
estimator.cpp——vins_estimator需要的所有函数都放在这里，是一个鸿篇巨制。
estimator_node.cpp——vins_estimator的入口，是一个ROS的node，实际上运行的是这个cpp文件。
feature_manager.cpp——负责管理滑窗内的所有特征点。
parameters.cpp——读取参数，是一个辅助函数。
utility——里面放着用于可视化的函数和tictok计时器。
然后就是要注意CmakeLists.txt和package.xml文件的写法，这两个文件也是相当于套公式，写错了就不能实现正常的ROS功能了。
【SLAM】VINS-MONO解析——综述
【SLAM】VINS-MONO解析——feature_tracker
【SLAM】VINS-MONO解析——IMU预积分
【SLAM】VINS-MONO解析——vins_estimator
【SLAM】VINS-MONO解析——初始化(理论部分)
【SLAM】VINS-MONO解析——初始化(代码部分)
【SLAM】VINS-MONO解析——后端优化(理论部分)
【SLAM】VINS-MONO解析——后端优化(代码部分)
【SLAM】VINS-MONO解析——sliding window
【SLAM】VINS-MONO解析——回环检测
【SLAM】VINS-Fusion解析——流程
【SLAM】VINS-MONO解析——对vins-mono的修改使流程逻辑更清晰
【SLAM】VINS-MONO解析——基于vins-mono的slam系统开发
5.1.1 main()主入口
1、ROS初始化、设置句柄
ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
2、读取参数，设置状态估计器参数
readParameters(n);
estimator.setParameter();
这个estimator.setParameter()需要注意一下，它在estimator.cpp里面：
void Estimator::setParameter()
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
tic[i] = TIC[i];
ric[i] = RIC[i];
}
f_manager.setRic(ric);
ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
td = TD;
}
它读取了每一个相机到IMU坐标系的旋转/平移外参数和非线性优化的重投影误差部分的信息矩阵。
3、发布用于RVIZ显示的Topic，本模块具体发布的内容详见输入输出
registerPub(n);
这个函数定义在utility/visualization.cpp里面：void registerPub(ros::NodeHandle &n)。
4、订阅IMU_TOPIC，执行imu_callback
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
这个回调函数比较重要，如下代码所示：
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
if (imu_msg->header.stamp.toSec() <= last_imu_t)
{
ROS_WARN("imu message in disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();
m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one();
last_imu_t = imu_msg->header.stamp.toSec();
{
std::lock_guard<std::mutex> lg(m_state);
predict(imu_msg);
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
}
}
imu_callback主要干了3件事，
第一件事就是往imu_buf里放IMU数据，缓存起来；
第二件事就是IMU预积分获得当前时刻的PVQ，见4.1.2；
第三件事就是如果当前处于非线性优化阶段的话，需要把第二件事计算得到的PVQ发布到rviz里去，见utility/visualization.cpp的pubLatestOdometry()函数。
在这个函数里，第一次出现了上锁操作，因为这个程序是多线程，需要考虑线程安全问题，对imu_buf的操作是一个生产者-消费者模型，加入和读取的时候不可以中断，必须加锁以保证数据安全，后续部分有多个地方都有上锁操作。
这部分出现了几个新的数据结构，解释如下：
数据结构:
tmp_Q，tmp_P，tmp_V：当前时刻的PVQ
header：当前时刻时间戳
5、订阅/feature_tracker/feature，执行feature_callback
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息，见3.3.6.。
feature_callback就只干了一件事，就是把cur帧的所有特征点放到feature_buf里，同样需要上锁。注意，cur帧的所有特征点都是整合在一个数据里的，也就是sensor_msgs::PointCloudConstPtr &feature_msg。
6、订阅/feature_tracker/restart，执行restart_callback
ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
restart_callback干了一件事，就是把所有状态量归零，把buf里的数据全部清空。
7、订阅/pose_graph/match_points，执行relocalization_callback
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
8、创建VIO主线程process()(VINS核心！)
std::thread measurement_process{process};
这一部分是最重要的，包含了VINS绝大部分内容和最难的内容。
5.1.2 process()主线程
1、对imu和图像数据进行对齐并配对
这一部分卡了我很久很久，终于最近彻底弄明白了。
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]{return (measurements = getMeasurements()).size() != 0; });
lk.unlock();
一上来就是一个非常复杂的数据结构measurements！在这里先解释一波。
数据结构: measurements
1、首先，measurements他自己就是一个vector；
2、对于measurements中的每一个measurement，又由2部分组成；
3、第一部分，由sensor_msgs::ImuConstPtr组成的vector；
4、第二部分，一个sensor_msgs::PointCloudConstPtr；
5、这两个sensor_msgs见3.1-6部分介绍。
6、为什么要这样配对(一个PointCloudConstPtr配上若干个ImuConstPtr)？
因为IMU的频率比视觉帧的发布频率要高，所以说在这里，需要把一个视觉帧和之前的一串IMU帧的数据配对起来。
容器measurements有了，接下来就是配对IMU和视觉帧的数据，放到容器里去。配对过程也需要上锁，而且是一个条件锁。作者在这里用了一个lambda表达式，也就是说，在return里面的部分是false时，保持上锁状态，继续配对数据；如果return里面时true，说明配对完成，释放锁，measurements完成了，以供后续使用。
接下来分析一下measurements()具体的功能：
//这个函数的作用就是 把图像帧 和 对应的IMU数据们 配对起来,而且IMU数据时间是在图像帧的前面
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
while (true)
{ //边界判断：数据取完了，说明配对完成
if (imu_buf.empty() || feature_buf.empty())
return measurements;
//边界判断：IMU buf里面所有数据的时间戳都比img buf第一个帧时间戳要早，说明缺乏IMU数据，需要等待IMU数据
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
{
sum_of_wait++;//统计等待的次数
return measurements;
}
//边界判断：IMU第一个数据的时间要大于第一个图像特征数据的时间(说明图像帧有多的)
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
{
feature_buf.pop();
continue;
}
//核心操作：装入视觉帧信息
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
feature_buf.pop();
//核心操作：转入IMU信息
std::vector<sensor_msgs::ImuConstPtr> IMUs;
while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
{
IMUs.emplace_back(imu_buf.front());
imu_buf.pop();
}
//注意：把最后一个IMU帧又放回到imu_buf里去了
//原因：最后一帧IMU信息是被相邻2个视觉帧共享的
IMUs.emplace_back(imu_buf.front());
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
2、对IMU数据的处理
m_estimator.lock();
for (auto &measurement : measurements)//2、对measurements中的每一个measurement (IMUs,IMG)组合进行操作
{//2.1、对于measurement中的每一个imu_msg，计算dt并执行processIMU()
auto img_msg = measurement.second;
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec();
double img_t = img_msg->header.stamp.toSec() + estimator.td;//相机和IMU同步校准得到的时间差
//对于大多数情况，IMU的时间戳都会比img的早，此时直接选取IMU的数据就行
if (t <= img_t) //http://wiki.ros.org/sensor_msgs
{
if (current_time < 0)
current_time = t;
double dt = t - current_time;
ROS_ASSERT(dt >= 0);
current_time = t;
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
//这里干了2件事，IMU粗略地预积分，然后把值传给一个新建的IntegrationBase对象
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//进行IMU预积分
}
//对于处于边界位置的IMU数据，是被相邻两帧共享的，而且对前一帧的影响会大一些，在这里，对数据线性分配
else//每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用的(出现次数少)
{
double dt_1 = img_t - current_time; //current_time < img_time < t
double dt_2 = t - img_t;
current_time = img_t;
ROS_ASSERT(dt_1 >= 0);
ROS_ASSERT(dt_2 >= 0);
ROS_ASSERT(dt_1 + dt_2 > 0);
//以下操作其实就是简单的线性分配
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
}
这个if-else的操作技巧非常值得学习！这一部分的核心代码是processIMU()，它在estomator.cpp里面，它的作用就是IMU预积分，对应着4.1.2的内容。
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
/*这一段作用就是就是给以下数据提供初始值/初始化：
*
*pre_integrations[frame_count]
*dt_buf[frame_count]
*linear_acceleration_buf[frame_count]
*angular_velocity_buf[frame_count]
*Rs[frame_count]
*PS[frame_count]
*Vs[frame_count]
*
* TODO 关于frame_count的更新，目前只在process_img里的solver_flag == INITIAL这里看到?
*
*/
//边界判断：如果当前帧不是第一帧IMU，那么就把它看成第一个IMU，而且把他的值取出来作为初始值
if (!first_imu)//force to be first_IMU
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
//边界判断：如果当前IMU帧没有构造IntegrationBase，那就构造一个，后续会用上
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
//核心操作
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
int j = frame_count;
//注意啊，这块对j的操作看似反智，是因为j时刻的值都拷贝了j-1时刻的值！！
//第一次使用实际上就是使用的是j-1时刻的值，所以在这些地方写上j-1是没有关系的！
//noise是zero mean Gauss，在这里忽略了
//TODO 把j改成j-1，看看效果是一样
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//world
//下面都采用的是中值积分的传播方式，noise被忽略了
//TODO 把j改成j-1，看看效果是一样
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
//PPT 1-27
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
}
//数据传递
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
关于IMU预积分部分，见4.1.2。这里出现了几个新的数据结构，分析如下：
数据结构:
1、Rs[frame_count]，Ps[frame_count]，Vs[frame_count]:是从IMU系转到world系的PVQ，数据是由IMU预积分得到的，目前在这里存放的是没有用bias修正过的值。
2、frame_count:这个值让我很疑惑，它只在processImage()里有过++操作，而且在estimator.hpp声明的时候，没有加上static关键字。它是在h文件中声明，在cpp文件里初始化的，后续需要再关注一下。
3、dt_buf，linear_acceleration_buf，angular_velocity_buf：帧数和IMU测量值的缓存，而且它们是对齐的。
3、pre_integrations[frame_count]，它是IntegrationBase的一个实例，在factor/integration_base.h中定义，它保存着frame_count帧中所有跟IMU预积分相关的量，包括F矩阵，Q矩阵，J矩阵等。
说实话，tmp_pre_integration这个数据结构我感觉很莫名其妙，明明有pre_integrations了，还要他干啥？而且processIMU()是在processImage()前面执行的，而tmp_pre_integration是在processImage()中才创建新对象的，所以对于第一帧，tmp_pre_integration的push_back操作是对谁的操作？？有没有大佬解释一下。
3、重定位/回环检测操作
回到estimator_node.cpp的process()函数。接下来的代码的作用是，在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
// set relocalization frame
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
//2.2、在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
while (!relo_buf.empty())
{
relo_msg = relo_buf.front();
relo_buf.pop();
}
if (relo_msg != NULL)
{
vector<Vector3d> match_points;
double frame_stamp = relo_msg->header.stamp.toSec();
for (unsigned int i = 0; i < relo_msg->points.size(); i++)
{
Vector3d u_v_id;
u_v_id.x() = relo_msg->points[i].x;
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
Matrix3d relo_r = relo_q.toRotationMatrix();
int frame_index;
frame_index = relo_msg->channels[0].values[7];
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);//设置重定位帧
}
4、对img信息进行处理(核心！)
(1)一开始，就定义了一个新的数据结构，解释一下：
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
数据结构: map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image
1、虽然它叫image，但是这个容器里面存放的信息是每一个特征点的！
2、索引值是feature_id；
3、value值是一个vector，如果系统是多目的，那么同一个特征点在不同摄像头下会有不同的观测信息，那么这个vector，就是存储着某个特征点在所有摄像头上的信息。对于VINS-mono来说，value它不是vector，仅仅是一个pair，其实是可以的。
4、接下来看这个vector里面的每一pair。int对应的是camera_id，告诉我们这些数据是当前特征点在哪个摄像头上获得的。
5、Matrix<double, 7, 1>是一个7维向量，依次存放着当前feature_id的特征点在camera_id的相机中的归一化坐标，像素坐标和像素运动速度，这些信息都是在feature_tracker_node.cpp中获得的。
(2)这个数据结构定义完之后，接下来就是往这个容器中放数据。
//遍历img_msg里面的每一个特征点的归一化坐标
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
//把img的信息提取出来放在image容器里去，通过这里，可以理解img信息里面装的都是些什么
int v = img_msg->channels[0].values[i] + 0.5; //channels[0].values[i]==id_of_point
//hash
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
double x = img_msg->points[i].x;
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
double p_u = img_msg->channels[1].values[i];
double p_v = img_msg->channels[2].values[i];
double velocity_x = img_msg->channels[3].values[i];
double velocity_y = img_msg->channels[4].values[i];
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
(3) 处理图像processImage() (核心！)
这一部分的内容非常多，将在5.2部分一层层剥开讲解！这里实现了视觉与IMU的初始化以及非线性优化的紧耦合。
estimator.processImage(image, img_msg->header);//处理图像帧：初始化，紧耦合的非线性优化
5、可视化
向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系，这些函数都定义在中utility/visualization.cpp里，都是ROS相关代码。
pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
pubRelocalization(estimator);
6、IMU的PVQ信息更新
更新IMU参数[P,Q,V,ba,bg,a,g]，需要上锁，注意线程安全。这里对应的知识点是4.1.1最后一个公式。
m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
update();
m_state.unlock();
m_buf.unlock();
update()的内容如下：
void update()//这个函数在非线性优化时才会在process()中被调用
{//1、从估计器中得到滑动窗口中最后一个图像帧的imu更新项[P,Q,V,ba,bg,a,g]
TicToc t_predict;
latest_time = current_time;
tmp_P = estimator.Ps[WINDOW_SIZE];
tmp_Q = estimator.Rs[WINDOW_SIZE];
tmp_V = estimator.Vs[WINDOW_SIZE];
tmp_Ba = estimator.Bas[WINDOW_SIZE];
tmp_Bg = estimator.Bgs[WINDOW_SIZE];
acc_0 = estimator.acc_0;
gyr_0 = estimator.gyr_0;
//2、对imu_buf中剩余的imu_msg进行PVQ递推（因为imu的频率比图像频率要高很多，在getMeasurements(）将图像和imu时间对齐后，imu_buf中还会存在imu数据）
queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
predict(tmp_imu_buf.front());
}
这里有一个问题，就是为什么要update()？
因为在进行完一个process()循环后，当前的PVQ的状态和循环开始的状态是不一样的。所以说我们需要再根据当前的数据，更新当前的PVQ状态，也就是tmp_X。同样，得上锁。
这个函数在estimator.cpp里。先看函数名，它传入的参数分别是当前帧上的所有特征点和当前帧的时间戳。
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
1、关键帧判断
VINS滑动窗口采取的是这样的策略，它判断当前帧是不是关键帧，如果是关键帧，滑窗的时候marg掉最老帧；如果不是关键帧，则marg掉上一帧。
//true：上一帧是关键帧，marg_old; false:上一帧不是关键帧marg_second_new
//TODO frame_count指的是次新帧还是最新帧？
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
marginalization_flag = MARGIN_OLD;
else
marginalization_flag = MARGIN_SECOND_NEW;
这里出现了一个新的数据结构f_manager和一个新的函数addFeatureCheckParallax()。
数据结构: f_manager
f_manager是FeatureManager的一个对象。它定义在utility/feature_manager.h里。这个h文件里定义了3个类，借用崔神的一个神图来表示如下：
f_manager可以看作为一个存放着滑窗内所有特征点信息的容器，其中最关键的部分是list<FeaturePerId> feature。其中每一个特征点，可以看作是一个FeaturePerId的对象，它存放着一个特征点在滑窗中的所有信息，其中最关键的部分是vector<FeaturePerFrame> feature_per_frame。其中一个特征点在一个帧中的信息，可以看作是一个FeaturePerFrame的对象，它存放着一个特征点在滑窗里一个帧里面的信息，包括归一化坐标，像素坐标，像素速度等。
套娃套了三层。
接下来看看addFeatureCheckParallax()函数里面是什么逻辑，这个函数定义在feature_manager.cpp里。
//关键帧判断
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
ROS_DEBUG("input feature: %d", (int)image.size());
ROS_DEBUG("num of feature: %d", getFeatureCount());
//总平行度
double parallax_sum = 0;
//平行特征点数
int parallax_num = 0;
//统计在滑窗中的特征点有多少个在当前帧中继续被追踪到了
last_track_num = 0;
//遍历当前帧的每一个特征点
for (auto &id_pts : image)
{
//把当前特征点封装成一个FeaturePerFrame对象
FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
//获取当前帧的feature_id
int feature_id = id_pts.first;
//在滑窗的所有特征点中，看看能不能找到当前这个特征点
auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
{//是find的一个谓词判断版本，它利用返回布尔值的谓词判断pred，检查迭代器区间[first, last)上的每一个元素，如果迭代器iter满足pred(*iter) == true，表示找到元素并返回迭代器值iter；未找到元素，则返回last。
return it.feature_id == feature_id;
});
//如果这个特征点是一个新的特征(在特征点库里没有找到),那么就把它加入到滑窗的特征点库里
if (it == feature.end())
{
feature.push_back(FeaturePerId(feature_id, frame_count));
feature.back().feature_per_frame.push_back(f_per_fra);
}
//如果这个特征在滑窗中已经被观测到过，那么就补充上这个特征点在当前帧的数据，并且把共视点统计数+1
else if (it->feature_id == feature_id)
{
it->feature_per_frame.push_back(f_per_fra);
last_track_num++;
}
}
//如果总共2帧，或者说共视点<20，那么说明次新帧是关键帧，marg_old
if (frame_count < 2 || last_track_num < 20)
return true;
//遍历滑窗中的每一个特征点
for (auto &it_per_id : feature)
{ //如果当前特征点在当前帧-2以前出现过而且至少在当前帧-1还在，那么他就是平行特征点
if (it_per_id.start_frame <= frame_count - 2 &&
it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
{
parallax_sum += compensatedParallax2(it_per_id, frame_count);
parallax_num++;//平行特征点数
}
}
if (parallax_num == 0)//判断标准1:平行特征点数为0
{ return true; }
else//判断标准2:平均平行度小于threshold
{ return parallax_sum / parallax_num >= MIN_PARALLAX; }
}
这一部分代码完全在秦通大神的IV.A部分体现了。秦神在这部分里写了2个判断关键帧的判断指标，第一个是“the average parallax apart from the previous keyframe”，对应着代码中parallax_num和parallax_sum / parallax_num；第二个是“If the number of tracked features goes below a certain threshold, we treat this frame as a new keyframe”，对应着代码里的last_track_num。注意，这部分里还有一个函数是compensatedParallax2()，用来计算当前特征点的视差。
2、将图像数据、时间、临时预积分值存到图像帧类中
ImageFrame imageframe(image, header.stamp.toSec());
imageframe.pre_integration = tmp_pre_integration;
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
这里出现了一个新的数据结构，分析如下：
数据结构: ImageFrame imageframe
imageframe是ImageFrame的一个实例，定义在initial/initial_alignment.h里。顾名思义，它是用于融合IMU和视觉信息的数据结构，包括了某一帧的全部信息:位姿，特征点信息，预积分信息，是否是关键帧等。
3、更新临时预积分初始值
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
4、如果需要标定外参，则标定
if(ESTIMATE_EXTRINSIC == 2)如果没有外参则进行标定
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ROS_WARN("initial extrinsic rotation calib success");
ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;
}
}
}
5、初始化
这是一个重要模块，见下一章节介绍。一般初始化只进行一次。
6、非线性优化
这是一个重要模块，见下下章节介绍。一般非线性优化是稳定状态下VINS系统一直要循环的部分。