vins fusion笔记

一、 相关介绍

1. OpenCV

开源、跨平台的计算机视觉和机器学习软件库。轻量级而且高效，由一系列 C 函数和少量C++类构成，同时提供了Python、Ruby、MATLAB等语言的接口，实现了图像处理和计算机视觉方面的很多通用算法。

2. VINS-Fusion

简介：基于优化的多传感器状态估计器

VINS-Fusion是一款基于优化的多传感器状态估计器，可为自主应用（无人机、汽车和AR/VR）实现精确的自定位。

支持多种视觉惯性传感器类型（单声道摄像头+ IMU，立体摄像头+ IMU，甚至仅立体摄像头），也能支持VINS 与 GPS 融合。特征：

（1）支持多个传感器（立体摄像头/单色摄像头+IMU/立体摄像头+IMU）

（2）在线空间校准（相机和IMU之间的转换）

（3）在线时间校准（相机和IMU之间的时间偏移）

（4）可视闭环

[GitHub - HKUST-Aerial-Robotics/VINS-Fusion: An optimization-based multi-sensor state estimator]

图 VINS Fusion整体框架

[Vins-Fusion整体框架，数据流分析_vins fusion代码框架_云端舞步的博客-CSDN博客]

整体框架分为三个节点vins_estimator，loop_fusion，global_fusion。

vins_estimator状态估计，主要完成：1.前端，基于LK光流法的特征点跟踪函数featureTracker、IMU预积分得到当前帧位姿processIMU、三角化得到路标点(特征点位置)trianglate；2.后端优化optimization，滑窗优化位姿，位置此外还会优化零偏、外参、相机与imu时差，滑窗、边缘化slideWindow。3.初始化，完成位姿、位置、零偏初始化计算，也相当于要走一遍优化过程。

loop_fusion主要作用：利用词袋模型进行图像的回环检测。回环检测的任务主要是为了检测机器人是否到达之前相同的位置，并且消累计的误差。

随着路径的不断延伸，机器人在建图过程中会存在一些累计误差，除了利用局部优化、全局优化等来调整之外，还可以利用回环检测来优化位姿。回环检测，又称闭环检测，是指机器人识别曾到达某场景，使得地图闭环的能力。说的简单点，就是机器人在左转一下，右转一下建图的时候能意识到某个地方是“我”曾经来过的，然后把此刻生成的地图与刚刚生成的地图做匹配。

在fusion中，VIO估计完全独立于回环检测的结果。即回环检测的全局估计会受到VIO的影响，但是VIO不受全局估计的影响。

[https://blog.csdn.net/huanghaihui_123/article/details/87357488]

学习资料：

官方论文：[VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator]

前后端详解：[https://mp.weixin.qq.com/s/hoPDnZhT7ltkKib6mqSTcA]

学习笔记：[https://blog.csdn.net/m0_46182398/article/details/136439785]

回环检测：[VINS_Fusion回环检测学习笔记 (xjx100.cn)]

3. 相机内外参数

简介：从相机坐标系转换到像素坐标系中，相机内参的作用；

从世界坐标系转换到相机坐标系中，相机外参的作用。

[一文带你搞懂相机内参外参(Intrinsics & Extrinsics) - 知乎 (zhihu.com)]

对于realsense d435i而言，包含两个红外相机、红外发射器、RGB相机和IMU四个模块。显然四个传感器的空间位置是不同的，我们在处理图像和IMU数据时需要将这些数据都放在统一的坐标系上去。

外参：比如我们用d435i运行vins，我们处理的图像和IMU数据都需要放在同一个坐标系下，因此我们需要标定IMU相对RGB相机的空间位置（包括旋转和位移）。

内参：另外，相机固有参数比如焦距、畸变参数等以及IMU的零偏和scale系数等都需要提前知道。

[realsense d435i标定imu与camera (taodudu.cc)]

相机标定：vins自带的参数估计标定就简单有效。

4. EVO库

EVO库是一个开源库、一个很好的测评工具，它可以根据时间戳将轨迹进行对齐，同时可以将不同尺度的轨迹按照你指定的标准轨迹进行拉伸对齐，并可以算出均方差等评定参数，用于测评slam算法性能。核心功能是能够绘制相机的轨迹，或评估估计轨迹与真值的误差。支持多种数据集的轨迹格式（TUM、KITTI、EuRoC MAV、ROS的bag），同时支持这些数据格式之间进行相互转换。

有关VINS_Fusion的evo测试-CSDN博客

二、 指令

(根据自己的代码名称修改）

1. roslaunch realsense2_camera rs_camera_vins.launch
//开启相机

2. roslaunch vins vins_rviz.launch·
//开启vins所需的rviz  rviz：三维可视化平台

3. rosrun vins vins_node ~/catkin_opencv/src/VINS-Fusion-gpu/config/realsense_ d435i/realsense_stereo_imu_config.yaml
//开启d435i立体相机（结合imu）配置参数下的vins节点
//VINS-Fusion-gpu是具有GPU加速功能的版本,可以在Nvidia TX2上实时运行。

4. rosrun loop_fusion loop_fusion_node ~/catkin_opencv/src/VINS-Fusion-gpu/ config/realsense_d435i/realsense_stereo_imu_config.yaml
//开启回环检测优化

三、理论相关

1. 坐标系（无人机）

1.1 各种环境下的坐标系规定

px4使用的世界、本地坐标系为NED（北东地），机体坐标系为FRD（前右下）。

mavros中常使用的世界、本地坐标系为ENU（东北天），机体坐标系为FLU（前左上）。

Nokov动捕坐标系与mavros相似，常使用的世界、本地坐标系为ENU（东北天），机体坐标系为FLU（前左上）。

题主目前Vins世界、本地坐标系为ENU（东北天），机体右前上。（和imu坐标系方向相关，可能会有不同！）

（世界、本地坐标系为ENU（东北天）与机体坐标系为FLU（前左上）可以看作直接对应的，也即初始状态下是可重合的。）

1.2 是否涉及坐标系转换？

-- mavros方面，一些setpoint等指令，mavros会自动将输入的ENU坐标值或者body系坐标值转换到NED坐标系中，再通过mavlink发送给飞控。mavros的local系坐标原点一般在起飞点。

也即使用set类指令，不必考虑到px4的坐标系转换。

-- vins方面，需要根据vins输出的位置、四元数信息编写处理节点，使其坐标系与ros下一般坐标系统一，便于确认方位与控制设计。

[https://blog.csdn.net/z1872385/article/details/128670710]