《VINS-mono框架入门及代码解析》笔记1：前端与特征点管理

Z-way

已于 2022-03-24 15:10:41 修改

阅读量4k

点赞数 1

分类专栏： VINS Mono学习笔记文章标签： slam

于 2022-03-24 13:43:55 首次发布

本文链接：https://blog.csdn.net/weixin_42751489/article/details/123707149

版权

VINS Mono学习笔记专栏收录该内容

2 篇文章 1 订阅

订阅专栏

系列文章目录

文章目录

系列文章目录
1. VINS-Mono框架介绍
2. VINS前端逻辑
3. 前端代码详解
4. 特征点管理代码详解
5. processImage函数
6. 图解VINS特征点管理策略

1. VINS-Mono框架介绍

在这里插入图片描述
每一部分的功能：

2. VINS前端逻辑

2.1 图解前端

在这里插入图片描述

2.2 代码中的当前帧

在这里插入图片描述

2.3 图解光流

在这里插入图片描述

已知：上一帧图像以及观测到的特征点
得到当前帧
对当前帧进行直方图均衡化
调用OpenCV函数在当前帧上利用光流法跟踪上一帧的特征点
剔除：跟踪失败的特征点、在图像边缘的特征点、外点
剔除后，为了保证特征点数量不太少，需要提取新的特征点
首先在跟踪成功的特征点附近设置Mask，Mask中不会提取特征点，保证特征点能够均匀分布
调用OpenCV函数提取新的特征点
对特征点进行归一化和去畸变处理

3. 前端代码详解

3.1 主函数

int main(int argc, char **argv)
{
    ros::init(argc, argv, "feature_tracker");   // ros节点初始化
    ros::NodeHandle n("~"); // 声明一个句柄，～代表这个节点的命名空间
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);    // 设置ros log级别
    readParameters(n); // 读取配置文件

    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);    // 获得每个相机的内参

    if(FISHEYE)
    {
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
            if(!trackerData[i].fisheye_mask.data)
            {
                ROS_INFO("load mask fail");
                ROS_BREAK();
            }
            else
                ROS_INFO("load mask success");
        }
    }

    // 这个向roscore注册订阅这个topic，收到一次message就执行一次回调函数
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
    // 注册一些publisher
    pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000); // 实际发出去的是 /feature_tracker/feature
    pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
    pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
    ros::spin();    // spin代表这个节点开始循环查询topic是否接收
    return 0;
}

ROS节点初始化，声明ROS句柄，设置log级别
读取配置文件
读取每个相机的内参和畸变参数
判断是否加入鱼眼
向roscore注册订阅这个topic，收到一次message就执行一次回调函数
发布跟踪到的特征点、带有特征点的图像、重启信息

3.2 回调函数

    if(first_image_flag) // 对第一帧图像的基本操作
    {
        first_image_flag = false;
        first_image_time = img_msg->header.stamp.toSec();
        last_image_time = img_msg->header.stamp.toSec();
        return;
    }
    // detect unstable camera stream
    // 检查时间戳是否正常，这里认为超过一秒或者错乱就异常
    // 图像时间差太多光流追踪就会失败，这里没有描述子匹配，因此对时间戳要求就高
    if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
    {
        // 一些常规的reset操作
        ROS_WARN("image discontinue! reset the feature tracker!");
        first_image_flag = true; 
        last_image_time = 0;
        pub_count = 1;
        std_msgs::Bool restart_flag;
        restart_flag.data = true;
        pub_restart.publish(restart_flag);  // 告诉其他模块要重启了
        return;
    }
    last_image_time = img_msg->header.stamp.toSec();
    // frequency control
    // 控制一下发给后端的频率
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)    // 保证发给后端的不超过这个频率
    {
        PUB_THIS_FRAME = true;
        // reset the frequency control
        // 这段时间的频率和预设频率十分接近，就认为这段时间很棒，重启一下，避免delta t太大
        if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
        {
            first_image_time = img_msg->header.stamp.toSec();
            pub_count = 0;
        }
    }
    else
        PUB_THIS_FRAME = false;

    // 即使不发布也是正常做光流追踪的！光流对图像的变化要求尽可能小

    cv_bridge::CvImageConstPtr ptr;
    // 把ros message转成cv::Mat
    if (img_msg->encoding == "8UC1")
    {
        sensor_msgs::Image img;
        img.header = img_msg->header;
        img.height = img_msg->height;
        img.width = img_msg->width;
        img.is_bigendian = img_msg->is_bigendian;
        img.step = img_msg->step;
        img.data = img_msg->data;
        img.encoding = "mono8";
        ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
    }
    else
        ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);

判断是否是第一帧图像，如果是，记录第一帧图像的时间
检查时间戳是否正常，如果间隔大于1秒或当前时间戳小于上一时间戳，则返回重启信息
控制发给后端的频率，保证频率在10Hz上下
进行图像格式转换，将ROS图像转换为CV图像

	cv::Mat show_img = ptr->image;
    TicToc t_r;
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ROS_DEBUG("processing camera %d", i);
        if (i != 1 || !STEREO_TRACK)
            trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
        else
        {
            if (EQUALIZE)
            {
                cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
                clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
            }
            else
                trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
        }

#if SHOW_UNDISTORTION
        trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
    }

对于不同的相机进行处理
对于第一个相机，使用readImage进行处理
对于后续的相机，只进行直方图均衡化处理

 	for (unsigned int i = 0;; i++)
    {
        bool completed = false;
        for (int j = 0; j < NUM_OF_CAM; j++)
            if (j != 1 || !STEREO_TRACK)
                completed |= trackerData[j].updateID(i);    // 单目的情况下可以直接用=号
        if (!completed)
            break;
    }

对于新提取的特征点，更新全局id

    // 给后端喂数据
   if (PUB_THIS_FRAME)
   {
        pub_count++;    // 计数器更新
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
        sensor_msgs::ChannelFloat32 id_of_point;
        sensor_msgs::ChannelFloat32 u_of_point;
        sensor_msgs::ChannelFloat32 v_of_point;
        sensor_msgs::ChannelFloat32 velocity_x_of_point;
        sensor_msgs::ChannelFloat32 velocity_y_of_point;

        feature_points->header = img_msg->header;
        feature_points->header.frame_id = "world";

        vector<set<int>> hash_ids(NUM_OF_CAM);
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            auto &un_pts = trackerData[i].cur_un_pts;   // 去畸变的归一化相机坐标系
            auto &cur_pts = trackerData[i].cur_pts; // 像素坐标
            auto &ids = trackerData[i].ids; // id
            auto &pts_velocity = trackerData[i].pts_velocity;   // 归一化坐标下的速度
            for (unsigned int j = 0; j < ids.size(); j++)
            {
                // 只发布追踪大于1的，因为等于1没法构成重投影约束，也没法三角化
                if (trackerData[i].track_cnt[j] > 1)
                {
                    int p_id = ids[j];
                    hash_ids[i].insert(p_id);   // 这个并没有用到
                    geometry_msgs::Point32 p;
                    p.x = un_pts[j].x;
                    p.y = un_pts[j].y;
                    p.z = 1;
                    // 利用这个ros消息的格式进行信息存储
                    feature_points->points.push_back(p);
                    id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
                    u_of_point.values.push_back(cur_pts[j].x);
                    v_of_point.values.push_back(cur_pts[j].y);
                    velocity_x_of_point.values.push_back(pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(pts_velocity[j].y);
                }
            }
        }
        feature_points->channels.push_back(id_of_point);
        feature_points->channels.push_back(u_of_point);
        feature_points->channels.push_back(v_of_point);
        feature_points->channels.push_back(velocity_x_of_point);
        feature_points->channels.push_back(velocity_y_of_point);
        ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
        // skip the first image; since no optical speed on frist image
        if (!init_pub)
        {
            init_pub = 1;
        }
        else
            pub_img.publish(feature_points);    // 前端得到的信息通过这个publisher发布出去

将特征点的归一化坐标、ID、像素坐标、速度发布给后端
第一帧不发布

3.3 readImage函数

    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    if (EQUALIZE)
    {
        // 图像太暗或者太亮，提特征点比较难，所以均衡化一下
        // ! opencv 函数看一下
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    // 这里forw表示当前，cur表示上一帧
    if (forw_img.empty())   // 第一次输入图像，prev_img这个没用
    {
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        forw_img = img;
    }

    forw_pts.clear();

    if (cur_pts.size() > 0) // 上一帧有特征点，就可以进行光流追踪了
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        // 调用opencv函数进行光流追踪
        // Step 1 通过opencv光流追踪给的状态位剔除outlier
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

        for (int i = 0; i < int(forw_pts.size()); i++)
            // Step 2 通过图像边界剔除outlier
            if (status[i] && !inBorder(forw_pts[i]))    // 追踪状态好检查在不在图像范围
                status[i] = 0;
        reduceVector(prev_pts, status); // 没用到
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);  // 特征点的id
        reduceVector(cur_un_pts, status);   // 去畸变后的坐标
        reduceVector(track_cnt, status);    // 追踪次数
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }
    // 被追踪到的是上一帧就存在的，因此追踪数+1
    for (auto &n : track_cnt)
        n++;

如果图片太暗或太亮，就进行直方图均衡化
如果forw_img为空，则表示是第一帧，对prev，cur，forw都进行赋值；否则说明不是第一帧，只对forw_img赋值
调用OpenCV函数，进行光流跟踪
剔除跟踪失败，以及图像边界之外的点

    if (PUB_THIS_FRAME)
    {
        // Step 3 通过对级约束来剔除outlier
        rejectWithF();
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            // 只有发布才可以提取更多特征点，同时避免提的点进mask
            // 会不会这些点集中？会，不过没关系，他们下一次作为老将就得接受均匀化的洗礼
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;   // 以上三个量无用
    cur_img = forw_img; // 实际上是上一帧的图像
    cur_pts = forw_pts; // 上一帧的特征点
    undistortedPoints();
    prev_time = cur_time;

调用rejectWithF，利用RANSAC函数剔除外点
剔除外点后，特征点数量不足，需要进一步提取特征点
首先设置Mask，使特征点均匀分布
然后统计需要提取的点的数量，调用OpenCV函数进行特征提取
将新提取的特征点加入特征点vector中
对特征点进行深度归一化和畸变校正

3.4 rejectWithF函数

void FeatureTracker::rejectWithF()
{
    // 当前被追踪到的光流至少8个点
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            Eigen::Vector3d tmp_p;
            // 得到相机归一化坐标系的值
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            // 这里用一个虚拟相机，原因同样参考https://github.com/HKUST-Aerial-Robotics/VINS-Mono/issues/48
            // 这里有个好处就是对F_THRESHOLD和相机无关
            // 投影到虚拟相机的像素坐标系
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;
        // opencv接口计算本质矩阵，某种意义也是一种对级约束的outlier剔除
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}

遍历所有特征点
得到相机归一化坐标的值，进行畸变校正，再转化回像素坐标
调用OpenCV函数计算基础矩阵，然后利用RANSAC剔除外点

4. 特征点管理代码详解

TicToc t_s;
// 特征点id->特征点信息
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
    int v = img_msg->channels[0].values[i] + 0.5;
    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];
    ROS_ASSERT(z == 1); // 检查是不是归一化
    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);
}
estimator.processImage(image, img_msg->header);

将特征点放入map容器中：

单目相机：相机ID始终为0

5. processImage函数

将所有特征点信息存储到链表feature中

list<FeaturePerId> feature

FeaturePerId类代表一个特征点

class FeaturePerId
{
  public:
    const int feature_id;
    int start_frame;
    vector<FeaturePerFrame> feature_per_frame;  // 该id对应的特征点在每个帧中的属性

    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;

    FeaturePerId(int _feature_id, int _start_frame)
        : feature_id(_feature_id), start_frame(_start_frame),
          used_num(0), estimated_depth(-1.0), solve_flag(0)
    {
    }

    int endFrame();
};

id、起始帧
在每个帧中的属性
观测次数
是否是外点、是否被边缘化
估计深度是否收敛
结束帧

featurePerFrame类

class FeaturePerFrame
{
  public:
    FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td)
    {
        point.x() = _point(0);
        point.y() = _point(1);
        point.z() = _point(2);
        uv.x() = _point(3);
        uv.y() = _point(4);
        velocity.x() = _point(5); 
        velocity.y() = _point(6); 
        cur_td = td;
    }
    double cur_td;
    Vector3d point;
    Vector2d uv;
    Vector2d velocity;
    double z;
    bool is_used;
    double parallax;
    MatrixXd A;
    VectorXd b;
    double dep_gradient;
};

每个特征点的归一化坐标、像素坐标、速度、id

6. 图解VINS特征点管理策略

在这里插入图片描述
代码

for (auto &id_pts : image)
{
    // 用特征点信息构造一个对象
    FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

    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++;   // 追踪到上一帧的特征点数目
    }
}

遍历每个特征点，取出特征点id
在feature中寻找id
如果是新特征点，push到feature中
如果是旧特征点，把featurePerFrame添加到featurePerId中

Z-way

关注

1
点赞
踩
24

收藏

觉得还不错? 一键收藏
0
评论
《VINS-mono框架入门及代码解析》笔记1：前端与特征点管理

系列文章目录提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加例如：第一章 Python 机器学习入门之pandas的使用文章目录系列文章目录1. VINS-Mono框架介绍2. VINS前端逻辑2.1 图解前端2.2 代码中的当前帧2.3 图解光流3. 前端代码详解3.1 主函数3.2 回调函数3.3 readImage函数3.4 rejectWithF函数4. 特征点管理代码详解5. processImage函数6. 图解VINS特征点管理策略1. VINS-Mono框架介绍
复制链接

扫一扫