Jetson TX2在ROS下使用Realsense D435i跑rtabmap、octomap、VINS-Mono和ORB-SLAM2

• 使用环境：Ubuntu 16.04(JetPack 3.3)，ROS Kinetic
• 硬件设备：英伟达Jetson TX2，英特尔Realsense D435i

安装Realsense相关的相机驱动

1.安装librealsense

• 方法一：首选方法是用二进制包方式安装（就是用sudo apt-get install …），该方法不用手动给内核打补丁。
  因为Jetson设备的Ubuntu系统内核跟普通PC上的不太一样，引用librealsense在Jetson上的官方安装教程installation_jetson.md一段话：“NVIDIA的L4T提供了基于Ubuntu的发行版，带有基于4.9版的自定义内核。内核的配置和部署方式与桌面Ubuntu映像不同，两个显着差异是默认配置中包含的内核模块列表以及新映像的刷新方式。”

步骤如下：
（1）注册服务器的公钥：

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

（2）将服务器添加到软件源列表中：
Ubuntu 16：

sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo xenial main" -u

Ubuntu 18：

sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo bionic main" -u

（3）安装SDK：

sudo apt-get install librealsense2-utils
sudo apt-get install librealsense2-dev

（4）重新连接RealSense设备并从终端运行：realsense-viewer验证安装

• 方法二：下载librealsense源码包编译，该方法有一个好处就是可以下载指定版本的librealsense包，方便与后续下载的realsense-ros版本匹配。

步骤如下：
（1）下载源码包

git clone https://github.com/IntelRealSense/librealsense.git

*若想下载指定版本的librealsense，可从Releases页面下载。
（2）构建和修补Jetson L4T的内核模块

cd librealsense2
./scripts/patch-realsense-ubuntu-L4T.sh  

（3）编译librealsense2 SDK

sudo apt-get install git libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev -y
./scripts/setup_udev_rules.sh  
mkdir build && cd build  
cmake .. -DBUILD_EXAMPLES=true -DCMAKE_BUILD_TYPE=release -DFORCE_RSUSB_BACKEND=false -DBUILD_WITH_CUDA=true && make -j$(($(nproc)-1)) && sudo make install

（4）连接Realsense设备，运行realsense-viewer并检查结果

2.安装realsense-ros

这一步要注意与librealsense的版本匹配问题。详见realsense-ros发行说明

推荐采用源码包方式安装，方便修改launch等文件。
步骤如下：
（1）先创建工作空间

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/
catkin_init_workspace

（2）下载realsense-ros到src里
若最新版本realsense-ros与librealsense版本不匹配，自行到releases里下载。

git clone https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros/
git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1` #检查所下载的版本是否是想要的
cd ..

（3）编译

cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

（4）插上相机测试一下

roslaunch realsense2_camera rs_camera.launch

ROS下使用D435i跑rtabmap和octomap

参考SLAM-with-D435i。

1. 安装以下三个包：imu_filter_madgwick、rtabmap_ros和robot_localization

sudo apt-get install ros-kinetic-imu-filter-madgwick  
sudo apt-get install ros-kinetic-rtabmap-ros  
sudo apt-get install ros-kinetic-robot-localization  

2. 启动
   这里启动的是realsense-ros包里带的opensource_tracking.launch

roslaunch realsense2_camera opensource_tracking.launch

效果如下：

注意到上图中ColorOccupacyGrid插件报错，原因是缺少安装Octomap在ROS中的包。

3. 安装octomap-ros

sudo apt-get install ros-kinetic-octomap*

4. 重新使用D435i启动节点

roslaunch realsense2_camera opensource_tracking.launch

效果如下：

ROS下使用D435i跑VINS-Mono

1. 先安装依赖项

sudo apt-get install ros-kinetic-cv-bridge ros-kinetic-tf ros-kinetic-message-filters ros-kinetic-image-transport
# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3
# 因为在安装ceres-solver时，要求Eigen版本≥3.3.4，而TX2上边已装的版本为3.3，故在Eigen官网eigen.tuxfamily.org/index.php?Title=Main_Page下载
# 解压在/home下，然后用cmake进行安装（该包不需要make编译），具体步骤如下
cd eigen
mkdir build
cd build
cmake ..
sudo make install  #默认安装路径为/usr/local/include/eigen3
# SuiteSparse 、 CXSparse (可选)
sudo apt-get install libsuitesparse-dev

2. 在/home下下载编译ceres-solver，步骤如下

cd
git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver/
mkdir build
cd build
cmake ..
make -j8
sudo make install

3. 下载源码包并编译

cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bash

4. 测试

# 测试所需的bag文件：https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play MH_01_easy.bag

5. 在D435i上运行VINS-Mono

（1）修改realsense包里的rs_camera.launch文件

• 第一处，修改unite_imu_method如下，这里是让IMU的角速度和加速度作为一个topic输出

<arg name="unite_imu_method"      default="copy"/>

• 第二处，修改enable_sync参数为true，这里是开机相机和IMU的同步

<arg name="enable_sync"           default="true"/>

（2）修改VINS-Mono包里的realsense_color_config.yaml文件

• 第一处，修改订阅的topic

imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"

• 第二处，修改相机内参，这里先再次打开运行realsesne包，然后可以通过如下命令获取相机内参

rostopic echo /camera/color/camera_info

• 第三处，IMU到相机的变换矩阵，这里我根据注释的提示修改成2
  
• 第四处，IMU参数，这里我全部修改注释给的参数
  
• 第五处，是否需要在线估计同步时差，根据这篇博客中的建议这里选择不需要
  
• 第六处，相机曝光改成全局曝光
  

(3)连接并打开摄像头，运行VINS-Mono

roslaunch realsense2_camera rs_camera.launch 
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

成功运行VINS-Mono，如下图

ROS下使用D435i跑ORB-SLAM2

1. 安装Intel Realsense SDK(之前已安装）
2. 安装ROS Wrapper for Intel Realsense（已安装）
3. 安装依赖

sudo apt-get install libglew-dev
sudo apt-get install cmake
sudo apt-get install libpython2.7-dev
sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev
sudo apt-get install libdc1394-22-dev libraw1394-dev
sudo apt-get install libjpeg-dev libpng12-dev libtiff5-dev libopenexr-dev

4. 安装Pangolin

cd ~
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake ..
make -j4

5. 安装Opencv。（已安装，可用opencv_version查看确认）
6. 安装Eigen3。（已源码安装3.3.4）
7. 下载ORB_SLAM2源码

cd ~
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2

8. 编译ORB_SLAM2

cd ORB_SLAM2
chmod +x build.sh
./build.sh

• 注：在执行./build.sh之前先在 /ORB_SLAM2/include/System.h文件开头的包含声明中添加#include <unistd.h>，不然会编译时会报错error: usleep is not declared in this scope，参考issues

9. 将下句加到.bashrc的最后一行，注意路径根据实际更改

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:<实际下载路径>/ORB_SLAM2/Examples/ROS

10. 执行 build_ros.sh 脚本

 chmod +x build_ros.sh
./build_ros.sh

• 上述步骤中在执行./build_ros.sh会出现关于boost库的错误，在/ORB_SLAM2/Examples/ROS/ORB-SLAM2/CMakeLists.txt文件下修改，按照下述位置加上-lboost_system，然后重新执行./build_ros.sh

// CMakeLists.txt文件部分内容节选如下
set(LIBS 
${OpenCV_LIBS} 
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
-lboost_system             // 此句加在这里
)

11. 按照以下步骤我们可获取相机参数，通过USB连接相机到电脑，然后执行。

roslaunch realsense2_camera rs_rgbd.launch
#新开终端运行以下命令：
rostopic echo /camera/color/camera_info

• 它的数据结构如下：

---
header: 
  seq: 214
  stamp: 
    secs: 1606120730
    nsecs: 750465811
  frame_id: "camera_color_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [612.8856811523438, 0.0, 322.271240234375, 0.0, 612.9019775390625, 244.7898712158203, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [612.8856811523438, 0.0, 322.271240234375, 0.0, 0.0, 612.9019775390625, 244.7898712158203, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

12. 在ORB_SLAM2/Examples/RGB-D/下新建D435i.yaml文件，内容根据上述终端输出的参数修改

%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV) 

Camera.fx: 612.8856811523438
Camera.fy: 612.9019775390625
Camera.cx: 322.271240234375
Camera.cy: 244.7898712158203

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.p3: 0.0

Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
# bf = baseline (in meters) * fx, D435i的 baseline = 50 mm 
Camera.bf: 30.797   

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 40.0

# Deptmap values factor
DepthMapFactor: 1000.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------

Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

• 摄像头节点发布的rgbd图和depth图话题名与ORB-SLAM2的订阅RGB图和depth图话题名不同，在ORB-SLAM2/Examples/ROS/ORB-SLAM2/src中修改ros_rgbd.cc的topic订阅：

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);

13. 最后打开终端在ORB_SLAM2工作目录下重新编译build_ros.sh

# 重新编译build_ros.sh
chmod +x build_ros.sh
./build_ros.sh

14. 在ROS下用realsense D435i运行ORB_SLAM2

# 启动相机
roslaunch realsense2_camera rs_rgbd.launch
# 运行ORB_SLAM2
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/D435i.yaml

效果如图：