`x86_64`架构的`VINS-fusion-GPU`部署

1. 环境配置（`Ubuntu 18.04`）

（0）`CUDA 10.2` 安装

由于笔记本的GPU太老（GeForce 840M），只能使用较低版本的 CUDA，但是也能有个好处就是能够同时兼顾 ARM 架构的 GPU 部署。

1.0.1 安装合适的新驱动

在 windows 系统中有驱动：

460.89-notebook-win8-win7-64bit-international-whql.exe
474.11-notebook-win8-win7-64bit-international-whql.exe
cuda_10.2.1_win10.exe
cuda_10.2.2_win10.exe
cuda_10.2.89_441.22_windows.exe
GeForce_Experience_v3.26.0.160_requires_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 工具包：

sudo apt-get update
sudo apt-get install cuda-toolkit-10-2

安装好之后，在 .bashrc中配置环境变量。source之后，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

（1）`Eigen` 安装与配置

#  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` 安装与配置

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 solver && cd ceres-bin
cmake ../ceres-solver-1.14.0 -DEXPORT_BUILD_DIR=ON -DCMAKE_INSTALL_PREFIX="../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 下。
如果需要删除原来的版本，执行以下命令：

# remove prebuilt opencv
sudo apt-get purge libopencv* python-opencv 
sudo apt-get update
sudo apt-get install -y build-essential pkg-config
 
## libeigen3-dev # recommend to build from source
 
sudo apt-get 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

# 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 <GL/gl.h>
//#endif

安装新的版本（因为 VINS-FUSION-GPU 版本需要支持CUDA 的 OpenCV，所以选择3.4.1 版本）

# 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=6.2 \
        -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技术说明