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

news2024/12/24 7:40:29

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/134436475
    int 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是10
    ROS_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 frame
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[second_lst_idx];//seconde last frame

    double 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;
}