【ZED&SLAM】Ubuntu18.04系统ZED 2i双目相机SDK安装、联合标定、SLAM测试

0.设备、环境和说明

笔记本电脑i5-8300H、GTX 1060、32GRAM
因为后面要测试Vins-Fusion和ORB-SLAM3，所以推荐安装Ubuntu 18.04（或者Ubuntu 20.04） + ROS 1（不建议用比Ubuntu18更低的版本）
ROS一键安装命令：

wget http://fishros.com/install -O fishros && . fishros

ZED 2i：双目相机配有9轴IMU
此前电脑已经配置好：Ubuntu 18.04，ROS 1，Vins-Fusion，OpenCV 3.2.0，ceres-solver 1.14.0，CMake 3.12，Eigen 3.3.3，g2o（20年版本），Pangolin-0.6，Kalibr，imu_code等。

相机的usb接口要插在电脑的usb3.0协议的插口中。

1.安装ZED SDK3.8.2

官方手册Linux版本: https://www.stereolabs.com/docs/installation/linux/

SDK3.8版本下载链接：https://www.stereolabs.com/developers/release/3.8/

选择静默安装即可，跳过CUDA检查。ZED相机依赖CUDA加速，所以电脑必须要安装CUDA！

The installer can be launched in silent mode with the – silent option (with a space between – and silent).

检查cuda的版本号，没有返回任何信息就是没有安装cuda

cat /usr/local/cuda/version.json

参考的安装CUDA的博客：https://blog.csdn.net/h3c4lenovo/article/details/119003405?spm=1001.2014.3001.5506

其中关于CUDA的解释：

CUDA 是 ZED SDK 用来在显卡上运行快速 AI 和计算机视觉任务的 NVIDIA® 库。在 ZED SDK 安装过程中，如果您的计算机上未检测到 CUDA，ZED SDK会自动下载并安装 CUDA。

跳过Python-api检查，我不需要

./ZED_SDK_Ubuntu18_cuda11.7_v3.8.2.zstd.run -- silent -- silent skip_python

安装完成后重启系统。安装全程不要加sudo。

z@h:~/zed/tools$ tree
.
├── ZED_Calibration
├── ZED_Depth_Viewer
├── ZED_Diagnostic
├── ZED_Explorer
├── ZEDfu
├── ZED_Sensor_Viewer
└── ZED_SVO_Editor

tools里面有一些小工具，可以检查相机原件是否正常。

例如打开/zed/tools中开发好的小工具，ZED_Calibration可以进行相机标定，ZED_Explorer可以查看相机分辨率和帧率参数以及标定信息等。
后续需要调整帧数和分辨率，所以可以看到ZED相机的配置参数：2k、1080p、720p和VGA，由于传输速度的限制，分辨率越高最大帧数越低。

2.安装ZED-ROS

上一步相当于是安装了相机驱动，保证了相机能够运行，下面是安装ZED相机的ROS节点，使用ROS来控制相机的话题和指令：https://www.stereolabs.com/docs/ros/

先创建一个zed的专属工作空间：

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

zed-ros-wrapper是一个catkin包，github下载链接：https://github.com/stereolabs/zed-ros-wrapper
下载完成后，将zed-ros-wrapper改成zed_ros_wrapper。

找准自己安装的SDK对应的版本，在右侧的release中查找，手动下载压缩包，解压缩后放到src文件夹下。然后安装github中的命令开始编译：

cd ..
rosdep install --from-paths src --ignore-src -r -y

报错：

错误原因比较清楚，是没有找到zed_interfaces这个依赖项，跟rosdep有关错误的一般是网络问题。依然是找到对应的版本v1.2：https://github.com/stereolabs/zed-ros-interfaces/releases/tag/v1.2，依然是改名为zed_ros_interfaces，解压缩后直接放到src文件夹下：

z@h:~/zed_ws$ tree -L 2
.
└── src
    ├── CMakeLists.txt -> /opt/ros/melodic/share/catkin/cmake/toplevel.cmake
    ├── zed_ros_interfaces
    └── zed_ros_wrapper

继续重新上面的操作：

## 直接make编译即可
cd ..
catkin_make -DCMAKE_BUILD_TYPE=Release

安装完成！

为了后续启动方便，可以编辑环境变量将节点添加进去：

gedit ~/.bashrc
## 添加：
source ~/zed_ws/devel/setup.bash

ZED 2i附加驱动、功能很丰富，不需要的例如zed-ros-examle可以不安装。

3.ROS相关命令

ROS是一个负责通讯的机器人控制系统，其信号连接是通过节点之间的发布和接收。
正常来说，如果没有启动roscore打开ros是没有任何话题和节点的。
所以我们在终端中输入，打开相机，观察相机正面Logo旁边会亮起蓝色指示灯：

roslaunch zed_wrapper zed2i.launch

a.查看节点，负责通信

rosnode list

b.查看话题

rostopic list

c.查看节点关系图（两端是节点，中间用话题传输）

rosrun rqt_graph rqt_graph

d.查看话题信息
例如打开相机后查看imu这个话题下的信息，确实就是查看imu实时工作状态。

rostopic echo /zed2i/zed_node/imu/data

e.查看话题频率

rostopic hz /zed2i/zed_node/imu/data

f.用rqt来看（rostopic list的可视化形式）

rosrun rqt_topic rqt_topic

勾选前面的对号，可以查看传输带宽和频率

g.修改话题频率

rosrun topic_tools throttle messages {topic1}  {想要修改的频率}  {topic2}

举例：

rosrun topic_tools throttle messages /zed2i/zed_node/imu/data 20.0 /zed2i/zed_node/imu/data_20

再次rostopic hz这个话题/zed2i/zed_node/imu/data_20

rostopic hz /zed2i/zed_node/imu/data_20

这些是后续运行SLAM常用的几个查看、修改指令，因为在标定过程中需要降低相机频率实现稳定等情况。

所有强烈建议使用X-超级终端窗口，不要使用Ubuntu自带的了。

sudo apt-get install terminator

Ctrl+Shift+O 和 Ctrl+Shift+E 实现一个窗口纵向和横向分栏。

可能遇到的小问题：
1.可能遇到Ctrl+Shift+E在terminator中失效的情况，这是因为安装的搜狗输入法快捷键冲突了。
2.有时候快捷键打不开terminator或者直接失灵了，检查一下你的Python版本是不是更换了，因为安装的时候默认绑定到了你的当前Python上。

h.录制rosbag

## 录制所有话题
rosbag record -a

#为文件命名
rosbag record -O bag_name.bag /topics.. 
#为文件名字做前缀
rosbag record -o bag_name /topics.. 

关于rosbag的录制和播放命令很多，详细可以查看官网或者参考博客：rosbag数据记录工具命令详解

4.双目相机标定

4.1 说明

像之前的介绍INDEMIND相机标定的博客一样，依然使用kalibr工具箱来标定。

Kalibr工具箱安装参考博客：【INDEMIND相机运行SLAM】

首先相机标定对稳定性要求很高，所以要对相机降频为4hz（官方推荐）；其次要打开一个可视化窗口来观察是否保证整个标定板信息全部位于图像中央；最后供kalibr处理的rosbag里面的数据格式（分辨率）要和后续测试数据相同（这一点容易被忽视）。

文献Porting A Visual Inertial SLAM Algorithm To Android Devices中：
These two different inputs led to the calibration results printed in Table 1 & 2. …Modern flat-panel displays serve the purpose of a perfect plane reasonably well.
可以看出，与显示器屏幕的重投影误差相比，印刷棋盘的误差要打大。其原因大概是由于打印机墨水量较多，打印的纸张上出现了轻微不均匀的波浪。表面不完全平坦导致误差增加两倍以上。现代平板显示器相当好的实现了完美平面的目的。

所以在标定的时候用显示器是可以的。

标定板我选择的是kalibr官方提供的二维码格Aprilgrid：https://github.com/ethz-asl/kalibr/wiki/downloads

kalibr官方介绍：

The camera system is fixed and the calibration target is moved in front of the cameras to obtain the calibration images.
It is recommended to lower the frequency of the camera streams to around 4 Hz while capturing the calibration data. This reduces redundant information in the dataset and thus lowering the runtime of the calibration.
相机系统固定，标定目标在相机前面移动以获得标定图像。
建议在捕获校准数据时将相机流的频率降低至 4 Hz 左右。这减少了数据集中的冗余信息，从而缩短了校准的运行时间。

4.2 修改相机分辨率和话题频率

（1）分辨率（注意下采样设置）

在zed_ws/src/zed_ros_wrapper/zed_wrapper/params中找到common.yaml通用配置文件，第26行general栏中找到resolution，默认情况下是720p&15hz。只是修改了配置文件，不需要重新make。

    resolution:                 2                              # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA

    # 修改后
    resolution:                 3                              # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA

重新打开相机，例如查看相机右目信息，发现ZED相机分辨率是原来的1/2，为啥呢？

rostopic echo /zed2i/zed_node/right/camera_info

再回到common.yaml中，找到video栏中的图像下采样因子（ img_downsample_factor）设置为1.0。下采样是为了降低图像分辨率，减少内存计算，但是为造成图像过度失真，我们已经选择了VGA最低分辨率，所以可以不需要下采样：

video:
    img_downsample_factor:      1.0                             # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image.

这样操作修改之后，重新查看分辨率info，分辨率为672*376。

也可以选择分辨率为720P，下采样选择0.5，最后实际分辨率为640*360。 这个我没试过。

（2）帧率（两种方法）

设置帧率有两种方法，第一种是跟上面一样，在配置文件common.yaml中设置配置参数，然后重启相机；

再打开common.yaml文件，在第19行发布帧率（pub_frame_rate）修改为20hz。

下方的抓取帧率（grab_frame_rate）是指从SDK硬件驱动中读取的原生帧率，设置为30hz。

这里的逻辑是，pub_frame_rate要小于等于grab_frame_rate，前者决定你最后实际的帧率
不是太理解为什么要设置两个开关…

第二种方法是利用topic_tools修改，这个是针对任何设备都可以修改的。

rosrun topic_tools throttle messages /zed2i/zed_node/left/image_rect_gray 4.0 /zed2i/zed_node/left/image_rect_gray4hz
rosrun topic_tools throttle messages /zed2i/zed_node/right/image_rect_gray 4.0 /zed2i/zed_node/right/image_rect_gray4hz

用ros topic_tools来修改

但是这里也有一个问题，我已经修改成4hz为了，用rqt或者hz查看是3.4左右？

zed github上也有人问类似的问题：
1.录制 Rosbag 时相机掉帧
2.ZED2i图像主题率无法通过common.yaml更改

4.3 可视化窗口（两种方法）

（1）image_view

可以直接用ROS提供的接口Viewer来调用话题显示，类似于这样：

## 直接打开双目
rosrun image_view image_view image:=/zed2i/zed_node/stereo/image_rect_color

（2）RViz

也可以使用rviz，rviz是ros系统下的一个可视化工具，可以添加需要的话题来显示。

## 启动rviz
rosrun rviz rviz

默认打开是什么话题都没有的，需要什么就添加。

这里说明一点，工业相机与网络usb摄像头（webcam）或者手机摄像头有区别的是，工业相机是做了出厂标定的，所以在相机话题下有类似raw-image和rect-image两种话题。

点击左边下方Add按钮，切换到By topic界面，我们之前是打开了zed-ros的节点的，所有zed相机的所有话题都在其中显示。这里选择/zed2i/zed_node/stereo/image_rect_color的Image，点击OK。

说明一下，双目相机左右目是以前进方向为准，不要搞反了。

4.4 双目相机标定

（1）kalibr Camera标定

kalibr标定更加离线一点，是先打包，后处理rosbag格式的数据。这意味着需要用的rosbag record命令。

在标定之前先理清楚需要将什么话题添加到bag当中，实现检查一下是否达到了需要的分辨率和帧数。

## 右目原始话题
/zed2i/zed_node/right_raw/image_raw_gray
## 左目原始话题
/zed2i/zed_node/left_raw/image_raw_gray

注意给摄像头降频：

rosrun topic_tools throttle messages /zed2i/zed_node/right_raw/image_raw_gray 4.0 /zed2i/zed_node/right_raw/image_raw_gray4hz
rosrun topic_tools throttle messages /zed2i/zed_node/left_raw/image_raw_gray 4./zed2i/zed_node/left_raw/image_raw_gray4hz

录制rosbag：

rosbag record -O cam.bag /zed2i/zed_node/left_raw/image_raw_gray4hz /zed2i/zed_node/right_raw/image_raw_gray4hz

kalibr标定工具进行标定

source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --bag /home/z/zed_ws/test/raw_cam_4hz/cam.bag --topics /zed2i/zed_node/left_raw/image_raw_gray4hz /zed2i/zed_node/right_raw/image_raw_gray4hz --models pinhole-radtan  pinhole-radtan --target /home/z/zed_ws/test/raw_cam_4hz/april_6x6_80x80cm.yaml --show-extraction

可以用- -bag-freq 4.0设置频率（学到了）

遇到的问题：

1.Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.

解决办法：终端中设置变量 KALIBR_MANUAL_FOCAL_LENGTH_INIT = 1 ，然后程序运行时手动给相机设置初始焦距。后面代码计算过程中会让我们输入给定的参数，一般设置为400或者500合适。这一步对最后的标定结果影响不大。

出现原因：相机角度太偏，通过–show命令来调出视角发现很多帧都没有detect到角点信息。

2.到底是使用raw还是rect？

一开始我的想法是在rect修正后的基础上进行标定，是不是会更好一点呢

通过在4hz（官方github）和20hz（中文博客）下标定，以出厂标定为准，发现raw_4hz话题下更接近出厂标定参数。

raw_4hz:

rect_4hz:

raw_20hz（运算量太大了）:

在zed/tools中Exploer：

最后选择了4hz的标定结果：

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.07080077760685485, -0.0424018228332835, 0.00022068947815120048, 6.697686252415628e-05]
  distortion_model: radtan
  intrinsics: [477.69373173433104, 477.87033668767555, 341.70467807850673, 191.9403406313537]
  resolution: [672, 376]
  rostopic: /zed2i/zed_node/left_raw/image_raw_gray4hz
cam1:
  T_cn_cnm1:
  - [0.9999885217251627, -0.002266971115991657, 0.004221049618012356, -0.12093465914416278]
  - [0.0022689986540341215, 0.9999973127151027, -0.00047561294730666463, -0.0003788661271713224]
  - [-0.004219960074035542, 0.0004851850439924218, 0.9999909782255244, 0.001691906030275231]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.07767291819090658, -0.025796049122962336, -8.188702736794962e-05, -0.0010006476827473499]
  distortion_model: radtan
  intrinsics: [478.1991541708856, 478.3316121759839, 336.32320067041985, 197.34686534720404]
  resolution: [672, 376]
  rostopic: /zed2i/zed_node/right_raw/image_raw_gray4hz

T_cn_cnm1:两个摄像头的变换矩阵T，第一行最后一个平移x为双目相机基线长度，刚好是符合出厂参数12cm。

5.IMU标定

我将相机静置了12h，使用频率为默认的200hz，用rosbag记录了话题 /zed2i/zed_node/imu/data。

rosbag record -O imu.bag /zed2i/zed_node/imu/data

使用的标定工具为imu_utils，之前博客中有介绍。

标定了三次，对比一下，贴出我的标定结果：

%YAML:1.0
---
type: IMU
name: ZED_12h_200hz
Gyr:
   unit: "gyr_n: rad / sqrt(s), gyr_w: rad / s^2 / sqrt(Hz)"
   avg-axis:
      gyr_n: 8.3453896153927679e-05
      gyr_w: 3.6885674003742779e-09
   x-axis:
      gyr_n: 8.0206203826638738e-05
      gyr_w: 2.0173606656839313e-11
   y-axis:
      gyr_n: 8.3831155222455024e-05
      gyr_w: 1.1032848952812728e-08
   z-axis:
      gyr_n: 8.6324329412689287e-05
      gyr_w: 1.2679641653266916e-11
Acc:
   unit: "acc_n: m / s^2 / sqrt(Hz), acc_w: m / s^3 / sqrt(Hz)"
   avg-axis:
      acc_n: 9.3187660062493856e-04
      acc_w: 3.8057416284488521e-06
   x-axis:
      acc_n: 9.8972867124666529e-04
      acc_w: 1.9950036416311564e-06
   y-axis:
      acc_n: 8.7377239353805889e-04
      acc_w: 3.2256034468604165e-09
   z-axis:
      acc_n: 9.3212873709009171e-04
      acc_w: 9.4189956402685393e-06

过程遇到的问题：

在launch文件中设置的最小时间为720，播包速度为200倍。但是播包都结束了，launch终端还是wait for imu data…
后来一步步排查测试，发现是播包速度太快，可能是硬盘读取速度的问题。
总之：先用rosbag info来查看整个过程持续的时间，然后设置一个最小时间一定要小于bag的时间。最后不要播包速度太快，200倍不行可以100倍。

6.双目+IMU联合标定

准备好之前几步标定过程得到的文件，并且按照格式整理好

参考格式：https://github.com/ethz-asl/kalibr/wiki/yaml-formats

事先修改频率：相机改为30hz，imu保持200hz

下面是采集联合数据打包，官方视频教程：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration

打开一个可视化窗口：

rosrun image_view image_view image:=/zed2i/zed_node/stereo/image_rect_color

标定打包：

rosbag record -O cam_imu.bag /zed2i/zed_node/left_raw/image_raw_gray /zed2i/zed_node/right_raw/image_raw_gray /zed2i/zed_node/imu/data

手法：三轴平移+三周旋转，每个自由度重复做2-3次，最后空中画8两圈。

kalibr标定:

source ~/kalibr_workspace/devel/setup.bash
rosrun  kalibr kalibr_calibrate_imu_camera --target april_6x6_80x80cm.yaml --bag cam_imu.bag --cam cam-camchain.yaml --imu imu.yaml

标定的结果图会在pdf中显示，之前的双目相机标定就误差就挺大的了，接近于1pix

得到了联合标定的参数文件，作为后面的config.yaml的输入：

cam0:
  T_cam_imu:
  - [0.013235730687675612, -0.9999051792716815, 0.003801039177760257, 0.023164130906256988]
  - [0.000946182400434592, -0.0037888460392637158, -0.9999923746632058, 0.026579126633135358]
  - [0.9999119562101604, 0.013239226237144797, 0.0008959445369374763, -0.015714267514029625]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.07080077760685485, -0.0424018228332835, 0.00022068947815120048, 6.697686252415628e-05]
  distortion_model: radtan
  intrinsics: [477.69373173433104, 477.87033668767555, 341.70467807850673, 191.9403406313537]
  resolution: [672, 376]
  rostopic: /zed2i/zed_node/left_raw/image_raw_gray
  timeshift_cam_imu: 0.001825245182411367
cam1:
  T_cam_imu:
  - [0.017454111776955716, -0.9998292294498397, 0.006071731204306596, -0.09789737893741968]
  - [0.000500640640348271, -0.00606391611089474, -0.9999814889688454, 0.026260222371213802]
  - [0.999847540048495, 0.01745682843874702, 0.00039471487606496103, -0.014107075626026234]
  - [0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:
  - [0.9999885217251638, -0.002266971115991657, 0.004221049618012358, -0.12093465914416278]
  - [0.0022689986540341215, 0.9999973127151038, -0.00047561294730666463, -0.0003788661271713224]
  - [-0.004219960074035543, 0.0004851850439924218, 0.9999909782255255, 0.001691906030275231]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.07767291819090658, -0.025796049122962336, -8.188702736794962e-05, -0.0010006476827473499]
  distortion_model: radtan
  intrinsics: [478.1991541708856, 478.3316121759839, 336.32320067041985, 197.34686534720404]
  resolution: [672, 376]
  rostopic: /zed2i/zed_node/right_raw/image_raw_gray
  timeshift_cam_imu: 0.0030721108460722776

7. VINS-Fusion配置文件

要将标定好的相机参数写入yaml文件中vins在运行的时候才能读取。

之前用INDEMIND相机配置：https://blog.csdn.net/qq_45306739/article/details/130642482?spm=1001.2014.3001.5501

之前测试都用的是VINS-Mono，那个的配置文件简单好写，VINS-Fusion有所改变，就以要改动最多的双目惯导配置为例：

包含这三个文件，相比于之前的是将两个目的相机参数写在外面了:

├── left.yaml
├── zed_stereo_imu_config.yaml
└── right.yaml