Jetson Orin NX 开发指南（6）: VINS-Fusion-gpu 的编译和运行

Jetson Orin NX 开发指南（6）: VINS-Fusion-gpu 的编译和运行
一、前言

由于 Jetson 系列的开发板 CPU 性能不是很好，因此在处理图像数据时往往需要 GPU 加速，而 VINS-Fusion 是针对同步定位与建图（SLAM）问题中十分出色的视觉算法，但是其在图像处理过程中资源消耗较大，仅仅依靠 Jetson 开发板的 CPU 往往很难实现事实效果。

本文在此背景下介绍如何配置带有 GPU 加速的 VINS-Fusion，主要参考以下文献

https://github.com/pjrambo/VINS-Fusion-gpu

Ubuntu 20.04 配置 VINS-Fusion-gpu + OpenCV 4.6.0-CSDN博客

并且需要一些必要的准备工作，如 CUDA 的安装，带有 GPU 加速的 OpenCV 的安装，cv_bridge 功能包的安装，这些工作在上一期内容中已经介绍，详见

Jetson Orin NX 开发指南（5）: 安装 OpenCV 4.6.0 并配置 CUDA 以支持 GPU 加速-CSDN博客

接下来我们开始介绍如何在 Jetson Orin NX 开发板上运行 VINS-Fusion-gpu

二、编译安装 ceres-solver-1.14.0

首先安装依赖
```
sudo apt-get install liblapack-dev libsuitesparse-dev libgflags-dev libgoogle-glog-dev libgtest-dev libcxsparse3 -y
```
然后将安装包下载在 ~/Documents/ 目录下并解压
```
wget ceres-solver.org/ceres-solver-1.14.0.tar.gz
tar -zxvf ceres-solver-1.14.0.tar.gz
```
进入 ceres-solver-1.14.0 文件夹
```
cd ~/Documents/ceres-solver-1.14.0
```
创建 build 并编译生成 Makefile 文件
```
mkdir build & cd build
cmake ..
```
最终编译并安装 ceres-solver-1.14.0
```
sudo make install -j8
```
最终结果如下

至此，ceres-solver-1.14.0 就编译安装完成了！

三、编译 VINS-Fusion-gpu

首先创建 VINS-Fusion-gpu 工作空间，
```
mkdir -p ~/catkin_ws/src/vins-fusion-gpu/src/
cd ~/catkin_ws/src/vins-fusion-gpu/src/
```
然后从 Github 上下载 VINS-Fusion-gpu 源码
```
git clone https://github.com/pjrambo/VINS-Fusion-gpu.git
```
下载完成后进入 VINS-Fusion-gpu 文件夹

（1）修改 vins_estimator/CMakeLists.txt 文件

注释一行
```
#include(/home/dji/opencv/build/OpenCVConfig.cmake)
```
设置 OpenCV 路径为带有 GPU 加速的 opencv 4.6.0 （安装方式见上一期内容）
```
include("~/Documents/opencv-4.6.0/build/OpenCVConfig.cmake")
```
（2）修改 loop_fusion/CMakeLists.txt 文件

注释一行
```
#include(/home/dji/opencv/build/OpenCVConfig.cmake)
```
设置 OpenCV 路径为带有 GPU 加速的 opencv 4.6.0 （安装方式见上一期内容）
```
include("~/Documents/opencv-4.6.0/build/OpenCVConfig.cmake")
```
保存并退出，最后进入 VINS-Fusion-gpu 文件夹
```
cd ~/catkin_ws/src/vins-fusion-gpu/
```
接下来开始编译，终端输入
```
catkin_make
```
出现报错

错误内容举例：

（1）编译报错 error: ‘CV_FONT_HERSHEY_SIMPLEX’ was not declared in this scope

（2）编译报错 error: ‘CV_LOAD_IMAGE_GRAYSCALE’ was not declared in this scope

（3）编译报错 error: ‘CV_BGR2GRAY’ was not declared in this scope

解决办法：

进入 VINS-Fusion-gpu 文件夹下
```
cd ~/catkin_ws/src/vins-fusion-gpu/src/VINS-Fusion-gpu
```
然后输入以下内容
```
sed -i 's/CV_FONT_HERSHEY_SIMPLEX/cv::FONT_HERSHEY_SIMPLEX/g' `grep CV_FONT_HERSHEY_SIMPLEX -rl ./`
sed -i 's/CV_LOAD_IMAGE_GRAYSCALE/cv::IMREAD_GRAYSCALE/g' `grep CV_LOAD_IMAGE_GRAYSCALE -rl ./`
sed -i 's/CV_BGR2GRAY/cv::COLOR_BGR2GRAY/g' `grep CV_BGR2GRAY -rl ./`
sed -i 's/CV_RGB2GRAY/cv::COLOR_RGB2GRAY/g' `grep CV_RGB2GRAY -rl ./`
sed -i 's/CV_GRAY2RGB/cv::COLOR_GRAY2RGB/g' `grep CV_GRAY2RGB -rl ./`
sed -i 's/CV_GRAY2BGR/cv::COLOR_GRAY2BGR/g' `grep CV_GRAY2BGR -rl ./`
sed -i 's/CV_CALIB_CB_ADAPTIVE_THRESH/cv::CALIB_CB_ADAPTIVE_THRESH/g' `grep CV_CALIB_CB_ADAPTIVE_THRESH -rl ./`
sed -i 's/CV_CALIB_CB_NORMALIZE_IMAGE/cv::CALIB_CB_NORMALIZE_IMAGE/g' `grep CV_CALIB_CB_NORMALIZE_IMAGE -rl ./`
sed -i 's/CV_CALIB_CB_FILTER_QUADS/cv::CALIB_CB_FILTER_QUADS/g' `grep CV_CALIB_CB_FILTER_QUADS -rl ./`
sed -i 's/CV_CALIB_CB_FAST_CHECK/cv::CALIB_CB_FAST_CHECK/g' `grep CV_CALIB_CB_FAST_CHECK -rl ./`
sed -i 's/CV_ADAPTIVE_THRESH_MEAN_C/cv::ADAPTIVE_THRESH_MEAN_C/g' `grep CV_ADAPTIVE_THRESH_MEAN_C -rl ./`
sed -i 's/CV_THRESH_BINARY/cv::THRESH_BINARY/g' `grep CV_THRESH_BINARY -rl ./`
sed -i 's/CV_SHAPE_CROSS/cv::MORPH_CROSS/g' `grep CV_SHAPE_CROSS -rl ./`
sed -i 's/CV_SHAPE_RECT/cv::MORPH_RECT/g' `grep CV_SHAPE_RECT -rl ./`
sed -i 's/CV_TERMCRIT_EPS/cv::TermCriteria::EPS/g' `grep CV_TERMCRIT_EPS -rl ./`
sed -i 's/CV_TERMCRIT_ITER/cv::TermCriteria::MAX_ITER/g' `grep CV_TERMCRIT_ITER -rl ./`
sed -i 's/CV_RETR_CCOMP/cv::RETR_CCOMP/g' `grep CV_RETR_CCOMP -rl ./`
sed -i 's/CV_CHAIN_APPROX_SIMPLE/cv::CHAIN_APPROX_SIMPLE/g' `grep CV_CHAIN_APPROX_SIMPLE -rl ./`
sed -i 's/CV_AA/cv::LINE_AA/g' `grep CV_AA -rl ./`
sed -i 's/CV_LOAD_IMAGE_UNCHANGED/cv::IMREAD_UNCHANGED/g' `grep CV_LOAD_IMAGE_UNCHANGED -rl ./`
sed -i 's/CV_MINMAX/cv::NORM_MINMAX/g' `grep CV_MINMAX -rl ./`
```
具体过程如下

其原理解释详见

Docker 制作各 ROS 版本 VINS-Fusion 镜像_docker ros 镜像_想要个小姑娘的博客-CSDN博客

简要总结：

VINS-Fusion-gpu 是在 Ubuntu 18.04 环境下写的，其对应的 ROS 版本为 melodic，对应的 OpenCV 是 3.2.0，

而 Jetson Orin NX 对应的是 Ubuntu 20.04，其对应的 ROS 版本为 noetic，对应的 OpenCV 版本为 4.2.0，

报错的关键原因就是 OpenCV 4 系列和 OpenCV 3 系列有一些变量的名称发生了改变，因此我们这里只要将相应的变量名称进行修正，就能顺利通过编译！（OpenCV 4.6.0 和 OpenCV 4.2.0 差别不大）

上面的语句通过 sed 命令和 grep 正则表达式的方式一键修改所有变量名称！

修改完成后进入工作空间，并编译
```
cd ~/catkin_ws/src/vins-fusion-gpu/
catkin_make
```
最终显示如下结果

这表明 VINS-Fusion-gpu 通过编译！

四、运行 VINS-Fusion-gpu

接下来我从测试数据集和使用真实相机数据两个方面来运行VINS-Fusion-gpu

4.1 数据集介绍

测试数据集一般选择 EuRoc 和 KITTI 数据集，其下载地址如下

kmavvisualinertialdatasets – ASL Datasets

The KITTI Vision Benchmark Suite

根据自己需要下载相应数据集

4.2 运行数据集

4.2.1 通过 rosrun 指令运行数据集

这里我们以 euroc 数据集为例，首先创建数据集存储路径，
```
mkdir -p ~/catkin_ws/datasets/euroc/
cd ~/catkin_ws/datasets/euroc/
```
这里我下载了 MH_01_easy 数据集，接下来我们运行测试，

首先打开超级终端（因为需要同时启动多个节点），安装方式参考

Jetson Orin NX 开发指南（2）: 基本环境配置_想要个小姑娘的博客-CSDN博客

将超级终端分为四栏，然后 ctrl + alt + A 全选所有终端，输入以下指令使 VINS-Fusion-gpu 工作空间对所有终端生效
```
source ~/catkin_ws/src/vins-fusion-gpu/devel/setup.bash
```
然后通过快捷键 ctrl + alt + O 取消全选，

接下来在第一个终端启动 rviz 可视化界面
```
roslaunch vins vins_rviz.launch
```
在第二个终端启动 vins 节点
```
rosrun vins vins_node ~/catkin_ws/src/vins-fusion-gpu/src/VINS-Fusion-gpu/config/euroc/euroc_stereo_imu_config.yaml
```
在第三个终端启动 loop_fusion 节点，进行回环检测（可选）
```
rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/vins-fusion-gpu/src/VINS-Fusion-gpu/config/euroc/euroc_stereo_imu_config.yaml
```
在第四个终端运行数据集
```
rosbag play ~/catkin_ws/datasets/euroc/MH_01_easy.bag
```
如下所示

逐个运行可以得到如下结果

4.2.2 通过 launch 文件运行数据集

也可以通过配置 launch 文件来运行数据集，接下来我们配置 VINS-Fusion-gpu 的 launch 文件，在 ~/catkin_ws/src/vins-fusion-gpu/src/VINS-Fusion-gpu/vins_estimator/launch/ 路径下创建一个 euroc.launch 文件
```
touch euroc.launch
```
并输入以下内容
```
<launch>
    <node name="vins_estimator" pkg="vins" type="vins_node" output="screen" args="$(find vins)/../config/euroc/euroc_stereo_imu_config.yaml" />
    <node name="loop_fusion" pkg="loop_fusion" type="loop_fusion_node" output="screen" args="$(find vins)/../config/euroc/euroc_stereo_imu_config.yaml" />
</launch>
```
完成后打开超级终端，创建三个终端，并 source 一下 VINS-Fusion-gpu 工作空间，全选后输入
```
source ~/catkin_ws/src/vins-fusion-gpu/devel/setup.bash
```
第一个终端输入
```
roslaunch vins vins_rviz.launch
```
第二个终端输入
```
roslaunch vins euroc.launch
```
第三个终端输入
```
rosbag play ~/catkin_ws/datasets/euroc/MH_01_easy.bag
```
如下所示

逐个执行可以得到如下结果

可以达到跟 rosrun 指令一样的效果。

4.3 运行 realsense 真实数据

由于 launch 文件启动的方式比较普遍且便捷，这里主要介绍如何通过 launch 文件读取真实数据（reaalsense d435i）运行 VINS-Fusion-gpu，但是这里需要注意的是 VINS-Fusion-gpu 的 config 文件夹中并没有 realsense 相机（realsense d435i）的配置文件，不过在 VINS-Fusion 源码中的 config 文件夹中有 realsense d435i 相机的配置文件，可以在其基础上修改。

4.3.1 添加 realsense d435i 配置文件

在 VINS-Fusion 的 github 网站上查看对应 realsense d435i 的配置文件，

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

在 ~/catkin_ws/src/vins-fusion-gpu/src/VINS-Fusion-gpu/config/realsense_d435i/ 路径下创建如下三个文件夹

其中 left.yaml 文件配置如下（相对于 VINS-Fuison 未做修改）
```
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0.0
   k2: 0.0
   p1: 0.0
   p2: 0.0
projection_parameters:
   fx: 385.7544860839844
   fy: 385.7544860839844
   cx: 323.1204833984375
   cy: 236.7432098388672
```
right.yaml 文件配置如下（相对于 VINS-Fuison 未做修改）
```
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0.0
   k2: 0.0
   p1: 0.0
   p2: 0.0
projection_parameters:
   fx: 385.7544860839844
   fy: 385.7544860839844
   cx: 323.1204833984375
   cy: 236.7432098388672
```
realsense_stereo_imu_config.yaml 文件配置如下
```
%YAML:1.0
 
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  
 
imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/dji/output/"
 
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
   
 
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
 
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -5.7586305857286746e-03, -4.0463318787729019e-03,
       9.9997523237933461e-01, 2.0329267950355900e-02,
       -9.9998287214160420e-01, -1.0224590553211677e-03,
       -5.7628118925283633e-03, 7.9325209639615653e-03,
       1.0457519809151661e-03, -9.9999129084997906e-01,
       -4.0403746097850135e-03, 2.8559824645148020e-03, 0., 0., 0., 1. ]
 
body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -1.0021770212322867e-03, 3.6313480322730518e-04,
       9.9999943188700535e-01, 1.5285779565991807e-02,
       -9.9999216342926500e-01, -3.8303422615924010e-03,
       -1.0007788055728661e-03, -5.2435791444330505e-02,
       3.8299766679101843e-03, -9.9999259827824449e-01,
       3.6697063849344680e-04, 8.6931302450199057e-03, 0., 0., 0., 1. ]
 
#Multiple thread support
multiple_thread: 1
use_gpu: 1
use_gpu_acc_flow: 1
 
#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 0           # publish tracking image as topic
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy
 
#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
 
#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.001         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805         # gravity magnitude
 
#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: 0.00                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
 
#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 0                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
```
相对于 VINS-Fuison 添加了两行，从而调用 GPU 加速算法运行

当然，其他相机的配置文件设置和上面的内容是一致的，因为这是 VINS-Fuion 源码中需要读取的参数的内容和格式，为了达到更高的精度，往往需要对相机进行标定，标定完成后修改配置文件中的内外参和时延，从而提高估计精度，标定的方法可以参考

d435i 相机和imu标定_d435i imu标定_想要个小姑娘的博客-CSDN博客

当然，在此标定可以在后面熟悉 VINS-Fusion 的使用之后在进行，由于我们这里只是介绍如何使用 VINS-Fusion-gpu 测试真实数据，因此标定可以不进行（也不推荐现在进行，很费时间）

4.3.2 通过 launch 文件运行 realsense 相机

首先必须安装 librealsense 和 realsense-ros，参考以下两片文章（分别针对 Ubuntu 系统和 Jetson 系列的系统）

Ubuntu 20.04 配置 realsense_ubuntu安装realsense2-CSDN博客

Jetson Orin NX 开发指南（4）: 安装 CUDA 和 Realsense_想要个小姑娘的博客-CSDN博客

安装完成后确保能够正常使用 ROS 启动 realsense 相机，并能够正确读取到相机数据和 IMU 数据，配置方式同样参考上面两篇文章。

接下来在 ~/catkin_ws/src/vins-fusion-gpu/src/VINS-Fusion-gpu/vins_estimator/launch/ 路径下创建一个 launch 文件
```
cd ~/catkin_ws/src/vins-fusion-gpu/src/VINS-Fusion-gpu/vins_estimator/launch/
touch realsense_d435i.launch
```
在 launch 文件中输入以下内容
```
<launch>
    <node name="vins_estimator" pkg="vins" type="vins_node" output="screen" args="$(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" />
    <node name="loop_fusion" pkg="loop_fusion" type="loop_fusion_node" output="screen" args="$(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" />
</launch>
```
这样 launch 文件就配置完成了

接下来打开超级终端，创建四个终端，并 source 一下 VINS-Fusion-gpu 工作空间，全选后输入
```
source ~/catkin_ws/src/vins-fusion-gpu/devel/setup.bash
```
第一个终端输入
```
roslaunch vins vins_rviz.launch
```
第二个终端输入
```
roslaunch vins realsense_d435i.launch
```
第三个终端输入
```
roslaunch realsense2_camera rs_camera.launch
```
如下所示

依次运行可以得到如下结果（这里暂时不放 rviz 的效果，因为需要标定相机之后才能有好的效果）

因为插入相机后数据量过大，鼠标会卡死不动，这时只需要把相机拔了鼠标就能恢复正常，这时可以在另外一台电脑通过 ssh 指令访问 Jetson Orin NX，终端输入
```
ssh onx001@172.101.10.106
```
输入密码后进入 Jetson Orin NX 系统，终端输入
```
jtop
```
显示如下内容

可以看到这里 vins_node 正在占用 GPU 资源，也就是说 VINS-Fusion-gpu 成功运行了

其中 Jetson Orin NX 的 ssh 和 jtop 的安装可以参考

Jetson Orin NX 开发指南（2）: 基本环境配置_想要个小姑娘的博客-CSDN博客

此外，需要注意 VINS-Fusion-gpu 的特征追踪图片的显示方式与 VINS-Fusion 有所不同，VINS-Fusion-gpu 是将特征追踪图像单独显示出来的，而在 VINS-Fusion 中，特征追踪图像是集成在 rviz 中显示的，如果需要将特征追踪的图像显示在 rviz 中，可以参考下文

Ubuntu 20.04 配置 VINS-Fusion-gpu + OpenCV 4.6.0-CSDN博客

最后附一张 VINS-Fusion-gpu 运行 realsense d435i 相机的节点图，

至此，VINS-Fusion-gpu 的运行就彻底完成了！
相关阅读:
第2章丨IRIS Global 使用多维存储
 实时从 httpd 的asscess_log 日志读取访问ip和根路径不在白名单内就iptables封堵
 React中组件通信01——props
豆瓣电影top250网页爬虫
 程序员如何利用周末提高自己？
面试题 04.02.最小高度数
 Java进阶——IO 流
 VB.NET媒体播放器PPT免费模板
 YOLOv8优化：独家创新（Partial_C_Detect）检测头结构创新，实现涨点 | 检测头新颖创新系列
 Linux：用户态与内核态
原文地址：https://blog.csdn.net/qq_44998513/article/details/133780129

一、前言

二、编译安装 ceres-solver-1.14.0

三、编译 VINS-Fusion-gpu

四、运行 VINS-Fusion-gpu

4.1 数据集介绍

4.2 运行数据集

4.2.1 通过 rosrun 指令运行数据集

4.2.2 通过 launch 文件运行数据集

4.3 运行 realsense 真实数据

4.3.1 添加 realsense d435i 配置文件

4.3.2 通过 launch 文件运行 realsense 相机