整体框架

上一节,我们对整个后端的结构有了初步的认识与了解,后端主线程主要进行了IMU与图像的组合,并在组合之后对每一对组合一一进行了处理,首先是IMU的预积分操作,前面已介绍,重点就是处理图像的操作,即函数processImage(),它的作用就是实现视觉与IMU的初始化以及非线性优化的紧耦合,它所涉及的内容十分庞大,重点就是初始化以及紧耦合两部分,首先我们通过下面的流程图,对整个processImage()有一个整体的了解,对应着流程图看代码,方能更好的理解代码的逻辑。

初始化与紧耦合实现流程图

VINS_by

在了解这个流程图具体每一步都做了什么之前,我们首先要知道processImage()传入参数的方式,它的参数为一个 map ,以及一个时间戳,map 的结构如下图所示:

image-20220629142128353

这里就涉及到VINS的特征点管理方式。

VINS中的特征点管理

VINS管理特征点的代码位置在:vins_estimator\src\feature_manager.h,代码可能不是很直观,这里做了一个简单的图,形象的介绍了特征点管理的具体方式。

如图所示,我们可以比较清晰的看出它特征点管理的方式。首先,存在1,2,3,4等四个路标点,以及0-5等6个图像帧,滑窗内一共有0-4等5个图像帧,每个图像帧可以观察的路标点已经用虚线做连接;


VINS中的特征点管理方式


VINS使用 FeatureManager 类中的 list<FeaturePerId> feature; 来存储滑窗内的每一个特征点,可以看出 list<FeaturePerId> feature;由若干个 FeaturePerId 组成,每个 FeaturePerId 主要包含的属性有:

  • 路标点的Id:feature_id
  • start_frame:特征点首次被观测的图像帧的id(图中用橘黄色半透明的涂鸦)
  • endFrame():最后一次观测到该特征点对应图像帧的id(图中用红色半透明的涂鸦)
  • vector\<FeaturePerFrame\> feature_per_frame:管理对应帧的属性

实际上,每个feature_per_frame 向量就是每个路标点在对应观测帧中的信息:$[x,y,z,u,v,v_x,x_y,t_d]$

代码中还有一些其他属性:

public:    
    const int feature_id; //特征点Id
    int start_frame;  //特征点首次被观测的图像帧的Id
    vector<FeaturePerFrame> feature_per_frame; //管理对应帧的属性

    int used_num;  //出现的次数
    bool is_outlier;// 是否外点
    bool is_margin;// 是否Marg边缘化
    double estimated_depth;//逆深度
    int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; // 求解器

关键帧的处理

processImage_Step1 将特征点信息加到 f_manager 这个特征点容器中,同时进行关键帧的检查

//Step1 将特征点信息加到 f_manager 这个特征点容器中,同时进行关键帧的检查
// 计算每一个点跟踪的次数,以及它的视差并通过检测两帧之间的视差决定是否作为关键帧。
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    // 如果上一帧是关键帧,则滑窗中最老的帧就要被移出滑窗
    marginalization_flag = MARGIN_OLD;
else
    // 否则,移除上一帧
    marginalization_flag = MARGIN_SECOND_NEW;

ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
ROS_DEBUG("Solving %d", frame_count);
ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());
Headers[frame_count] = header;

// 将图像数据、时间、临时预积分值存到图像帧类中
// all_image_frame用来做初始化相关操作,它保留滑窗起始到当前的所有帧
// 有些帧会因为不是图像帧,被MARGIN_SECOND_NEW,但是即使是最新的帧,它也会保留在容器中,因为初始化要求使用所有帧,而非只有KF
ImageFrame imageframe(image, header.stamp.toSec());
imageframe.pre_integration = tmp_pre_integration;
// 这里就是简单的把图像和预积分绑在一起,这里的预积分是两帧之间的,滑窗中实际上是两个KF之间的
// 实际上是准备来初始化的相关数据
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));

// 更新临时预积分初始值
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

通过流程图,不难看出,系统在进行初始化之前,首先使用addFeatureCheckParallax()函数进行了一些预处理,主要功能就是将之前检测到的特征点添加到 feature 容器 list 中,然后计算每一个点跟踪的次数,以及它的视差并通过检测两帧之间的视差决定是否作为关键帧。

vins-mono的关键帧选择策略

  • 与前一帧的平均视差。如果跟踪特征的平均视差超过某个阈值,我们会将此图像视为关键帧。
  • 另一个是跟踪质量。如果跟踪特征的数量低于一个阈值,我们把这一帧看做一个新的关键帧。

image-20220629144445135

/**
 * @description: 添加之前检测到的特征点到 feature 容器 list 中,
 *O_  计算每一个点跟踪的次数,以及它的视差并通过检测两帧之间的视差决定是否作为关键帧。
 * @param {int} frame_count 窗口内帧的个数
 * @param {map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>} &image 
 *         某帧所有特征点的 [camera_id,[x,y,z,u,v,vx,vy]] 构成的 map, 索引为 feature_id
 * @param {double} td 相机和 IMU 同步校准得到的时间差
 * @return {*}
 */
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)
    {
        // 用特征点信息构造一个对象 f_per_fra([x,y,z,u,v,vx,vy],td)
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
        // 取出索引 feature_id
        int feature_id = id_pts.first;
        // 在已有的id中寻找是否有相同的特征点
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {
            return it.feature_id == feature_id;
                          });
        // 这是一个新的特征点
        if (it == feature.end())
        {
            // 在特征点管理器中,新创建一个特征点Id,这里的frame_count就是该特征点在滑窗中的当前位置,作为这个特征点的起始位置
            feature.push_back(FeaturePerId(feature_id, frame_count));
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        // 如果这是一个已知的特征点,就在对应的“组织”下增加一个帧属性
        else if (it->feature_id == feature_id)
        {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++; // 准踪上一帧的特征点数目
        }
    }
    /***********判断是否作为关键帧************/
    // 前两帧都设为KF,追踪次数过少也设为KF
    if (frame_count < 2 || last_track_num < 20)
        return true;

    for (auto &it_per_id : feature)
    {
        // 计算的实际上是 frame_count-1 也就是前一帧是否为关键帧
        // 因此起始帧至少得是 frame_count-2 同时至少覆盖到 frame_count-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)
    {
        return true;
    }
    else
    {
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
}
🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/77.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 06 月 30 日
赏杯咖啡