VINS fusion软件架构分析（1）----主函数 rosNodeTest.cpp

weixin_40084095

已于 2022-05-12 23:11:22 修改

阅读量701

点赞数 4

分类专栏： VINS FUSION 文章标签： opencv c++ 人工智能计算机视觉

于 2022-05-08 22:21:50 首次发布

本文链接：https://blog.csdn.net/weixin_40084095/article/details/124647161

版权

VINS FUSION 专栏收录该内容

6 篇文章 23 订阅

订阅专栏

整体架构

在这里插入图片描述

1. 主函数 rosNodeTest.cpp

1.1 头文件

#include 文件包含命令，用来引入对应的头文件（.h文件）

使用尖括号< >，编译器会到系统路径下查找头文件；
而使用双引号" "，编译器首先在当前目录下查找头文件，如果没有找到，再到系统路径下查找。
stdio.h是stand input & output的缩写，意思是标准输入输出头文件
queue是一种容器转换器模板，如queue＜char, list＜char＞＞ q1；
map的功能是自动建立key － value的对应。key 和 value可以是任意你需要的类型，包括自定义类型。
thread主要声明了std::thread线程类
C++11中提供了std::mutex互斥量，提供了独占所有权的特性
cv_bridge是ROS图像消息和OPENCV图像之间进行转换的一个功能包
estimator是状态估计器
parameters是读取yaml文件中的参数
visualization是向其他结点发布信息，可视化操作
.h是声明， .cpp是实现

在这里插入图片描述

1.2 rosNodeTest.cpp的整体结构

在这里插入图片描述

1.3 rosNodeTest.cpp的main函数

在这里插入图片描述

1）初始化结点，结点名字为vins_estimator；
2）句柄可以让你通过构造函数指定命名空间
ros::NodeHandle pn1(“~”); //pn1 命名空间为/node_namespace/node_name
3） INFO：输出大量用户需要的信息。默认有Debug,Info,Warn,Error,Fatal(首字母大写)
在这里插入图片描述

4）运行该结点是通过下面这行代码进行的，argc 是指命令行输入参数的个数，命令行输入参包括：vins_node
和euroc_mono_imu_config.yaml，则argc = 2。
因此如果不是2，则报出故障，提醒操作人员输入参数的格式有问题。

 `rosrun   vins    vins_node     euroc_mono_imu_config.yaml`

在这里插入图片描述用readParameters读如yaml文件里的参数，下面是readParameters的部分代码和YAML文件的部分代码

5） registerPub(n)是用来创建多个发布者，发布多个话题。

在这里插入图片描述
advertise()括号里面的第一个参数是话题名字，第二个参数是用于发布消息的消息队列的大小。 <>里面指定消息的类型

6）创建多个订阅者，订阅IMU和相机消息

在回调函数中完成对消息的处理，如对图像提取特征点等操作。使用的回调函数包括：

imu_callback
feature_callback
img0_callback
img1_callback
restart_callback
imu_switch_callback
cam_switch_callback

在这里插入图片描述

A) IMU的回调函数

功能：接收到IMU的消息，提取其中的时间戳、线加速度、角加速度。放入estimator中。

在这里插入图片描述

B) 特征点的回调函数

注意：特征点的回调函数接收的节点名是/feature_tracker/feature，而在 VINS FUSION中没有发布这个节点，后续运行这个VINS FUSION使用的bag文件中也只要求有imu 和相机的信息。把这个回调函数注释后再运行也可以正常运行。
功能：接收到特征点的点云信息，提取其中特征点的ID、哪个相机获取的、3D位置、像素、速度。放入estimator中。
在这里插入图片描述

在这里插入图片描述

C) 相机的回调函数

功能：接收到相机的信息，放入buffer里

在这里插入图片描述

D) 相机信息的同步

std::thread sync_thread{sync_process};

· 比较相机1和2最新的图像信息的时间差别，相差在0.003s以内，则认为没有时间差
· 会以相机1的时间戳作为同步后的时间戳
· 把同步的图像信息放入estimator中。
注意：从相机获取的消息是以queue形式存储的。用push函数把最新的图像信息存入队尾。但是在同步的时候，取用的是队头front的图像消息。同步完成的时候会利用pop删除队头front的图像消息。从而保持queue里图像数目固定，不会无限制扩大。
在这里插入图片描述

· 会利用getImageFromMsg函数把ROS的图像消息转化为OPenCV格式的消息。利用了cv_bridge这个库。

当cv_bridge将ROS的图像消息转换为OpenCV图像格式时都是通过CvImage类实现的。一般来说，cv_bridge提供了两种方式转换为CvImage类，分别为复制（copy）和共享（share）。toCvCopy函数会从ROS消息中拷贝一个图像数据，不管如何修改CvImage类的内容都不会影响源数据

在这里插入图片描述