Vins-Mono 论文 && Coding 一 1. 系统流程

最新推荐文章于 2023-02-28 21:50:41 发布

置顶 scofield_ly

最新推荐文章于 2023-02-28 21:50:41 发布

阅读量342

点赞数

分类专栏： # vio 文章标签： slam

本文链接：https://blog.csdn.net/weixin_40072161/article/details/108292539

版权

vio 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

一、System Pipeline

二、视觉数据处理流程

1. API

// image: feature_id<-->视觉特征(有可能多摄像头)
// 视觉特征:  camera_id <--> x, y, z, p_u, p_v, velocity_x, velocity_y
// header: img_msg->header 
void processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)

2. 流程图

3. 流程详解

1. 特征点跟踪

更新 FeaturesPerId

class FeaturePerFrame
{
  public:
    FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td) {......}
    double cur_td;  // imu, image timestamp offset
    Vector3d point;  // camera 下三维坐标 (深度未知, z=1)
    Vector2d uv;  // 图像观测
    Vector2d velocity;  // 速度
    double z;
    bool is_used;
    double parallax;
    MatrixXd A;
    VectorXd b;
    double dep_gradient;
};

class FeaturePerId
{
  public:
    const int feature_id;
    int start_frame;  // 第一次被观测的帧在窗口中的索引
    vector<FeaturePerFrame> feature_per_frame;  // 所有的观测
    int used_num;
    bool is_outlier;
    bool is_margin;
    double estimated_depth;
    int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail;
    Vector3d gt_p;
    int endFrame();
};

// frame_count: 当前帧在滑窗中的索引
// image: feature_id<-->视觉特征(有可能多摄像头)
// 视觉特征:  camera_id <--> x, y, z(=1), p_u, p_v, velocity_x, velocity_y
// td: imu, img timestamp offset
// return: whether the seconde last frame is keyframe
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    double parallax_sum = 0;
    int parallax_num = 0;
    last_track_num = 0;
    for (auto &id_pts : image) {
        // 构造 feature per-frame
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
        // 查找该 feature 是否之前被观测过
        if (it == feature.end())   {  // 第一次被观测
            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++;
        }
    }

    //check the second last frame is keyframe or not
    //parallax betwwen seconde last frame and third last frame
    // 通过对比次新帧和次次新帧的视差量
    if (frame_count < 2 || last_track_num < 20)  // 不计算 parallax
        return true;
    for (auto &it_per_id : feature) {
        // 选择从 third last frame 到 second last frame 均被观测到的 feature
        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  {
        return parallax_sum / parallax_num >= MIN_PARALLAX;  // parallax 超过阈值, seconde last frame 视作关键帧
    }
}

2. 更新边缘化策略

判断次新帧是否是关键帧

if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    marginalization_flag = MARGIN_OLD;
else
    marginalization_flag = MARGIN_SECOND_NEW;

(1). 当次新帧为关键帧时，marginalization_flag 是 MARGIN_OLD，将 marg 掉最老帧

(2). 当次新帧不是关键帧时，marginalization_flag 是 MARGIN_SECOND_NEW，我们将直接扔掉次新帧及它的视觉约束，而不对次新帧进行 marg

三、System Pipeline 详解

1. Measurement Preprocess

(1). 功能

视觉特征提取和跟踪

imu 数据预积分

(2). 详解

Vins-Mono 论文 && Coding 一 2. feature_tracker

Vins-Mono 论文 && Coding 一 3. visual-imu 数据同步

Vins-Mono 论文 && Coding 一 4(1). imu 预积分

Vins-Mono 论文 && Coding 一 4(2). imu factor

2. Initialization

(1). 功能

纯视觉 SFM

视觉惯导数据对齐

外参标定

(2). 详解

Vins-Mono 论文 && Coding 一 5(1). 初始化: SFM 初始化

Vins-Mono 论文 && Coding 一 5(2). 初始化: visual-inertial 对齐

3. visual-inertial odometry

(1). 功能

滑动窗口优化系统状态变量

(2). 详解

Vins-Mono 论文 && Coding 一 6(1). VIO: solveOdometry

Vins-Mono 论文 && Coding 一 6(1). VIO: slideWindow

4. loop closure && pose-graph

(1). 功能

回环检测，位姿图优化

(2). 详解

Vins-Mono 论文 && Coding 一 7(1). pose_graph: pose_graph 流程

Vins-Mono 论文 && Coding 一 7(2). pose_graph: 回环检测 && 重定位

Vins-Mono 论文 && Coding 一 7(3). pose_graph: 4DOF pose_graph

四、运行示例

1. launch 文件

euroc.launch 文件如下

<launch>
    ......
    <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
        ......
    </node>

    <node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
       ......
    </node>

    <node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
        ......
    </node>
</launch>

可以看到 vins-mono 运行了三个节点：feature_tracker, vins_estimator, pose_graph

feature_tracker 负责相邻帧角点跟踪

vins_estimator 是 vins 算法的核心，实现 visual-inertial odometry

pose_graph 是负责回环检测以及 pose_graph 优化