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 （安装方式见上一期内容）
set(OpenCV_DIR "~/Documents/opencv-4.6.0/build")
（2）修改 loop_fusion/CMakeLists.txt 文件
注释一行
#include(/home/dji/opencv/build/OpenCVConfig.cmake)设置 OpenCV 路径为带有 GPU 加速的 opencv 4.6.0 （安装方式见上一期内容）
set(OpenCV_DIR "~/Documents/opencv-4.6.0/build")
保存并退出，最后进入 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并输入以下内容
完成后打开超级终端，创建三个终端，并 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.7432098388672right.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.7432098388672realsense_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 文件就配置完成了
接下来打开超级终端，创建四个终端，并 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 [email protected]输入密码后进入 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 的运行就彻底完成了！