RealSense D455的标定并运行VINS-FUSION

一、使用D455运行VINS-FUSION

1.1 RealSense D455的使用

1.1.1 Intel® RealSense™ SDK 2.0的安装

安装过程可以参照官网教程.
Reconnect the Intel RealSense depth camera and run: realsense-viewer to verify the installation.
使用D455相机的原装usb线到电脑(接口最好是usb3.0,否则可能导致数据传输过慢而使得数据无法实时传输，出现画面卡住的现象)，并且打开一个终端输入

realsense-viewer

看这个软件能否打开来验证是否成功安装。
成功的界面如下：
我们通过左边的选项打开imu，双目和rgb相机模块，同时可以查看各个模块的参数并进行设置

1.1.2 realsense-ros的安装编译

安装过程可以参考官网教程.跟着README.md进行安装就可以了
安装编译完之后,插上相机，打开一个终端

source ~/catkin_ws/devel/setup.bash
roslaunch realsense2_camera rs_camera.launch

注意：这个时候要关掉realsense-viewer，看到如图就是启动成功了

主要看RealSense Node IS Up!,如果报错显示运动模块没有启动成功，拔掉usb重新插一下，或者重新运行launch文件，像屏幕中这种错误warning就没有影响

再打开一个新的终端

rostopic list

就可以看到这个相机发布的所有topic 如图

打开一个终端

rviz

Fixed Frame选择camera_link(连接realsense数据)，通过Add添加topic：/camera/imu、camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw后可以看到如下结果为准（说明可以成功读取到加速计、陀螺仪、双目数据，陀螺仪无法可视化）

2.1 VINS-FUSION的安装编译

1. 安装ROS 根据你的Ubuntu版本安装对应的ROS版本，安装过程可以参照ROS官方教程
2. 安装ceres可以参照ceres官方教程.
3. 下载编译VINS-FUSION

cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
#或者 echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
#这样就不用每次打开一个新终端都要source一次了

VINS-FUSION/config/下放着各种config文件，这里也有realsense-d435i的配置文件，我们复制这个文件夹并改名为realsense-d455。

2.2 使用D455运行VINS-FUSION

2.2.1 修改VINS-FUSION中的config文件

这里我们运行双目加IMU的，进入到catkin_ws/src/realsense-ros/realsense2_camera/launch/realsense-d455（刚刚我们复制的）

1. 修改realsense_stereo_imu_config.yaml

realsense_stereo_imu_config.yaml的topic改成D455相机的topic，输出的路径也改成你自己，图片的高和宽也要修改，如图所示：

2. 修改left.yaml和right.yaml
   修改相机图片的尺寸为D455的
   
   保存，修改config文件不需要重新编译工程

2.2.2 修改realsense-ros中的launch文件

进入到catkin_ws/src/realsense-ros/realsense2_camera/launch/中打开rs_camera.launch（可以对原先的文件做一个备份）

1. 打开双目，修改图片的width、height
   
2. 打开imu的加速度计和陀螺仪并合并为一个topic
   
   
   保存，修改launch不需要重新编译工程

2.2.3 运行VINS-FUSION

1. 打开一个终端 ，用roslaunch 打开realsense摄像头

source ~/catkin_ws/devel/setup.bash
roslaunch realsense2_camera rs_camera.launch

2. 打开一个终端，用于打开rviz`

source ~/catkin_ws/devel/setup.bash
roslaunch vins vins_rviz.launch

3. 打开一个终端，跑起来

source ~/catkin_ws/devel/setup.bash
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/yourconfig_path/your_config_file.yaml 
#我的是rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/src/VINS-Fusion-master/config/realsense_d455/realsense_stereo_imu_config.yaml

三个终端运行成功的话，你的rviz会有数据，特征点和frame的pose
如图所示

相机还没有标定，轨迹特别抖，下面我们对D455相机进行标定

二、对D455进行标定

我们使用kalibr对相机进行标定，code_utils和imu_utils对imu进行标定，但是这个标定出来是没有重力加速度的。因此我们首先要看一下我们相机的加速度在静置的时候是不是正常的，正常的话我们加速度计的g_norm: 9.82左右（可以打开realsense-viewer,开启Motion Module 鼠标放在Accel上面查看）如图

如果不正常，例如我校准前的g_norm为9.13左右，你再怎么标定，最后运行VINS-FUSION都是抖的。

2.1 校准IMU

如果你的设备imu的加速度计是正常的，那你可以跳过以下的校准过程。

我们采用官方的方法对imu进行校准
可以参考https://dev.intelrealsense.com/docs/imu-calibration-tool-for-intel-realsense-depth-camera
使用官方的校准脚本按照指示进行校准

校准完之后我们会发现加速度计的值变成9.82左右，恢复正常。要是标定出来的结果的相机的重投影误差Reprojection error在0.15以上的话，当排除是标定过程不够规范的情况下，我们就要思考是不是相机健康状态的问题了。因此建议都进行相机健康状态的检查，避免浪费时间。

2.2 检查相机的健康状态

我们可以参照https://dev.intelrealsense.com/docs/calibration对相机进行各种校准

我们使用realsense-viewer的Self Calibration查看相机的健康状态。

具体的可以看链接中的官方教程
如果健康状态不佳，我们再使用Dynamic Calibration进行校准，要是结果还是不好，我们就使用Custom Calibration进行校准。

2.3 下载编译code_utils

1. 创建ros工作空间，用于code_utils以及后面的imu_utils，工作空间名和路径可以自己更改，code_utils和imu_utils都是imu标定需要用到的，用于标定imu噪声密度以及随机游走系数

mkdir -p ~/imu_catkin_ws/src
cd ~/imu_catkin_ws/src
catkin_init_workspace
cd ..
catkin_make
source ~/imu_catkin_ws/devel/setup.bash

2. 下载编译code_utils

cd ~/imu_catkin_ws/src
git clone git@github.com:gaowenliang/code_utils.git
cd ..
catkin_make

可能出现以下报错：
（1）catkin_make时出现libdw.h没有找到

解决方法：

sudo apt-get install libdw-dev

（2）catkin_make时出现backward.hpp没有找到

解决方法：将sumpixel_test.cpp中# include "backward.hpp"改为：#include “code_utils/backward.hpp”

2.3 下载编译imu_utils

cd ~/imu_catkin_ws/src/
git clone git@github.com:gaowenliang/imu_utils.git
cd ..
catkin_make

2.4 下载编译Kalibr

参考官方教程https://github.com/ethz-asl/kali