VINS-MONO代码解读----vins_estimator(重点部分)-编程知识

VINS-MONO代码解读----vins_estimator(重点部分)

1.

代码目录如下，重点和难点是factor部分，是关于IMU部分的，有较多关于IMU预积分公式的推导。
在这里插入图片描述

1. 条件变量con.wait读取测量值：getMeasurements()

读取buf中IMU和IMG的数据，并进行align，最后的结果是这样：
在这里插入图片描述

2.读取到之后，对IMU数据进行预积分，

processIMU的实现需要结合公式弄明白。

3. 设置重定位帧

relo_buf：订阅pose graph node发布的回环帧数据，存到relo_buf队列中，供重定位使用
关于relo的部分需要搞清楚数据结构，何时pub？
为何要在process中执行estimator.setReloFrame，何时设置reloframe？

4. 重点函数estimator.processImage

视觉与IMU的初始化以及非线性优化的紧耦合

4.0 frame_count变量

int frame_count;// 最新帧在滑动窗口中的索引（0，1，2，... ，WINDOW_SIZE-1）WINDOW_SIZE=10，等frame_count==10时就该进行marg了，以保证window内只有10帧数据，该变量在window没满之前会自增，达到window_size之后就一直保持为window_size(10)

在这里插入图片描述

4.1 `addFeatureCheckParallax`中的条件判断

如下代码，其中进入compensatedParallax2函数的条件比较难理解，这里debug研究一下

for (auto &it_per_id : feature)
{// it_per_id.feature_per_frame.size()就是sliding window内该id的feature被tracking的次数，// 这两个判断条件意思是：要有至少3帧，且第3新帧和第2新帧之间要tracking上，保证有共视点来计算视差 TODO:可以debug看看，Done：已经debug过了，确实是这个意思// 这里img一定是it_per_id.feature_per_frame.size()>=1，即至少tracking了1次，即使在1st lost了但是在3rd和2nd时分别都tracking上了，那么3rd和2nd之间就有视差int condition_2 = it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 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) //后一个判断条件是什么意思？{ROS_DEBUG("condition matched: condition_1: %d, condition_2: %d, frame_count: %d", it_per_id.start_frame, condition_2, frame_count);// 对于给定id的特征点，计算第2最新帧和第3最新帧之间该特征点的视差（当前帧frame_count是第1最新帧）//（需要使用IMU数据补偿由于旋转造成的视差）parallax_sum += compensatedParallax2(it_per_id, frame_count);parallax_num++;} else {ROS_WARN("condition not matched: condition_1: %d, condition_2: %d, frame_count: %d", it_per_id.start_frame, condition_2, frame_count);}
}

如下所示：
在这里插入图片描述
一个最极端的例子：
该feature在3rd时被extract出来，it_per_id.feature_per_frame.size()=1，
在2nd时被tracking上了，it_per_id.feature_per_frame.size()=2，
在1st时lost掉了，it_per_id.feature_per_frame.size()=2，start=0，frame_count=2
也是符合条件的：如下打印的0，1，2就是这种例子，start=0，size()=1+1=2，frame_count=2
而0，0，2则对应的是该feature目前只在[0]帧被extract出来一次，没有被tracking上
2，2，2则代表在3rd刚被extract出来，故在3rd和2nd之间不可能存在共视点，也就没有视差。

调试结果：
在这里插入图片描述

在这里插入图片描述

4.2 compensatedParallax2()

/*** 对于给定id的特征点* 计算第2最新帧和第3最新帧之间该特征点的视差（当前帧frame_count是第1最新帧）* （需要使用IMU数据补偿由于旋转造成的视差）*/
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{//check the second last frame is keyframe or not//parallax betwwen seconde last frame and third last frame//计算3rd和2nd在it_per_id.feature_per_frame下的index，size是该id的feature已经被tracking的次数，// 如frame_count=4时，从第0帧开始被tracking，start_frame=0，则3rd和2nd时，it_per_id.feature_per_frame.size()分别为3[index=2],4[index=3]，则他们的index分别为4-2-0=2，4-1-0=3// start_frame=1时，size()=2[index=1],3[index=2], index分别为4-2-1=1，4-1-1=2，图示见博客4.2节：https://blog.csdn.net/qq_37746927/article/details/134436475int third_lst_idx = frame_count - 2 - it_per_id.start_frame;int second_lst_idx = frame_count - 1 - it_per_id.start_frame;//这里的window size是10ROS_INFO("======here1: frame_count: %d, third_lst_idx: %d, second_lst_idx: %d", frame_count, third_lst_idx, second_lst_idx);const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[third_lst_idx];//third last frameconst FeaturePerFrame &frame_j = it_per_id.feature_per_frame[second_lst_idx];//seconde last framedouble ans = 0;Vector3d p_j = frame_j.point;double u_j = p_j(0);double v_j = p_j(1);Vector3d p_i = frame_i.point;Vector3d p_i_comp;//int r_i = frame_count - 2;//int r_j = frame_count - 1;//p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;p_i_comp = p_i;double dep_i = p_i(2);//深度double u_i = p_i(0) / dep_i;double v_i = p_i(1) / dep_i;double du = u_i - u_j, dv = v_i - v_j;//计算i,j帧间的视差double dep_i_comp = p_i_comp(2);double u_i_comp = p_i_comp(0) / dep_i_comp;double v_i_comp = p_i_comp(1) / dep_i_comp;double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;//这俩货不一样？？ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));return ans;
}