x86_64架构的VINS-fusion-GPU部署
x86_64架构的VINS-fusion-GPU部署
1. 环境配置（Ubuntu 18.04）
（0）CUDA 10.2 安装
由于笔记本的GPU太老（GeForce 840M），只能使用较低版本的 CUDA，但是也能有个好处就是能够同时兼顾 ARM 架构的 GPU 部署。
1.0.0 卸载原来的版本CUDA-11.4
sudo apt remove cuda
sudo apt autoremove
sudo apt remove cuda*
进入到安装目录下，删除剩余的安装包文件。
cd /usr/local/
sudo rm -r cuda-11.4
查看剩余的残留文件
sudo dpkg -l |grep cuda
rc cuda-cudart-10-2 10.2.89-1 amd64 CUDA Runtime native Libraries
rc cuda-cudart-dev-10-2 10.2.89-1 amd64 CUDA Runtime native dev links, headers
rc cuda-cufft-10-2 10.2.89-1 amd64 CUFFT native runtime libraries
rc cuda-cupti-10-2 10.2.89-1 amd64 CUDA profiling tools runtime libs.
rc cuda-curand-10-2 10.2.89-1 amd64 CURAND native runtime libraries
rc cuda-cusolver-10-2 10.2.89-1 amd64 CUDA solver native runtime libraries
rc cuda-cusparse-10-2 10.2.89-1 amd64 CUSPARSE native runtime libraries
rc cuda-npp-10-2 10.2.89-1 amd64 NPP native runtime libraries
rc cuda-nvcc-10-2 10.2.89-1 amd64 CUDA nvcc
rc cuda-nvgraph-10-2 10.2.89-1 amd64 NVGRAPH native runtime libraries
rc cuda-nvjpeg-10-2 10.2.89-1 amd64 NVJPEG native runtime libraries
rc cuda-nvprof-10-2 10.2.89-1 amd64 CUDA Profiler tools
rc cuda-nvrtc-10-2 10.2.89-1 amd64 NVRTC native runtime libraries
rc cuda-nvtx-10-2 10.2.89-1 amd64 NVIDIA Tools Extension
ii cuda-repo-ubuntu1804-10-2-local 10.2.2-1 amd64 cuda repository configuration files
ii cuda-repo-ubuntu1804-10-2-local-10.2.89-440.33.01 1.0-1 amd64 cuda repository configuration files
rc cuda-sanitizer-api-10-2 10.2.89-1 amd64 CUDA Sanitizer API
rc cuda-toolkit-10-2 10.2.89-1 amd64 CUDA Toolkit 10.2 meta-package
rc cuda-visual-tools-10-2 10.2.89-1 amd64 CUDA visual tools
卸载对应的残留，即可完成cuda版本的纯净卸载。
sudo dpkg -P 残留文件名 # 例如：
sudo dpkg -P cuda-visual-tools-11-4
1.0.1 安装合适的新驱动
在 windows 系统中有GeFroce 840M 的驱动和 Cuda-Toolkit-10.2 工具包：
# 型号GeFroce 840M 的 GPU 驱动
460.89-notebook-win8-win7-64bit-international-whql.exe
474.11-notebook-win8-win7-64bit-international-whql.exe
GeForce_Experience_v3.26.0.160_requires_win10.exe
# Cuda-Toolkit-10.2 工具包及其补丁
cuda_10.2.89_441.22_windows.exe
cuda_10.2.1_win10.exe
cuda_10.2.2_win10.exe
以及和 pytorch、python 等对应的 CUDA 和 cudnn-tools 等安装包。为了在 pytorch中使用 GPU 的安装包。
torchvision-0.9.1-py38_cu102.tar.bz2 # 0.9.1, 0.8.1 二种任选一
torchvision-0.8.1-py38_cu102.tar.bz2 # 0.9.1, 0.8.1 二种任选一
pytorch-1.8.0-py3.8_cuda10.2_cudnn7_0.tar.bz2 # py3.6, py3.8, py3.9 三种任选一
pytorch-1.8.0-py3.9_cuda10.2_cudnn7_0.tar.bz2 # py3.6, py3.8, py3.9 三种任选一
pytorch-1.8.0-py3.6_cuda10.2_cudnn7_0.tar.bz2 # py3.6, py3.8, py3.9 三种任选一
首先，安装 cuda-toolkit-10-2 工具包：
CUDA-Toolkit-10.2 的下载位置为：https://developer.nvidia.com/cuda-toolkit-archive
或者https://developer.nvidia.com/cuda-10.2-download-archive
对应的安装指令为：
Base Installer Installation Instructions:
cd ~/CUDA-Toolkit-10.2
wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu1804/x86_64/cuda-ubuntu1804.pin
sudo mv cuda-ubuntu1804.pin /etc/apt/preferences.d/cuda-repository-pin-600
下载 deb 包
wget https://developer.download.nvidia.com/compute/cuda/10.2/Prod/local_installers/cuda-repo-ubuntu1804-10-2-local-10.2.89-440.33.01_1.0-1_amd64.deb
wget https://developer.download.nvidia.com/compute/cuda/10.2/Prod/patches/2/cuda-repo-ubuntu1804-10-2-local_10.2.1-1_amd64.deb
wget https://developer.download.nvidia.com/compute/cuda/10.2/Prod/patches/2/cuda-repo-ubuntu1804-10-2-local_10.2.2-1_amd64.deb
# 安装 deb 包
sudo dpkg -i cuda-repo-ubuntu1804-10-2-local-10.2.89-440.33.01_1.0-1_amd64.deb
sudo dpkg -i cuda-repo-ubuntu1804-10-2-local_10.2.1-1_amd64.deb
sudo dpkg -i cuda-repo-ubuntu1804-10-2-local_10.2.2-1_amd64.deb
# 增加 密钥然后 安装 CUDA（连带的把 cudnn 和 nvidia驱动也一并安装了）
sudo apt-key add /var/cuda-repo-10-2-local-10.2.89-440.33.01/7fa2af80.pub
sudo apt update
sudo apt -y install cuda
# 重启系统
sudo reboot
以及两个补丁包：
Patch 1 (Released Aug 26, 2020) 和 Patch 2 (Released Nov 17, 2020)。
或者使用以下命令（没有测试过，不保证成功率）。
sudo apt-get update
sudo apt-get install cuda-toolkit-10-2
安装好之后，在 .bashrc中配置环境变量。source ~/.bashrc之后，nvcc -- version 即可查看cuda版本。
export PATH=/usr/local/cuda-10.2/bin${PATH:+:${PATH}}
export LD_LIBRARY_PATH=/usr/local/cuda-10.2/lib64${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}
export CUDA_ROOT=/usr/local/cuda
运行nvidia-smi 即可查看GPU的驱动版本、CUDA的版本以及 GPU 的使用情况。
Mon Jan 2 19:41:44 2023
+-----------------------------------------------------------------------------+
| NVIDIA-SMI 440.33.01 Driver Version: 440.33.01 CUDA Version: 10.2 |
|-------------------------------+----------------------+----------------------+
| GPU Name Persistence-M| Bus-Id Disp.A | Volatile Uncorr. ECC |
| Fan Temp Perf Pwr:Usage/Cap| Memory-Usage | GPU-Util Compute M. |
|===============================+======================+======================|
| 0 GeForce 840M On | 00000000:03:00.0 Off | N/A |
| N/A 39C P8 N/A / N/A | 189MiB / 2004MiB | 7% Default |
+-------------------------------+----------------------+----------------------+
+-----------------------------------------------------------------------------+
| Processes: GPU Memory |
| GPU PID Type Process name Usage |
|=============================================================================|
| 0 913 G /usr/lib/xorg/Xorg 15MiB |
| 0 983 G /usr/bin/gnome-shell 46MiB |
| 0 1293 G /usr/lib/xorg/Xorg 55MiB |
| 0 1448 G /usr/bin/gnome-shell 67MiB |
+-----------------------------------------------------------------------------+
如果是双显卡，则还可以设法配置日常系统不使用Nvidia的显卡，例如本例程的GeForce 840M。
方法一：使用nvidia-settings来切换。
方法二：使用下面的命令来切换。
sudo prime-select nvidia # 切换回独显 Nidia GeForce 840M
sudo reboot
或者
sudo prime-select intel # 切换回集显 Intel HD Graphics 550 (BDW GT2)
sudo reboot
（1）Eigen 安装与配置
设置编译目录为 eigen-build，默认安装路径 /usr/local。如果要修改，可以安装并使用cmake-gui来设置。
# Remove pre-built Eigen
sudo apt-get remove libeigen3-dev
cd ~/Downloads/
wget -O eigen-3.3.7.zip https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip #check version
unzip eigen-3.3.7.zip
mkdir eigen-build && cd eigen-build
cmake ../eigen-3.3.7/ && sudo make install
pkg-config --modversion eigen3 # Check Eigen Version
（2）Ceres solver 安装与配置
设置编译目录为 ceres-bin，设置安装路径 /Downloads/ceres-solver。如果要修改，可以安装并使用cmake-gui来设置。
cd ~/Downloads/
sudo apt-get install -y cmake libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev
wget http://ceres-solver.org/ceres-solver-1.14.0.tar.gz
tar zxf ceres-solver-1.14.0.tar.gz
mkdir ceres-bin
mkdir ceres-solver && cd ceres-bin
cmake ../ceres-solver-1.14.0 -DEXPORT_BUILD_DIR=ON -DCMAKE_INSTALL_PREFIX="../ceres-solver"
#good for build without being root privileged and at wanted directory
make -j3 # 6 : number of cores
make install
# 可如下进行测试：
bin/simple_bundle_adjuster ../ceres-solver-1.14.0/data/problem-16-22106-pre.txt # to check version
显示的输出结果为：
（3） OpenCV 安装与配置
关于 OpenCV 的安装需要特别谨慎，不建议频繁删除以前的库，因为多个项目使用的版本可能不同，所以可以将常用的版本安装在 /usr/local下，将其余版本安装在自己Download 或者 home 下，并设置 CUDA_ARCH_BIN=5.0——与机器上的显卡型号有关，可以去Nvidia官网查看确认。
如果需要删除原来的版本并重修安装相应的依赖：
# remove prebuilt opencv
sudo apt purge libopencv* python-opencv
sudo apt update
sudo apt install -y build-essential pkg-config
## libeigen3-dev # recommend to build from source
sudo apt install -y cmake libavcodec-dev libavformat-dev libavutil-dev \
libglew-dev libgtk2.0-dev libgtk-3-dev libjpeg-dev libpng-dev libpostproc-dev \
libswscale-dev libtbb-dev libtiff5-dev libv4l-dev libxvidcore-dev \
libx264-dev qt5-default zlib1g-dev libgl1 libglvnd-dev pkg-config \
libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev mesa-utils
sudo apt-get install python2.7-dev python3-dev python-numpy python3-numpy
如果是 arm 架构的系统则需要对 /usr/lib/aarch64-linux-gnu/ 和 /usr/local/cuda/include/cuda_gl_interop.h 作一点修改。
# To fix OpenGL related compilation problems
cd /usr/lib/aarch64-linux-gnu/
sudo ln -sf libGL.so.1.0.0 libGL.so
sudo vim /usr/local/cuda/include/cuda_gl_interop.h
# Comment (line #62~68) of cuda_gl_interop.h
//#if defined(__arm__) || defined(__aarch64__)
//#ifndef GL_VERSION
//#error Please include the appropriate gl headers before including cuda_gl_interop.h
//#endif
//#else
#include 安装新的版本（因为 VINS-FUSION-GPU 版本需要支持CUDA 的 OpenCV，所以选择3.4.1 版本）安装opencv-3.4.1的教程可以参见《opencv-3.4.1-x86编译安装 – 超详细》。
设置编译目录为 .\build，设置安装路径 /usr/local。如果要修改，可以安装并使用cmake-gui来设置。根据 GPU 的型号来设置 CUDA_ARCH_BIN，我的显卡是GeForce 840M，由官网链接查到为 5.0。
# Then once linking is done, go to Downloads to begin opencv installation
cd ~/Downloads/
wget -O opencv.zip https://github.com/opencv/opencv/archive/3.4.1.zip # check version
unzip opencv.zip
cd opencv-3.4.1/ && mkdir build && cd build
cmake -D CMAKE_BUILD_TYPE=RELEASE \
-D CMAKE_INSTALL_PREFIX=/usr/local \
-D WITH_CUDA=ON \
-D CUDA_ARCH_BIN=5.0 \
-D CUDA_ARCH_PTX="" \
-D ENABLE_FAST_MATH=ON \
-D CUDA_FAST_MATH=ON \
-D WITH_CUBLAS=ON \
-D WITH_LIBV4L=ON \
-D WITH_GSTREAMER=ON \
-D WITH_GSTREAMER_0_10=OFF \
-D WITH_QT=ON \
-D WITH_OPENGL=ON \
-D CUDA_NVCC_FLAGS="--expt-relaxed-constexpr" \
-D WITH_TBB=ON \
../
make # 建议使用单核编译，以免出现意外错误 running in single core is good to resolve the compilation issues
sudo make install
cd ../../ && sudo rm -rf opencv-3.4.1 # optional (can save 10GB Disk Space)
pkg-config --modversion opencv # Check opencv Version
OpenCV 在Jeston NX上的安装比较慢，差不多需要1个小时。OpenCV 安装好以后，需要进一步编译 cv_bridge ( cv_bridge 是在ROS 图像消息和 OpenCV 图像之间进行转换的一个功能包)。
（4）cv_bridge的编译 （ARM架构适用）
cv_bridge 的安装需要下载对应的版本，由于 Jeston NX 是基于 Ubuntu 18.04 的 ARM 系统，因此下载 melodic 版本的 cv_bridge。首先，通过以下连接进入下载页面：
https://github.com/ros-perception/vision_opencv
选择下载 melodic 版本到 catkin_ws/src 目录下，并安装：
gedit vision_opencv/cv_bridge/CMakeLists.txt
# Edit OpenCV PATHS in CMakeLists and include cmake file
# /usr/local/share/OpenCV 选择自己安装的OpenCV对应路径
find_package(OpenCV 3 REQUIRED PATHS /usr/local/share/OpenCV NO_DEFAULT_PATH
COMPONENTS
opencv_core
opencv_imgproc
opencv_imgcodecs
CONFIG
)
include(/usr/local/share/OpenCV/OpenCVConfig.cmake) #under catkin_python_setup()
# Save and close CMakeLists
# Build the package
cd .. && catkin_make
（5） ROS 安装与配置
参考ros.org 网站自行安装。此处与教程（vins-fusion-gpu-tx2-nano）略有差异，我直接使用了ros内置的cv_bridge。因为涉及到源码安装opencv，cv_bridgeConfig.cmake中include的地址要做修改，否则后续编译报错，可以参考这一篇文章。
3. VINS-Fusion-GPU 安装及编译
具体参数可看源文档 https://github.com/pjrambo/VINS-Fusion-gpu。
具体操作如下：
（1）下载和更改 cmake 文件
cd ~/catkin_ws/src && git clone https://github.com/pjrambo/VINS-Fusion-gpu #GPU
sudo apt-get install ros-melodic-tf
sudo apt-get install ros-melodic-image-transport
sudo apt-get install ros-melodic-rviz
下载完之后，需要更改vins_estimator/CMakeLists.txt第20行，把OpenCV的路径改成自己的安装路径。
#
# Edit CMakeLists.txt for loop_fusion and vins_estimator
cd ~/catkin_ws/src/VINS-Fusion-gpu/loop_fusion && gedit CMakeLists.txt
需要更改的还有 loop_fusion/CMakeLists.txt，同样也需要更改路径
##For loop_fusion : line 19
#find_package(OpenCV)
include(/usr/local/share/OpenCV/OpenCVConfig.cmake)
cd ~/catkin_ws/src/VINS-Fusion-gpu/vins_estimator && gedit CMakeLists.txt
##For vins_estimator : line 20
#find_package(OpenCV REQUIRED)
include(/usr/local/share/OpenCV/OpenCVConfig.cmake)
更改完之后，开始编译。
cd ~/catkin_ws/
source devel/setup.bash
catkin_make
如果OpenCV 和 CUDA环境安装不对，这里就会报错。
（2） 更改配置参数
在 config 配置文件中，有两个 gpu 加速 参数。
use_gpu: 0 # 0 for off, 1 for on
use_gpu_acc_flow: 0 # 0 for off, 1 for on
当 GPU 资源受限或者程序需要使用 GPU 进行其他计算时，可以设置为：
use_gpu: 1
use_gpu_acc_flow: 0
否者，可以设置为：
use_gpu: 1
use_gpu_acc_flow: 1
在 Jeston TX2 中后者可能会占用 20% 的GPU 使用率。
IMU的数据需要由事先的测试试验标定得到。
需要注意在线估计IMU与相机之间的时间差，虽然说Intel D435i已经做好硬件同步，但是用Kalibr标定出来还是会有1.5ms左右的时间误差。
（3） 启动VINS
前期的参数配置工作完成后，我们就可以启动 VINS
3-1 启动D435i
修改D435i的发布频率，建议为15Hz，这样NX上不会有较大的延迟。路径还是你自己安装 realsense_ros 的路径。
启动 D435i方式：realsense2_camera rs_camera.launch
需要查看下 D435i 是否发布 IMU 数据，主要是X、Y、Z三个轴的线速度。
rostopic echo /camera/imu
3-1 启动VINS
启动方式：rosrun vins vins_node + D435i配置文件的路径 。
例如：
rosrun vins vins_node ~/vins_gpu/src/VINS-Fusion-gpu-master/config/realsense/realsense1.yaml
可以显示出当前位姿信息。
然后启动RVIZ：
roslaunch vins vins_rviz.launch
启动完之后，可以拿着飞机走两圈，看看定位精度以及延时情况。如果没有问题就可以接入Prometheus中了。
4. 数据测试
4.1 公开数据集测试
以EuRoC（Stereo cameras + IMU）为例，
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
数据可以通过这个链接在百度网盘下载到。
4.2 测试设备（D435i+P450）
在配置参数文件中加入以下参数，使得GPU起作用。
imu_topic: "/mavris/imu/data_raw"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
IMU和相机外参标定可以通过VINS实时在线标定或者使用Kalibr工具包进行标定。
5. 接入Prometheus
5.1 修改px4_pos_estimator.cpp
在Prometheus/Modules/control/src文件夹中修改文件px4_pos_estimator.cpp。
主要修改内容如下：回调函数中，主要是VINS发布数据格式为YXZ，而Prometheus需要ENU，所以这里做一个变换。VINS的四元数虚部在前，这里也需要调整下。
5.2 修改p450_vio的启动文件
在 Amov_lab/src/P450_expriments/launch_basic 中找到 p450_vio的启动文件 p450_vio_onboard.launch。修改位姿数据来源，因为我们不再依赖T265。
5.3 修改订阅话题
最后，需要修改主函数里面的订阅话题。
6. 启动VINS和Prometheus配置文件
roslaunch realsense2_camera rs_camera.launch
rosrun vins vins_node + D435i配置文件路径 # 具体应该是在VINS 包下面
roslaunch p230_expriments p230_vio_onboard.launch
查看下 rqt_graph，可以看到 VINS 发布的位姿已经被px4_pos_estimator 订阅，类似于T265发布的 odom/sample，通信链路正常。
我们还可以看到终端可以正确打印位姿数据，即代表VINS接入Prometheus成功。
至此，我们已经成功把 VINS 接入到 Prometheus 中。
文献参考
1、无人机自主导航（ARM架构的vins-fusion-GPU部署）
2、安装环境OpenCV
3、调内外参数
4、带你具体部署VINS_FUSION_GPU版本
5、NVIDIA Jetson Xavier NX部署VINS-fusion-GPU版本
6、VINS-Fusion-GPU-TX2-nano的github技术说明