JetSon TX2上使用RealSense D435 (外加IMU芯片) 运行RTAB-Map与VINS-MONO的全流程记录

本周成功的在JetSon TX2上移植了Vins-Mono与RTAB-Map，并使用摄像头RealSense D435顺利跑通了这两个框架。中间遇到了各种各样神奇的问题，踩坑无数，现整理记录一下整体流程，便于后续查询与复现。

JetSon系列是Nvidia公司推出的面向人工智能领域的嵌入式平台，这块嵌入式板子的出现使得我们可以在移动设备上处理复杂数据。

一、在JetSon TX2上运行VINS-Mono

已有环境配置：我这里JetSon TX2上已经刷好了系统版本JetPack3.2.1。
具体刷机流程后面会深入学习一下，到时候再写一篇博客记录下来。

1、配置Vins-Mono的编译环境

在JetSon TX2上安装与编译Vins-Mono框架时，主要流程与Ubuntu 16.04系统下的搭建流程基本一致，具体可以参考本人之前的博客：搭建Vins-Mono环境与评估EuRoc定位精度、安装RealSense ZR300、D435驱动 中的安装流程。

需要安装的依赖项有：ROS Kinetic，OpenCV 3.3.1，Eigen 3.3.4，Ceres-Solver。
其中有两点与Ubuntu 16.04系统下不太一样的地方，需要留意一下：

（1）ROS Kinetic在JetSon TX2上的安装：

直接运行如下命令行安装ROS Kinetic：

mkdir -p ~/ROS
cd ~/ROS
git clone https://github.com/ncnynl/installROSTX2.git
 
# 打开脚本文件
cd installROSTX2
gedit installROS.sh 
#将ros-kinetic-ros-base，更改为：ros-kinetic-desktop-full

#安装
./installROS.sh

安装完，运行：roscore 查看是否安装成功。

（2）ceres-solve在JetSon TX2上的安装：

在TX2上安装ceres-solver时，如果先安装了Eigen 3.3.3的版本，则会出现以下错误：

– Found installed version of Eigen: /usr/lib/cmake/eigen3
– Found Eigen version 3.2.92: /usr/include/eigen3
CMake Error at CMakeLists.txt:240 (message):
– Ceres requires Eigen version >= 3.3.4 on aarch64. Detected version of
Eigen is: 3.2.92.

因此需要升级Eigen版本。在Eigen官网下载大于或等于3.3.4的版本，解压后使用cmake安装。

2、安装Realsense D435摄像头驱动与IMU驱动

由于Vins-Mono算法里融合了IMU惯导数据，所以需要带惯导信息的摄像头才能运行，例如Intel RealSense ZR300，RealSense D435i等。Intel Realsense D435摄像头不含惯导信息，于是我们购买了IMU芯片，与Realsense D435摄像头一起使用。
参考博客：
https://blog.csdn.net/u013595260/article/details/86522385
https://blog.csdn.net/weixin_44580210/article/details/89789416

（1）安装Realsense D435的SDK库

Realsense官方提供的SDK安装方法无法在arm架构上安装成功，于是JetSonhacks提供了针对JetSon TX2的Realsense D400系列SDK安装脚本。
github官方链接：https://github.com/jetsonhacks/buildLibrealsense2TX

本次安装使用的是v0.9版本，发布日期为July，2018，配置要求： L4T 28.2 / L4T 28.2.1 (JetPack 3.2.1)
如果系统不满足上述要求，需要对JetSon TX2重新安装系统。
将源码zip下载并解压，执行如下命令行进行安装：

#重新构建内核
./buildPatchedKernel.sh
#安装SDK
./installLibrealsense.sh 

在安装过程中发生如下错误：

原因是没有安装CUDA，需要安装CUDA-8.0并添加环境变量，安装命令行如下：

wget http://developer.download.nvidia.com/devzone/devcenter/mobile/jetpack_l4t/013/linux-x64/cuda-repo-l4t-8-0-local_8.0.84-1_arm64.deb
sudo dpkg -i cuda-repo-l4t-8-0-local_8.0.84-1_arm64.deb
sudo apt-get update
sudo apt-get install cuda-toolkit-8-0

在根目录下执行命令 nvcc -V 。如果执行命令后的结果不是CUDA的版本信息而是提示有问题，则需要在JetSon TX2上设置环境变量：

sudo gedit /etc/bash.bashrc

在文件的末尾加入：

export PATH=/usr/local/cuda-8.0/bin:$PATH
export LD_LIBRARY_PATH=/usr/local/cuda-8.0/lib64:$LD_LIBRARY_PATH

之后运行nvcc -V如果显示有版本信息，并在CUDA的example目录下执行sudo make能够正确运行示例，则证明CUDA已成功安装，重新执行./installLibrealsense.sh 安装SDK库。

最后执行realsense-viewer如果能正确显示摄像头采集图像界面，如下所示，则证明SDK库已成功安装。

（2）将librealsense和RealSense安装为ROS包

安装基于ROS使用的RealSense包。
官方链接：https://github.com/jetsonhacks/installRealSense2ROSTX

将源码zip下载并解压，执行如下命令行进行安装：

 ./installRealSenseROS.sh

这里不知道为什么一直编译不通过，单独下载realsense源码进行catkin_make也没有用，可能是因为使用的这个版本比较旧后续有改动，导致找不到一些依赖库之类的问题。于是我卸载了之前安装的SDK（否则librealsense会出现版本冲突）命令行：sudo make uninstall，用ROS包进行安装，命令行如下：

sudo apt-get install librealsense2
sudo apt-get install realsense_camera
sudo apt-get install ros-kinetic-realsense2-camera 
sudo apt-get install ros-kinetic-rgbd-launch

执行完毕上面命令后重启系统：sudo reboot 完成ROS上RealSense包的安装。

（3）安装IMU驱动

由于RealSense D435摄像头不含IMU惯性测量模块，于是我们购入了一块轻量级IMU，集成两个加速度传感器与三个速度传感器（陀螺），用于测量物体三轴姿态角(或角速率)以及加速度，并以此解算出物体的姿态，使得最终的定位结果更加精准。
这里使用的IMU驱动是一位不愿意透露姓名的大神同事自己写的，非常牛逼，涉及到个人版权在这里就不开源了。

直接catkin_make进行编译，之后运行如下命令行：

sudo cp `rospack find serial_com_node`/scripts/jy901.rules  /etc/udev/rules.d
sudo service udev reload
sudo service udev restart
#重启系统
sudo reboot

3、使用Realsense D435摄像头运行Vins-Mono

下面进入激动人心的Vins-Mono运行环节，首先测试Realsense D435摄像头是否能够正常工作。

（1）测试Realsense D435摄像头

打开三个终端，分别执行如下命令行：

source devel/setup.bash
roslaunch realsense2_camera rs_rgbd.launch #运行Realsense2的RGB-D相机

source devel/setup.bash
roslaunch serial_com_node serial.launch  #运行IMU

rosrun rviz rviz

选择CloudPoints2，或者运行roslaunch realsense2_camera rs_camera.launch filters:=pointcloud，如果能够正确看到Realsense D435获得的点云信息，如下图所示，则证明摄像头驱动已正确安装。

（2）运行Vins-Mono

将vins-mono-catkin_ws/src/VINS-Mono/config/realsense/realsense_color_config.yaml文件中订阅的topic更改为Realsense D435与IMU发布的topic，如下所示：

imu_topic: "/imu"
image_topic: "/camera/color/image_raw"
output_path: "/home/nvidia/output/"

打开四个终端，分别执行如下命令行：

source devel/setup.bash
roslaunch realsense2_camera rs_rgbd.launch 

source devel/setup.bash
roslaunch serial_com_node serial.launch 

source devel/setup.bash
roslaunch vins_estimator realsense_color.launch 

source devel/setup.bash
roslaunch vins_estimator vins_rviz.launch 

如果显示界面如下所示，则证明Vins-Mono框架已正确安装，并能够使用Realsense D435摄像头正确运行。

二、在JetSon TX2上运行RTAB-MAP

RTAB-MAP（Real-Time Appearance-Based Mapping）提供了一个基于外观的定位与构图解决方案，解决大型环境中的在线闭环检测问题。
官方链接：http://introlab.github.io/rtabmap/

1、安装与编译RTAB-MAP

在JetSon TX2上安装与编译RTAB-MAP时遇到了很多问题，并且编译速度很慢，需要一些耐心慢慢解决。
源码链接：https://github.com/introlab/rtabmap
官方用户手册：https://github.com/introlab/rtabmap_ros#installation
参考博客：https://www.ncnynl.com/archives/201706/1749.html

首先检查是否已安装OpenCV3，这里不做过多安装说明，之后直接使用源码安装RTAB-MAP，命令行如下：

#安装依赖项 (Qt, PCL, VTK, OpenCV, ...)
sudo apt-get install ros-kinetic-rtabmap ros-kinetic-rtabmap-ros
cd ~
git clone https://github.com/introlab/rtabmap.git rtabmap
cd rtabmap/build
cmake ..
make -j4
make install

在编译过程中遇到如下错误：

/home/nvidia/rtabmap/corelib/src/optimizer/OptimizerGTSAM.cpp:45:42: fatal error: gtsam/sam/BearingRangeFactor.h: No such file or directory

分析原因，应该是没有安装GTSAM依赖项。运行如下命令行安装GTSAM：

git clone https://bitbucket.org/gtborg/gtsam.git
cd gtsam
mkdir build
cd build
cmake ..
make check
make install #需改为sudo make install，否则报如下错误

编译到最后一步时出现了一个错误：

[100%] Built target ConcurrentFilteringAndSmoothingExample Install the project... 
-- Install configuration: "Release" 
-- Installing: /usr/local/lib/cmake/GTSAM/gtsam_extra.cmake CMake Error at cmake_install.cmake:44 (file): 
file INSTALL cannot copy file "/home/shruti/gtsam/build/gtsam_extra.cmake" to
"/usr/local/lib/cmake/GTSAM/gtsam_extra.cmake".
Makefile:105: recipe for target 'install' failed make: *** [install] Error 1

发现是权限不够的问题，需要添加sudo来执行， 命令行：sudo make install
安装完GTSAM依赖项后重新进行RTAB-MAP的编译，依旧没有通过，于是又安装了G2O依赖包，命令行如下：

#安装g2o依赖包:
sudo apt-get install cmake libeigen3-dev libsuitesparse-dev
#源码编译g2o
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o/
mkdir build
cd build/
cmake ../
make

再次进行RTAB-MAP的编译，还是没有通过，报错： error: no matching function for call to ‘g2o::BlockSolver......。
分析原因是G2O初始化时Block出错，以及迭代算法部分出错。由于github上的G2O换了新的接口，可以换回高博的旧版G2O，也可以参考博客https://blog.csdn.net/try_again_later/article/details/82585025进行代码部分修改。

然而这里我依旧没能编译成功，于是直接取消了G2O的依赖关系，命令行如下。这次终于能够正确编译，但可能会对RTAB-MAP的重建精度有一些影响。如果有人知道如何正确安装GTSAM与G2O依赖项欢迎一起讨论 ~ ~

cmake -DWITH_G2O=OFF -DWITH_GTSAM=OFF ..
make
sudo make install 

2、使用摄像头RealSense D435运行RTAB-MAP

参考博客：https://answers.ros.org/question/308137/run-rtab-map-but-without-image-open/
首先将rtabmap.launch文件中订阅的topic更改为RealSense D435摄像头发布的信息，如下所示：

"rgb_topic"               default="/camera/color/image_raw" />
"depth_topic"             default="/camera/aligned_depth_to_color/image_raw" />
"camera_info_topic"       default="/camera/color/camera_info" />

之后分别打开三个终端，分别运行如下命令行：

source devel/setup.bash
roslaunch realsense2_camera rs_rgbd.launch 

source devel/setup.bash
roslaunch serial_com_node serial.launch 

#启动RTAB-MAP建图
source devel/setup.bash
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start"

#或者
source devel/setup.bash
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rviz:=true rtabmapviz:=false

显示如下三维场景重建界面，表明RTAB-MAP已经正确安装，Realsense D435摄像头可以正确运行。

每天都好困想要冬眠的乔木同学
记于2019年11月21日