VINS-Mono算法代码解读

最新推荐文章于 2025-10-25 18:08:55 发布

原创

最新推荐文章于 2025-10-25 18:08:55 发布 · 2.9k 阅读

CC 4.0 BY-SA版权

文章标签：

本文详细解析了VINS-Mono算法的实现细节，包括特征跟踪、位姿估计及位姿图优化等模块。介绍了特征点跟踪流程、预积分计算、滑动窗口管理以及位姿图优化的具体实现。

算法分为三个部分，分别在3个独立的ros工程中，分别为 feature_tracker vins_estimator pose_graph

运行算法的launch文件，如euroc.launch中则运行该三个节点，并获取配置文件 config/euroc_config.yaml 路径，传递给各程序

在这里插入图片描述

因此，大致的算法思想为feature_tracker模块对图像数据进行处理，得到特征点跟踪信息，vins_estimator为主体程序处理跟踪得到的特征点信息和imu数据，pose_graph则为后端优化以及重定位功能，接下再仔细对每一个模块进行分析

feature_tracker 模块

本模块中核心实现为特征点跟踪类，基本流程为获取得到一帧图片数据转换为opencv格式并控制帧率，最后送入FeatureTracker类中跟踪特征点，最后发布特征点数据。核心调用在于95行的

95:trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());

其中函数readImage的主要流程如下，包含两个较为重要的知识点

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PNodgt9E-1614779106378)(https://storage.live.com/items/24342272185BBA7E!4893?authkey=AJzdbBYZIQ_AuAo)]

主要的执行函数如下