VINS-Mono 代码解析——视觉跟踪 feature_trackers

最新推荐文章于 2022-11-10 14:43:08 发布

TB81266

最新推荐文章于 2022-11-10 14:43:08 发布

阅读量1.7k

点赞数 5

分类专栏： VINS

本文链接：https://blog.csdn.net/qq_43247439/article/details/107254445

版权

本文详细解析VINS-Mono中的视觉跟踪模块feature_trackers，包括KLT稀疏光流算法跟踪特征、特征检测与分布、RANSAC外点剔除、相机模型校正以及ROS实现框架。内容涵盖理论分析、代码结构和关键函数功能介绍。

摘要由CSDN通过智能技术生成

文章目录

参考链接：https://blog.csdn.net/qq_41839222/article/details/86030962
参考链接：https://blog.csdn.net/try_again_later/article/details/104847052
二位博主写的太好了感谢博主 ~~~O(∩_∩)O哈哈~

理论分析

这部分主要说一下视觉跟踪模块（feature_trackers），先说一下涉及到的具体内容：

对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪；
检测新的角点特征以保证每个图像特征的最小数目(100-300)
通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布；
利用基本矩阵模型的RANSAC算法进行外点剔除；
对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)。
关键帧选取
- 当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）
- 当前帧跟踪到的特征点数量小于阈值视为关键帧；

部分理论说明

这里补充一下理论部分的:对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)

m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);

可以根据不同的相机模型将二维坐标转换到三维坐标：
对于CATA（卡特鱼眼相机）将像素坐标投影到单位圆内，这里涉及了鱼眼相机模型
而对于PINHOLE（针孔相机）将像素坐标直接转换到归一化平面（z=1）并采用逆畸变模型(k1,k2,p1,p2)去畸变等。

节点在启动时会先读取相机内参，根据config_file文件中model_type的值决定采用何种相机模型，并创建相应模型的对象指针，读取在该模型下需要的参数。他们之间的调用顺序:
readIntrinsicParameter ------> generateCameraFromYamlFile -----> CameraPtr

for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//读取相机内参,保存到CAM_NAMES[i],如何获取相机的内呢???
        
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
   
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}

CameraPtr
CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
   
    cv::FileStorage fs(filename, cv::FileStorage::READ);
    if (!fs.isOpened())
    {
   
        return CameraPtr();
    }
    Camera::ModelType modelType = Camera::MEI;
    if (!fs["model_type"].isNone())
    {
   
        std::string sModelType;
        fs["model_type"] >> sModelType;
        if (boost::iequals(sModelType, "kannala_brandt"))
        {
   
            modelType = Camera::KANNALA_BRANDT;
        }
        else if (boost::iequals(sModelType, "mei"))
        {
   
            modelType = Camera::MEI;
        }
        else if (boost::iequals(sModelType, "scaramuzza"))
        {
   
            modelType = Camera::SCARAMUZZA;
        }
        else if (boost::iequals(sModelType, "pinhole"))
        {
   
            modelType = Camera::PINHOLE;
        }
        else
        {
   
            std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
            return CameraPtr();
        }
    }
    switch (modelType)
    {
   
    case Camera::KANNALA_BRANDT:
    {
   
        EquidistantCameraPtr camera(new EquidistantCamera);
        EquidistantCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::PINHOLE:
    {
   
        PinholeCameraPtr camera(new PinholeCamera);
        PinholeCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::SCARAMUZZA:
    {
   
        OCAMCameraPtr camera(new OCAMCamera);
        OCAMCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::MEI:
    default:
    {
   
        CataCameraPtr camera(new CataCamera);
        CataCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    }
    return CameraPtr();
}

数据预处理框架

代码流程如下图：主要三个源程序，feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。
在这里插入图片描述

我们有了整体的理论以及部分理论的补充之后,我们看一下视觉跟踪整体的ROS框架

ROS代码实现

在这里插入图片描述
feature_trackers可被认为是一个单独的模块
输入：

图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”

ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

输出：

发布topic：“/feature_trackers/feature_img” 即跟踪的特征点图像，主要是之后给RVIZ用和调试用
发布topic：“/feature_trackers/feature” 即跟踪的特征点信息，由/vins_estimator订阅并进行优化
发布topic：“/feature_trackers/restart” 即判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅

pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);

现在我们知道了视觉跟踪的ROS框架, 订阅者和发布者等信息. 接下来一起看一下视觉跟踪的代码结构.

视觉跟踪代码结构

VINS对于视觉跟踪相关的代码在feature_tracker文件夹中。
feature_trackers_node.cpp：main()函数 ROS的程序入口, 进入后配置参数,在parameters.cpp中.
在这里插入图片描述

feature_trackers_node.cpp 中的局部代码

 //1.读取yaml中的一些配置参数 地址为config->euroc->euroc_config.yaml
    readParameters(n);
    //2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目
    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//读取相机内参,保存到CAM_NAMES[i],如何获取相机的内呢???
    // 3.判断是否加入鱼眼mask 来去除边缘噪声
    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");
        }
    }
    // 4.订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时，执行回调函数img_callback
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

feature_trackers.cpp/feature_trackers.h：构建feature_trackers类，实现特征点跟踪的函数位于feature_tracker.cpp 与feature_tracker.h中,我们看一下各个函数的功能:
在这里插入图片描述
feature_trackers类的成员变量

	cv::Mat mask;//图像掩码
    cv::Mat fisheye_mask;//鱼眼相机mask，用来去除边缘噪点

    // prev_img是上一次发布的帧的图像数据
    // cur_img是光流跟踪的前一帧的图像数据
    // forw_img是光流跟踪的后一帧的图像数据
    cv::Mat prev_img, cur_img, forw_img;

    vector<cv::Point2f> n_pts;//每一帧中新提取的特征点
    vector<cv::Point2f> prev_pts, cur_pts, forw_pts;//对应的图像特征点
    vector<cv::Point2f> prev_un_pts, cur_un_pts;//归一化相机坐标系下的坐标
    vector<cv::Point2f> pts_velocity;//当前帧相对前一帧特征点沿x,y方向的像素移动速度
    vector<int> ids;//能够被跟踪到的特征点的id
    vector<int> track_cnt;//当前帧forw_img中每个特征点被追踪的时间次数

    map<int, cv::Point2f> cur_un_pts_map;
    map<int, cv::Point2f> prev_un_pts_map;

    camodocal::CameraPtr m_camera;//相机模型
    double cur_time;
    double prev_time;

    static int n_id;//用来作为特征点id，每检测到一个新的特征点，就将n_id作为该特征点的id，然后n_id加1

可以看到，视觉跟踪的主要实现方法集中在feature_trackers_node.cpp的回调函数和feature_trackers类中，接下来将从main()函数开始对加粗的函数进行具体解读:

一、feature_tracker_node.cpp

主要分为两部分：int main（）函数为程序入口，void img_callback（）为ROS的回调函数，对图像进行特征点追踪，处理和发布。

int main(int argc, char **argv)
{
   
    ros::init(argc, argv, "feature_tracker");  //ros初始化和设置句柄
    ros::NodeHandle n("~");// 命名空间为/node_namespace/node_name

    //设置logger的级别。 只有级别大于或等于level的日志记录消息才会得到处理。
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    // 1.读取yaml中的一些配置参数 地址为config->euroc->euroc_config.yaml
    readParameters(n);

    // 2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目
    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

    // 3.判断是否加入鱼眼mask 来去除边缘噪声
    if(FISHEYE)
    {
   
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
   
            trackerData[i].fisheye_mask = cv::imread(

最低0.47元/天解锁文章

TB81266

关注

5
点赞
踩
19

收藏

觉得还不错? 一键收藏
0
评论
VINS-Mono 代码解析——视觉跟踪 feature_trackers

文章目录理论分析数据预处理框架一、feature_tracker_node.cpp理论分析数据预处理框架代码流程如下图：主要三个源程序，feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。一、feature_tracker_node.cpp主要分为两部分：int main（）函数为程序入口，void img_callback（）为ROS的回调函数，对图像进行特征点追踪，处理和发布。
复制链接

扫一扫

专栏目录