一、Ubuntu18.04安装VINS-Fusion
1.1安装ceres
- VINS-Fusion依赖的Ceres库版本已经很老了，最近版本的ceres并不支持VINS-Fusion。VINS-Fusion比较适配的Ceres版本是1.14。
1
2
3
4
5
6
7
$ 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 && cd ceres-bin
$ cmake ../ceres-solver-1.14.0 && make -j8
$ make test
$ sudo make install
1.2编译
如果你的电脑安装了Opecv4，那么你需要进行这一步的修改，如果没有安装请忽略此步。
由于VINS-Fusion依赖的是OpenCV3,为了避免找的OpenCV的版本是OpenCV4的版本，我们需要修改CMakeList.txt。分别修改以下三个文件夹中的CMakeList.txt找到find_package(OpenCV REQUIRED),并在后面填上版本号。
1
2
3
4
5
find_package(OpenCV REQUIRED) #修改前
find_package(OpenCV 3 REQUIRED) #修改前
###或
find_package(OpenCV ) #修改前
find_package(OpenCV 3 ) #修改前
尝试运行以下命令进行编译：
1
2
cd ~/catkin_ws
catkin_make
二、安装Intel® RealSense™ SDK 2.0
- Intel® RealSense™ SDK 2.0 的DKMS内核驱动包（librealsense2-dkms）支持Ubuntu LTS内核版本4.4、4.8、4.10、4.13、4.15、4.18、5.0、5.3、5.4。
2.1查看自己的内核版本：
1
sudo cat /proc/version
如果非上述版本，需要手动安装和修补版本，请移步官网教程：
下载地址
本教程不适合你。
2.2 注册服务器公钥：
1
sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
如果公钥无法检索，检查并指定代理设置：
1
export http_proxy="http://<proxy>:<port>"
重新运行该命令。
2.3将服务器添加到储存库列表
1
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
2.4 安装库
1
2
sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils
2.5 可选择安装开发人员和调试包：
1
2
sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg
2.6 验证内核是否已经更新：
1
modinfo uvcvideo | grep "version:"
version后面出现realsense表示更新成功。
2.7 启动realsense-viewer
插入D455相机，可能需要USB3.0及以上接口。启动realsense-viewer：
1
realsense-viewer
通过界面左侧的选项可以打开imu，双目和rgb相机模块，右上可切换2D、3D界面：
成功！退出realsense-viewer界面。
报错提示：如果已经将D455相机连接至虚拟机上但仍不出图像（如下图所示）
请前往虚拟机进行设置，设置虚拟机的兼容性为3.0即可
三、realsense-ros的安装与编译
3.1创建工作空间
创建工作空间 ，这里~/Packages/RealsenseRos_ws可以自定义路径，但必须包含/src：（建议路径与本文档保持一致，否则后续改路径会很麻烦）
1
2
mkdir -p ~/Packages/RealsenseRos_ws/src
cd ~/Packages/RealsenseRos_ws/src
3.2 从github上下载realsense-ros并修改指针：
1
2
3
4
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.3 安装ddynamic_reconfigure
初始化工作空间前，最好检查一下ddynamic_reconfigure是否安装。
执行如下命令（注意将melodic改为自己的ROS对应版本，修改“melodic”为”kinect”、”neotic”等等）：
1
sudo apt-get install ros-melodic-ddynamic-reconfigure
否则会出现报错：提示没有ddynamic_reconfigure。
3.4 初始化工作空间
1
2
3
catkin_init_workspace
cd ..
catkin_make clean
3.5 安装源码
1
2
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install
3.6 添加工作路径到bashrc文件
注意修改~/Packages/RealsenseRos_ws/为自己的路径。
1
2
echo "source ~/Packages/RealsenseRos_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
3.7 插上相机并启动相机
插上相机运行
1
roslaunch realsense2_camera rs_camera.launch
如果出现报错：[camera/realsense2_camera_manager-2]进程已经死亡。报错如下：
/opt/ros/melodic/lib/nodelet/nodelet: symbol lookup error: /home/khz/ROS/catkin_ws/devel/lib//librealsense2_camera.so: undefined symbol: _ZN2cv3MatC1EiiiRKNS_7Scalar_IdEE [camera/realsense2_camera_manager-2] process has died [pid 4924, exit code 127, cmd /opt/ros/melodic/lib/nodelet/nodelet manager __name:=realsense2_camera_manager __log:=/home/khz/.ros/log/81b1e24a-8f95-11ec-8270-344b50000000/camera-realsense2_camera_manager-2.log]. log file: /home/khz/.ros/log/81b1e24a-8f95-11ec-8270-344b50000000/camera-realsense2_camera_manager-2*.log [camera/realsense2_camera-3] process has finished cleanly log file: /home/khz/.ros/log/81b1e24a-8f95-11ec-8270-344b50000000/camera-realsense2_camera-3*.log
由于出现提示：未定义标识符undefined symbol: _ZN2cv3MatC1EiiiRKNS_7Scalar_IdEE。猜测是OpenCV库的问题。
解决方案：
首先看看自己有没有安装OpenCV，没有安装请先安装，安装过程略。
如果已经安装，打开realsense-ros的功能包文件夹下 realsense2_camera 文件夹下的 CmakeLists.txt文件，做出如下修改：
添加set(OpenCV_DIR /usr/local/share/OpenCV/)到find_package前（下图11行，注意检查自己OpenCV的路径），添加OpenCV REQUIRED到find_package里（下图26行，注意放在括号里）。
添加${OpenCV_LIBS}到目标链接库里（下图127行）
然后跳回2.5再次安装，完成后执行roslaunch realsense2_camera rs_camera.launch。看到如下图就是启动成功了（有Realsense Node Is Up就行，警告可以忽略）。
3.8 启动RGBD相机
如果是RGBD相机，需启用第二步验证：
1
roslaunch realsense2_camera rs_rgbd.launch
如果出现报错：
Resource not found: rgbd_launch ROS path [0]=/opt/ros/melodic/share/ros ROS path [1]=/home/andyvictory/Packages/RealsenseRos_ws/src ROS path [2]=/opt/ros/melodic/share The traceback for the exception was written to the log file
安装RGBD的launch（注意ROS版本）：
1
sudo apt install ros-melodic-rgbd-launch
安装成功！
四、IMU标定
4.1 重力加速度自检
插入相机并静置， 终端输入realsense-viewer，开启realsense-viewer左侧的Motion Module模块，将鼠标放在加速度计（Accel stream）上，观察g_norm读数是否在9.8附近。
4.2 利用官方的rs_imu_calibration.py工具进行IMU自校准
注意：g_norm读数正常的，请跳过此步骤，直接进入步骤3 ； g_norm读数不正常的，利用官方的rs_imu_calibration.py工具进行IMU自校准，详情参考官方网站：
https://dev.intelrealsense.com/docs/imu-calibration-tool-for-intel-realsense-depth-camera
英文好的可以看官方网站，英文不好的可以参考此文档。
4.2.1 下载rs_imu_calibration.py脚本
https://github.com/IntelRealSense/librealsense
通过上面的链接下载，下载后在~/主目录/librealsense-master/tools/rs_imu_calibration文件夹里可以找到脚本文件rs_imu_calibration.py。(我的文件是在主目录下，也可放在Package中)
4.2.2 构建Python环境及其他依赖包
检查Python版本，通过命令：python –version 这边使用的是Python2，如果2和3都安装的，运行如下命令进行手动切换。
1
sudo update-alternatives --config python
显示如下，键入python3的序号（这里是2），回车。
有 2 个候选项可用于替换 python (提供 /usr/bin/python)。 选择 路径 优先级 状态 ------------------------------------------------------------ * 0 /usr/bin/python3 150 自动模式 1 /usr/bin/python2 100 手动模式 2 /usr/bin/python3 150 手动模式
注意：此方法不会修改Python的优先级，也就是之后还会切换回Python3。如果想更换Python2为默认环境，需要sudo update-alternatives –install /usr/bin/python python /usr/bin/python2 150加上sudo update-alternatives –install /usr/bin/python python /usr/bin/python3 100命令来修改优先级。两种方法二选一！！！
有些依赖或者软件之前安装过，以防万一，建议还是install一下。
1
2
3
sudo apt-get install python-pip
sudo pip install numpy
sudo pip install pyrealsense2
4.2.3 运行脚本
插上相机，进入rs_imu_calibration.py文件所在文件夹（我的在~/主目录/librealsense-master/tools/rs-imu-calibration下，建议直接按此文档的路径进行配置，不然后面找路径很麻烦），运行
1
python rs-imu-calibration.py
出现如下界面，并不停地有读数进来。
4.2.4 从6个位置捕获IMU数据
将相机以下图所示位置摆放
放置正确后终端会出现：Status.collect_data[……………….]]，期间不要移动相机，直到省略号满格出现Direction data collected。若不小心移动，会出现Status.collect_dataWARNING: MOVING的警告，此时摆正位置不要移动直到出现Direction data collected
出现Direction data collected后翻转相机到下一个位置。依次在位置2~6重复操作：
4.2.5 更新数据
6个位置都捕获完成后，会提示
意思是是否保存原数据，按回车不保存原数据。然后会计算出重力加速度值：
计算完后会提示：Would you like to write the results to the camera? (Y/N)。键入Y写入到设备
4.3 下载编译用于IMU标定的code_utils和imu_utils
4.3.1 Ceres安装
依赖安装：
1
2
3
4
5
sudo apt-get install cmake
sudo apt-get install libgoogle-glog-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev
Ceres安装：
1
2
3
4
5
6
7
git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver
mkdir build
cd build
cmake ..
make -j4
sudo make install
4.3.2 下载编译code_utils
1
2
3
4
5
mkdir -p ~/Calibration/IMU_ws/src ##可自定义
cd ~/Calibration/IMU_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make -j4
报错解决1：error：libdw.h没有那个文件或目录
解决措施：sudo apt-get install libdw-dev
报错解决2：error：backward.hpp没有那个文件或目录
解决措施：将sumpixel_test.cpp中# include "backward.hpp" 改为：#include "code_utils/backward.hpp"
报错解决3：error：‘integer_sequence’ is not a member of ‘std’
解决措施： 将CMakeLists.txt中的 set(CMAKE_CXX_FLAGS "-std=c++11") 改成 set(CMAKE_CXX_STANDARD 14)
4.3.3 下载编译imu_utils
1
2
3
4
cd ~/Calibration/IMU_ws/src ##和code_utils的工作空间保持一致
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make -j4
4.4 IMU标定
4.4.1 修改launch文件并启动相机
在~/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch文件夹下（路径仅供参考，具体参照之前realsense-ros的工作空间，建议直接按此文档的路径进行配置，不然后面找路径很麻烦），复制rs_camera.launch到同一个目录下，并重命名为rs_imu_calibration.launch(命名随意)。并将新文件中的
1
<arg name="unite_imu_method" default=""/>
修改为
1
<arg name="unite_imu_method" default="linear_interpolation"/>
以及将
1
2
<arg name="enable_gyro" default="false"/>
<arg name="enable_accel" default="false"/>
修改为
1
2
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
这样做的目的是将accel和gyro的数据合并得到imu话题，不这样做发布的topic没有合并的camera/imu话题。 用新launch文件启动相机
1
2
3
cd ~/Packages/RealsenseRos_ws
source devel/setup.bash ##一定要记得刷新环境变量，不然imu话题出不来
roslaunch realsense2_camera rs_imu_calibration.launch
使用rostopic list查看是否有/camera/imu话题发布。
4.4.2 录制imu数据包
在保存bag包的路径打开终端，静置相机，终端输入：
1
rosbag record -O imu_calibration /camera/imu
放置时间建议大于2h（官方建议），内存不够的话可以放置五六十分钟，整个标定完成之后，记得删除此文件，很占内存！！！
4.4.3 编写启动文件
1
2
cd ~/Calibration/IMU_ws/src/imu_utils/launch ##看看自己存放的路径是哪个
gedit d455_imu_calibration.launch
写入
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<!--TOPIC名称和上面一致-->
<param name="imu_topic" type="string" value= "/camera/imu"/>
<!--imu_name 无所谓-->
<param name="imu_name" type="string" value= "d455"/>
<!--标定结果存放路径-->
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<!--数据录制时间-min-->
<param name="max_time_min" type="int" value= "120"/>
<!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为200，为后面的rosbag play播放频率-->
<param name="max_cluster" type="int" value= "200"/>
</node>
</launch>
4.4.4 imu标定
在刚才imu_utils的工作空间下打开终端（我的是~/Calibration/IMU_ws），运行
1
2
source ./devel/setup.bash
roslaunch imu_utils d455_imu_calibration.launch
回放数据包,打开新的终端
1
2
cd 存放imu_calibration.bag的路径
rosbag play -r 200 imu_calibration.bag
标定完成后在~/Calibration/IMU_ws/src/imu_utils/data下查看，主要看d455_imu_param.yaml文件，文件内容如下（结果仅供参考）：
可将标定结果移动到自己创建的文件夹里，后续的标定结果也放在这里（我的路径是~/Calibration/D455/imu_calibration）。
五、多相机标定
5.1 kalibr安装编译
5.1.1 安装依赖项
所有 Ubuntu 版本的通用要求如下：
1
2
3
4
5
sudo apt-get install -y \
git wget autoconf automake nano \
libeigen3-dev libboost-all-dev libsuitesparse-dev \
doxygen libopencv-dev \
libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev
然后由于不同的Python版本，您将需要安装以下内容：
1
2
3
4
5
6
7
8
9
# Ubuntu 16.04
sudo apt-get install -y python2.7-dev python-pip python-scipy \
python-matplotlib ipython python-wxgtk3.0 python-tk python-igraph
# Ubuntu 18.04
sudo apt-get install -y python3-dev python-pip python-scipy \
python-matplotlib ipython python-wxgtk4.0 python-tk python-igraph
# Ubuntu 20.04
sudo apt-get install -y python3-dev python3-pip python3-scipy \
python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph
本文为ubuntu18.04系统，安装命令如下：
1
2
sudo apt-get install -y python3-dev python-pip python-scipy \
python-matplotlib ipython python-wxgtk4.0 python-tk python-igraph
注：安装python-igraph时，我用pip命令无法安装，故改为apt-get命令，安装得很顺利。
5.1.2 创建kalibr的工作空间
Ubuntu18.04 创建kalibr的工作空间
1
2
3
4
5
6
7
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
5.1.3 源码下载
地址：https://github.com/ethz-asl/kalibr
官网教程是从网站克隆，但克隆版本并不符合安装系统，总是出错，显示kalibr_calibrate_imu_camera命令未找到，多次重新编译也未成功。
所以此次选用kalibr与ubuntu18.04对应的版本（其他ubuntu系统同理）
下载后将kalibr-fix-18.04.zip解压到 ~/kalibr_workspace/src文件夹下：
5.1.4 源码编译
1
2
cd ~/kalibr_workspace/
catkin build -DCMAKE_BUILD_TYPE=Release -j4
如果出现错误，可以多编译几次，好多错误重新编译就没有了catkin build -DCMAKE_BUILD_TYPE=Release -j4
5.1.5 安装成功
注意：是36个包
5.2 标定板制作或淘宝购买
下载打印标定板或购买标定板 到https://github.com/ethz-asl/kalibr/wiki/downloads选择 需要科学上网（翻墙！！） 下载，然后缩放到40%，用A4纸就可以打印出来（有条件去淘宝买一个视觉标定板，标出来的误差会更小） 原始pdf的格子参数是： 6*6的格子 大格子边长：5.5cm 小格子边长：1.65cm 小格子与大格子边长比例：0.3 调整后的格子参数是： 大格子边长：2.2cm 小格子边长：0.66cm 小格子与大格子边长比例：0.3 但这只是理想情况，实际情况还得实际测量。 新建april_6x6_A4.yaml文件，格式参考上图的yaml，内容展示如下：
1
2
3
4
5
6
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags 列数
tagRows: 6 #number of apriltags 行数
tagSize: 0.022 #size of apriltag, edge to edge [m] 大格子尺寸，单位为米
tagSpacing: 0.3 #ratio of space between tags to tagSize 小格子与大格子尺寸的比值
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
千万要自己测量大格子边长，即tagSize
5.3 修改相机启动文件
在~/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch文件夹下（路径仅供参考，具体参照之前realsense-ros的工作空间），创建文件rs_imu_stereo_calibration_640.launch(懒得对比原文件了直接写个新的，后面联合标定也用这个，命名随意)。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
<launch>
<arg name="serial_no" default=""/>
<arg name="usb_port_id" default=""/>
<arg name="device_type" default=""/>
<arg name="json_file_path" default=""/>
<arg name="camera" default="camera"/>
<arg name="tf_prefix" default="$(arg camera)"/>
<arg name="external_manager" default="false"/>
<arg name="manager" default="realsense2_camera_manager"/>
<arg name="output" default="screen"/>
<arg name="respawn" default="false"/>
<arg name="fisheye_width" default="-1"/>
<arg name="fisheye_height" default="-1"/>
<arg name="enable_fisheye" default="false"/>
<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="enable_depth" default="true"/>
<arg name="confidence_width" default="-1"/>
<arg name="confidence_height" default="-1"/>
<arg name="enable_confidence" default="true"/>
<arg name="confidence_fps" default="-1"/>
<arg name="infra_width" default="640"/>
<arg name="infra_height" default="480"/>
<arg name="enable_infra" default="false"/>
<arg name="enable_infra1" default="true"/>
<arg name="enable_infra2" default="true"/>
<arg name="infra_rgb" default="false"/>
<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="enable_color" default="true"/>
<arg name="fisheye_fps" default="-1"/>
<arg name="depth_fps" default="30"/>
<arg name="infra_fps" default="30"/>
<arg name="color_fps" default="30"/>
<arg name="gyro_fps" default="200"/>
<arg name="accel_fps" default="250"/>
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
<arg name="enable_pointcloud" default="false"/>
<arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
<arg name="pointcloud_texture_index" default="0"/>
<arg name="allow_no_texture_points" default="false"/>
<arg name="ordered_pc" default="false"/>
<arg name="enable_sync" default="true"/>
<arg name="align_depth" default="false"/>
<arg name="publish_tf" default="true"/>
<arg name="tf_publish_rate" default="0"/>
<arg name="filters" default=""/>
<arg name="clip_distance" default="-2"/>
<arg name="linear_accel_cov" default="0.01"/>
<arg name="initial_reset" default="false"/>
<arg name="reconnect_timeout" default="6.0"/>
<arg name="wait_for_device_timeout" default="-1.0"/>
<arg name="unite_imu_method" default="linear_interpolation"/>
<arg name="topic_odom_in" default="odom_in"/>
<arg name="calib_odom_file" default=""/>
<arg name="publish_odom_tf" default="true"/>
<arg name="stereo_module/exposure/1" default="7500"/>
<arg name="stereo_module/gain/1" default="16"/>
<arg name="stereo_module/exposure/2" default="1"/>
<arg name="stereo_module/gain/2" default="16"/>
<group ns="$(arg camera)">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="external_manager" value="$(arg external_manager)"/>
<arg name="manager" value="$(arg manager)"/>
<arg name="output" value="$(arg output)"/>
<arg name="respawn" value="$(arg respawn)"/>
<arg name="serial_no" value="$(arg serial_no)"/>
<arg name="usb_port_id" value="$(arg usb_port_id)"/>
<arg name="device_type" value="$(arg device_type)"/>
<arg name="json_file_path" value="$(arg json_file_path)"/>
<arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
<arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
<arg name="pointcloud_texture_index" value="$(arg pointcloud_texture_index)"/>
<arg name="enable_sync" value="$(arg enable_sync)"/>
<arg name="align_depth" value="$(arg align_depth)"/>
<arg name="fisheye_width" value="$(arg fisheye_width)"/>
<arg name="fisheye_height" value="$(arg fisheye_height)"/>
<arg name="enable_fisheye" value="$(arg enable_fisheye)"/>
<arg name="depth_width" value="$(arg depth_width)"/>
<arg name="depth_height" value="$(arg depth_height)"/>
<arg name="enable_depth" value="$(arg enable_depth)"/>
<arg name="confidence_width" value="$(arg confidence_width)"/>
<arg name="confidence_height" value="$(arg confidence_height)"/>
<arg name="enable_confidence" value="$(arg enable_confidence)"/>
<arg name="confidence_fps" value="$(arg confidence_fps)"/>
<arg name="color_width" value="$(arg color_width)"/>
<arg name="color_height" value="$(arg color_height)"/>
<arg name="enable_color" value="$(arg enable_color)"/>
<arg name="infra_width" value="$(arg infra_width)"/>
<arg name="infra_height" value="$(arg infra_height)"/>
<arg name="enable_infra" value="$(arg enable_infra)"/>
<arg name="enable_infra1" value="$(arg enable_infra1)"/>
<arg name="enable_infra2" value="$(arg enable_infra2)"/>
<arg name="infra_rgb" value="$(arg infra_rgb)"/>
<arg name="fisheye_fps" value="$(arg fisheye_fps)"/>
<arg name="depth_fps" value="$(arg depth_fps)"/>
<arg name="infra_fps" value="$(arg infra_fps)"/>
<arg name="color_fps" value="$(arg color_fps)"/>
<arg name="gyro_fps" value="$(arg gyro_fps)"/>
<arg name="accel_fps" value="$(arg accel_fps)"/>
<arg name="enable_gyro" value="$(arg enable_gyro)"/>
<arg name="enable_accel" value="$(arg enable_accel)"/>
<arg name="publish_tf" value="$(arg publish_tf)"/>
<arg name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
<arg name="filters" value="$(arg filters)"/>
<arg name="clip_distance" value="$(arg clip_distance)"/>
<arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/>
<arg name="initial_reset" value="$(arg initial_reset)"/>
<arg name="reconnect_timeout" value="$(arg reconnect_timeout)"/>
<arg name="wait_for_device_timeout" value="$(arg wait_for_device_timeout)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="topic_odom_in" value="$(arg topic_odom_in)"/>
<arg name="calib_odom_file" value="$(arg calib_odom_file)"/>
<arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
<arg name="stereo_module/exposure/1" value="$(arg stereo_module/exposure/1)"/>
<arg name="stereo_module/gain/1" value="$(arg stereo_module/gain/1)"/>
<arg name="stereo_module/exposure/2" value="$(arg stereo_module/exposure/2)"/>
<arg name="stereo_module/gain/2" value="$(arg stereo_module/gain/2)"/>
<arg name="allow_no_texture_points" value="$(arg allow_no_texture_points)"/>
<arg name="ordered_pc" value="$(arg ordered_pc)"/>
</include>
</group>
</launch>
5.4 启动相机并关闭结构光
1
2
roslaunch realsense2_camera rs_imu_stereo_calibration_640.launch
rosrun rqt_reconfigure rqt_reconfigure
将此部分改为OFF
5.5 打开rviz并修改相机帧数
新终端运行roscore 和 rviz，点击add添加双目对应的topic，/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw
按照官方建议修改相机帧数为4Hz，并命名为新话题（此方法会使得发布频率略小于4Hz，报告影响不大）。
1
2
3
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
查看实际频率
1
rostopic hz /color /infra_left /infra_right
5.6 录制bag包
在bag保存路径打开终端
1
rosbag record -O camera_calibration_4Hz /infra_left /infra_right /color
录制时的要求：通过rviz界面观察，手持相机对准标定板。偏航角左右摆动3次，俯仰角摆动3次，滚转角摆动3次，上下移动3次，左右移动3次，前后移动3次，然后自由移动一段时间，摆动幅度要大一点，让视角变化大一点，但是移动要缓慢一点，同时要保证标定板在3个相机视野内部，整个标定时间要在90s以上更好。
5.7 使用kalibr标定
1
2
cd ~/Calibration/Kalibr_ws #路径仅供参考
source ./devel/setup.bash
关于第三句命令， 需要根据自己的情况修改。修改说明如下
首先是rosrun kalibr kalibr_calibrate_cameras， 第二段--target ~/Calibration/D455/Aprilgrid.yaml（改为自己标定板yaml文件的路径）， 第三段--bag ~/Dataset/Calibration_D455/640_480/camera_calibration_4Hz.bag， 第四五六段--models pinhole-radtan pinhole-radtan pinhole-radtan这句话不用改，表示三个摄像头的相机模型和畸变模型（详细解释参考Supported models · ethz-asl/kalibr Wiki · GitHub） 第七段--topics /infra_left /infra_right /color话题名称，按我之前步骤来的不用改， 第八段----bag-from-to 5 169 表示处理bag中5-169秒的数据，根据自己录制的bag来确定（通过rosbag info+bag名称 来查看）， 第九段--show-extraction表示显示检测特征点的过程 第十段----approx-sync 0.04是三个相机时间同步的容许误差（可以不加这段试试，有的会标定失败）
综合起来，对于我自己的情况，完整命令如下：
1
rosrun kalibr kalibr_calibrate_cameras --target ~/Calibration/D455/april_6x6_A4.yaml --bag ~/Calibration/D455/camera_calibration_4Hz.bag --models pinhole-radtan pinhole-radtan pinhole-radtan --topics /infra_left /infra_right /color --bag-from-to 5 169 --show-extraction --approx-sync 0.04
标定过程可能有点漫长…………….
如果出现报错：cannot import name NavigationToolbar2Wx
将PlotCollection.py中的NavigationToolbar2Wx换成NavigationToolbar2WxAgg
最终得到的三个结果文件保存在了kalibr的工作空间下，将文件移动到自己保存结果的文件夹里。看一下camera_calibration_4Hz-report-cam.pdf中的三个相机重投影误差reprojection error，至少也得在1px以内吧，下面是我标定的结果。
六、 IMU+双目相机联合标定
6.1 编写chain.yaml文件
编写chain.yaml并保存（能找到就行，我还是放在了~/Calibration/D455/640_480下），根据自己之前IMU和相机标定的结果，参照下面的格式编写（注意修改成自己的！！！）：
cam0: camera_model: pinhole distortion_coeffs: [0.006252205171105866, -0.0040039163191851815, 0.0028253923865729047, 0.0014894484127909796] distortion_model: radtan intrinsics: [387.3061040278026, 388.70426392594715, 321.2185553432099, 243.710879951254] resolution: [640, 480] rostopic: /infra_left cam1: T_cn_cnm1: - [0.9999988621914934, -0.0012557567512840304, 0.0008358771987739831, -0.09586169575038382] - [0.0012560162153501813, 0.9999991631746745, -0.0003099567850981058, -0.0002414148329795646] - [-0.0008354872689652846, 0.000311006307742342, 0.9999996026179666, -0.0001722652642823169] - [0.0, 0.0, 0.0, 1.0] camera_model: pinhole distortion_coeffs: [0.006593825120537277, -0.008671743399946441, 0.0037681282760781776, 0.0014848895024164256] distortion_model: radtan intrinsics: [388.18404676436523, 389.27254650876796, 321.4035775541331, 243.8629884469389] resolution: [640, 480] rostopic: /infra_right
其中，T_cn_cnm1:表示的是左目相机到右目相机的旋转和平移。
6.2 录制bag包
1
2
3
4
roscore
rviz
roslaunch realsense2_camera rs_imu_stereo_calibration_640.launch
rosrun rqt_reconfigure rqt_reconfigure ##关闭结构光
修改相机和IMU发布的频率（这里录制的画面包括了/color相机，我这边是为了以后使用，要是不需要，只用于标定的话可以不录制color的话题即把下面第三句删掉）：
1
2
3
4
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
在bag的保存路径打开终端并录制（只录制两个相机画面的这里不需要/color）
1
rosbag record -O imu_stereo_640.bag /infra_left /infra_right /color /imu
6.3 联合标定
1
2
cd ~/Calibration/Kalibr_ws #路径仅供参考
source ./devel/setup.bash
运行命令（参照之前相机标定过程修改）
1
rosrun kalibr kalibr_calibrate_imu_camera --bag ~/Dataset/Calibration_D455/640_480/imu_stereo_640.bag --cam ~/Calibration/D455/640_480/second/chain.yaml --imu ~/Calibration/D455/IMU/imu.yaml --target ~/Calibration/D455/Aprilgrid.yaml --bag-from-to 5 136 --show-extraction
标定结果的四个文件还是放在kalibr的工作空间下，打开imu_stereo_640-results-imucam.txt查看，重投影误差Reprojection error，两个相机的mean都起码都得在0.5以下。
至此，相机和IMU各自的标定和联合标定的工作都已完成，下一步就是根据标定结果跑自己的算法了。下面将以VINS-Fusion算法运行自己用Realsense D455相机录制的数据包。
七、 使用D455运行VINS-FUSION
7.1 VINS-FUSION的安装编译
1.安装ROS 根据你的Ubuntu版本安装对应的ROS版本，安装过程可以参照ROS官方教程 2.安装ceres可以参照ceres官方教程. 3.下载编译VINS-FUSION
1
2
3
4
5
6
7
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。
7.2 使用D455运行VINS-FUSION
7.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文件不需要重新编译工程
7.2.2 修改realsense-ros中的launch文件
进入到~/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch中打开rs_camera.launch（可以对原先的文件做一个备份）
1.打开双目，修改图片的width、height
2.打开imu的加速度计和陀螺仪并合并为一个topic
保存，修改launch不需要重新编译工程
7.2.3 运行VINS-FUSION
1.打开一个终端 ，用roslaunch 打开realsense摄像头
1
2
source ~/Packages/RealsenseRos_ws/devel/setup.bash
roslaunch realsense2_camera rs_camera.launch
2.打开一个终端，用于打开rviz
1
2
source ~/catkin_ws/devel/setup.bash
roslaunch vins vins_rviz.launch
3.打开一个终端，跑起来
1
2
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
注：如果轨迹特别抖，则重复上述操作，一般要多标定几次才行。
