一、前言
由于 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博客
将超级终端分为四栏,然后 Alt + A 全选所有终端,输入以下指令使 VINS-Fusion-gpu 工作空间对所有终端生效
source ~/catkin_ws/src/vins-fusion-gpu/devel/setup.bash
然后通过快捷键 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 的运行就彻底完成了!