0.设备、环境和说明
笔记本电脑i5-8300H、GTX 1060、32G RAM 后续一些工作转移到了PC上：i7-12700 因为后面要测试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_utils，anaconda，CUDA等。
相机的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/
选择静默安装即可。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
SDK中关于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。 完成后找到usr/local/zed（我把zed移动到主目录下了）
z@h:~/zed/tools$ tree
.
├── ZED_Calibration
├── ZED_Depth_Viewer
├── ZED_Diagnostic
├── ZED_Explorer
├── ZEDfu
├── ZED_Sensor_Viewer
└── ZED_SVO_Editor
tools文件夹里面有一些小工具，可以检查相机原件是否正常。 例如ZED_Calibration可以进行相机标定，ZED_Explorer可以查看相机分辨率和帧率参数以及标定信息等。
后续需要调整帧数和分辨率，所以可以看到ZED相机的配置参数：2k、1080p、720p和VGA，由于传输速度的限制，为了控制计算量，通常情况下分辨率越高，支持到的最大帧数越低。
2.安装ZED-ROS
上一步相当于是安装了相机驱动，保证了相机能够运行，下面是安装ZED相机的ROS节点，使用ROS来控制使用相机 ZED安装官网：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
手动下载后不需要用rosdep来下载了，因为网络问题需要改代理。直接make编译！
## 直接make编译即可
cd ..
catkin_make -DCMAKE_BUILD_TYPE=Release
安装完成！
为了后续启动方便，可以编辑环境变量将source添加进去：
gedit ~/.bashrc
## 添加：
source ~/zed_ws/devel/setup.bash
zed-ros-examle可以不安装。
我们在终端中输入，打开相机，观察相机正面Logo旁边会亮起蓝色指示灯：
roslaunch zed_wrapper zed2i.launch
3.ROS常用指令了解
ROS是一个负责通讯的机器人控制系统，其信号连接是通过节点之间的发布和接收。
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常用的几个查看、修改指令，因为在标定过程中需要降低相机频率实现稳定等情况。
h.录制rosbag
## 录制所有话题
rosbag record -a
#为文件命名
rosbag record -O bag_name.bag /topics..
#为文件名字做前缀
rosbag record -o bag_name /topics..
关于rosbag的录制和播放命令很多，详细可以查看官网或者参考博客：rosbag数据记录工具命令详解
4.双目相机标定
4.1 说明
高分辨率图像会影响算法性能，并且分辨率和频率成反比。在SLAM应用里，我们更偏向频率。
使用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
Aprilgrid的特点：其是有顺序的，如果你用标定板记得不要拿反，每一个二维码格都是有id的。
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的下采样）
在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。 但后续试验是以672*376为准，主要是为了对比出厂参数。
（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
但是这里也有一个问题，我已经修改成4hz为了，用rqt或者hz查看是3.4左右啊？
ZED github上也有人问类似的问题：
1.录制 Rosbag 时相机掉帧 2.ZED2i图像主题率无法通过common.yaml更改
ZED相机在usb3.0的接口下还是会掉帧，因为我用的是老版本的SDK，不知道新版本的SDK修复了这一bug没有。 ZED-ROS作者说是ROS的原因，彳亍！ 总结：相机和IMU修改频率的方法都尽量用第一种修改yaml的方式
4.3 打开可视化窗口
（1）image_view
可以直接用ROS提供的接口Viewer来调用话题显示，类似于这样：
## 直接打开双目
rosrun image_view image_view image:=/zed2i/zed_node/stereo/image_rect_color
但是这里有个很蛋疼的事情 -.- 窗口大小是固定的，这也太占地方了！所以我更喜欢用rviz来监视
（2）RViz
rviz是ros系统下的一个可视化工具，可以添加需要的话题来显示。
## 启动rviz
rosrun rviz rviz
默认打开是什么话题都没有的，需要什么就添加。
点击Add，切换到By topic界面，我们之前是打开了zed-ros的节点的，所有zed相机的所有话题都在其中显示。这里选择/zed2i/zed_node/stereo/image_rect_color的Image，点击OK。
这里说明一点，工业相机与网络usb摄像头（webcam）或者手机摄像头有区别的是，工业相机是做了出厂标定的，所以在相机话题下有类似raw-image和rect-image两种话题。双目相机左右目是以前进方向为准，不要搞反了。
后文有对rect和raw这话题如何选取的分析
4.4 相机标定
（1）kalibr Camera标定
kalibr标定更加离线一点，是先打包，后处理。这意味着需要用的rosbag record命令。
标定一定要记录正确的话题，检查一下分辨率和帧数是否修改。双目相机标定需要的话题：
## 右目话题
/zed2i/zed_node/right/image_rect_gray
## 左目话题
/zed2i/zed_node/left/image_rect_gray
录制rosbag：
rosbag record -O cam.bag /zed2i/zed_node/right/image_rect_gray /zed2i/zed_node/left/image_rect_gray
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/right/image_rect_gray /zed2i/zed_node/left/image_rect_gray --models pinhole-radtan pinhole-radtan --target /home/h/zed_ws/calibrate/cam/april_6x6_80x80cm.yaml --show-extraction
录制过程20hz，但是想用4hz：可以用- -bag-freq 4.0设置播包频率，类似于抽帧
标定好的yaml文件：
cam0:
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.0030377843309094005, 0.004210524674852196, 0.00019855973400005665, 5.749987456220585e-05]
distortion_model: radtan
intrinsics: [478.96600313819334, 479.3232440904635, 344.2341502361638, 196.48383632179534]
resolution: [672, 376]
rostopic: /zed2i/zed_node/left/image_rect_gray
cam1:
T_cn_cnm1:
- [0.9999999757228988, 7.782186907499867e-05, 0.00020615031581389614, -0.12041395979740278]
- [-7.7973658081506e-05, 0.9999997258198265, 0.0007363968866463767, -1.6048854091668036e-05]
- [-0.00020609295150946752, -0.0007364129430630358, 0.9999997076107907, -0.00012147493813981137]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.00803712578699968, 0.011455857217680072, -3.654819836873565e-05, -0.0008413343274505274]
distortion_model: radtan
intrinsics: [478.94609638482524, 479.3824623913225, 343.53437679115063, 195.81784799794175]
resolution: [672, 376]
rostopic: /zed2i/zed_node/right/image_rect_gray
重投影误差效果图：
问题：
1.Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.
解决办法：终端中设置变量 export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1（在rosrun之前先输入），然后程序运行时手动给相机设置初始焦距。后面代码计算过程中会让我们输入给定的参数，一般设置为400或者500合适。这一步对最后的标定结果影响不大。
但是！！Kalibr标定出问题，最好的方式就是重新录制！
出现原因：相机角度太偏，通过–show命令来调出视角发现很多帧都没有detect到角点信息。
2.到底是使用raw还是rect？
使用rect话题就可以了，虽然有的博客说直接用出厂参数，但是后续涉及到联合标定，并不符合kalibr的文件格式，也是不怎么方便的。
经过n多次的相机标定，发现只要手法正确，采集环境合理，标定出来的内参和出厂内参其实差的并不多（尽管kalibr给出的误差看起来有点大…）
而且我感觉出厂参数效果并不是很好啊…，后面的联合标定重投影误差都飘到天上去了
3.关于相机频率设置问题，20Hz or 4Hz？
结论：都可以
看了很多博客，大多数中文博客对此没有过多解释，为了方便用20hz的居多。而外文博客用4hz的居多（官方推荐）。
我的理解是差别并不大，考虑到我们并没有专业设备（特制标定板）和合适的标定环境（其实最好是在阴暗环境中），频率反而成为了次要的问题，尽量使用rect话题录制和熟练稳定的手法倒是比较重要的。
当然，如果你用20hz来录制，尽量还是要慢一些，否则后续kalibr很容易无法detect角点进而失败，出现问题1。
4.重投影误差效果看起来很差怎么办（那个红色圆圈很大）？
关于这个问题，似乎没有一个合理一致的答案…
看过的中文博客中，有人用rs相机标定出接近0.1个像素的误差，也有人用zed相机标定的接近1。这个很大程度上取决于标定设备和环境。
比如我使用的这款zed 2i，我用的标定板是一个27寸的显示器，无论是用raw还是rect，效果甚至都是差不多的。都是接近于1个像素点的误差。
结论：就这个误差范围而言，0.5以内确实是比较好的，1以内也是完全可以接受。只要相机标定大部分像素点在1个pixel内，是可以继续做的。
（2）ZED tools标定
别用，依托shit。
及其不稳定的同时，问题还贼多。标定两次结果差别巨大，你信谁？
（3）MATLAB工具箱标定
后续补充~
（4）ROS标定
后续补充~
5.IMU标定
5.1 imu_utils工具（港科大版）
将相机按照前进方向放到地上，静置了12h，使用频率为默认的200hz，用rosbag记录了话题 /zed2i/zed_node/imu/data。
rosbag record -O imu.bag /zed2i/zed_node/imu/data
使用的标定工具为imu_utils，之前博客中有介绍。
这里标定了12+小时，也就是720多分钟，但是配置文件中我写的是700，因为720算不出来，一直卡wait for imu data…
标定了三次，对比一下，贴出我的标定结果：
%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
问题：
1.播包结束终端还是wait for imu data…
纯玄学问题！就是因为这个问题我才换了一套工作环境，以为是算力不够…
后来一步步排查测试，发现是播包速度太快，可能是硬盘读取速度跟不上的问题。
结论：先用rosbag info来查看整个过程持续的时间，然后设置一个最小时间一定要小于bag的时间，不行的话就再小一点。
2.imu话题频率如何设置？&& 为什么不用官方推荐的400Hz？
同上面的相机标定一样，这个问题似乎也是没有准确的答案。在我看来更多的是根据不同传感器的型号而有所不同。github有个issue中提到设置为最大…
考虑到数据量（后续运算量）和实际情况（zed相机400hz下实际只能跑到360hz），我最终选择了大多数人选择的200hz。
给个参考：只标定这一个话题12小时数据量大概是3.5GB。
3.每次标定结果不相同？甚至差别很大？
不是很理解，从第一篇博客Android手机离线采集数据就有这个疑问了。
是单核CPU计算的问题吗？
虽然不知道为什么每次都会差一点，标定了五次甚至是五个差别很大的数字。但是之前在博客中提到过，几次的结果越接近，说明效果是越好的。
4.imu应该被静置多久？
结论：越久越好
5.2 allan_variance_ros工具（kalibr官方推荐）
之前一直使用的是imu_utils来标定imu，在手机等廉价触感器上似乎这并不影响什么，我严重怀疑廉价imu甚至不需要严格的标定参数。 也就意味着直接使用公制参数效果也是很可观的。
但是上面的utils工具似乎在zed相机上表现并不好，那就换一种工具来测试：https://github.com/ori-drs/allan_variance_ros
之前并没有用过，网上教程也几乎没有，问题也不少，所以从头开始编译！
事先说明：要安装很多包，所以我创建了一个conda环境专门来干活，Python版本3.8.18
同之前的方法一样创建一个allan_ws工作空间，catkin_init_workspace初始环境，catkin_make在allan_ws下编译。
数据包就继续使用之前以频率200hz，话题为/zed2i/zed_node/imu/data，标定了12个小时的rosbag为测试数据。
第一步：
从官方教程中可以看出，最后处理的数据格式是csv file。所以需要将rosbag分解成csv。当然在这之前教程中说需要按照时间戳重新整理时序。
rosrun allan_variance_ros cookbag.py --input imu.bag --output imu_cooked.bag
运行完成后，查看两个rosbag的区别：
第二步：
切记：先将imu.bag放到其他地方！
文件夹中只保留imu_cooked.imu，参考示例：
## 一定要注意：第一个参数是包含rosbag的路径，而不是具体到bag文件！否则就是“核心已转储”
rosrun allan_variance_ros allan_variance [path_to_folder_containing_bag] [path_to_config_file]
运行前要打开roscore：
rosrun allan_variance_ros allan_variance /home/h/zed_ws/calibrate/imu/3rd_allan/ /home/h/zed_ws/calibrate/imu/3rd_allan/zed_allan.yaml
得到csv file文件
第三步：
我发现官方github很扯淡，示例都不准确，翻了好一顿issue才解决问题！
标定出来后我发现话题和频率对不上的原因：要加config的yaml文件！
rosrun allan_variance_ros analysis.py --data allan_variance.csv --config zed_allan.yaml
计算完成后，会得到两张图片和imu.yaml（可以直接用与kalibr联合标定）。大功告成！
报错整理：
1.正常来说新的环境是没有下面这些包的，所以会提示ModuleNotFoundError: No module named ‘***’
pip install pyyaml pycryptodomex gnupg rospkg matplotlib scipy
这一步受很多环境影响，强烈建议用conda虚拟环境来处理！
2.上面的包都pip install成功了，运行上面的整理时序的脚本 cookbag.py，还是会出错： ValueError: compression must be one of: none, bz2
看了一圈没有人解释过，在github的issue中有提到类似的，提供了一个改正思路。
当时ros安装依赖的是python2.7，也就是ubuntu18自带的，python2是不支持lz4压缩的。
查看了cookbag.py的源码，将压缩方法从lz4改成bz2，做了如下的改动：
with rosbag.Bag(args.output, 'w', compression='bz2') as outbag:
改动完成后，还是会提示有错误，但是程序是在运行的，时间可能久一点。
如果想关掉错误提示，去/opt/ros/melodic/lib/python2.7/dist-packages/rosbag/bag.py下把捕获异常关掉~（没必要）
Failed to load Python extension for LZ4 support. LZ4 compression will not be available.
3.rosrun allan_variance_ros allan_variance [path_to_folder_containing_bag] [path_to_config_file] 这一步出现问题，无法找到通讯节点服务
记得提前打开roscore
4.关于标定出的误差参数如何使用问题
kalibr官方新出的一个视频中说到，将随机噪声乘以5倍放缩，将随机游走乘以10倍放缩。还别说，效果还是有的！
改动后的数值写入imu.yaml中，直接作为后续联合标定的传入文件。
imu_allen.yaml：
#Accelerometer
accelerometer_noise_density: 0.00586199308
accelerometer_random_walk: 0.00031251141
#Gyroscope
gyroscope_noise_density: 0.0005018344
gyroscope_random_walk: 2.939491625389424e-06
rostopic: '/zed2i/zed_node/imu/data'
update_rate: 200
6.双目+IMU联合标定
准备好之前几步标定过程得到的文件，并且按照格式整理好
参考格式：https://github.com/ethz-asl/kalibr/wiki/yaml-formats
事先修改频率：相机改为20hz，imu保持200hz
下面是采集联合数据打包，官方视频教程：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration
录制话题：
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_allan.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
左右目yaml就不写了，直接从相机标定的yaml文件中复制过去就行。
在zed_stereo_imu_config.yaml中：
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/zed2i/zed_node/imu/data"
image0_topic: "/zed2i/zed_node/left/image_rect_gray"
image1_topic: "/zed2i/zed_node/right/image_rect_gray"
output_path: "/home/zh/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 672
image_height: 376
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0098577, -0.0030732, 0.99994669, 0.01987704,
-0.99995079, 0.00108633, 0.00986108, 0.0227025,
-0.00111658, -0.99999469, -0.00306234, 0.00350025,
0.0, 0.0, 0.0, 1.0]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.0100636, -0.00233761, 0.99994663, 0.02121027,
-0.99994865, 0.00117156, 0.01006636, -0.09770404,
-0.00119503, -0.99999658, -0.0023257, 0.00334002,
0.0, 0.0, 0.0, 1.0]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation.
gyr_n: 0.01 # gyroscope measurement noise standard deviation.
acc_w: 0.001 # accelerometer bias random work noise standard deviation.
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation.
g_norm: 9.77281881 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.011 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
问题：
1.标定的重投影误差看起来很大
从pdf中的那个红圈圈来看，假如相机标定的重投影误差接近1个pixel，如果imu标定数据较好，联合标定重投影误差基本不变；如果imu给的很离谱，联合标定重投影误差只会更大。
话说回来，虽然标定过程是联合标定受两个因素有关，但是imu的标定过程我们是无法控制的。如果你是用allan工具来标定的，建议重新对双目相机进行标定，去和出厂参数进行对比。
如果确定相机标定没有问题， 再去检查imu。
2.Vins-Fusion配置文件中的外参矩阵如何填写？
这一步是最关键的，联合标定就是为了得到外参矩阵，但是kalibr的结果给的是两个变换矩阵，一个是从imu到cam，一个是cam到imu，刚好是个逆过程。
结论：是这个T_ic: (cam0 to imu0) 不想多说是谁到谁（我也分不清），因为说谁到谁跟容易乱。这个外参矩阵可以在stereo_imu-results-imucam.txt中找到。
3.一动就飘或者呈现一种抖动状态
解决方法：前者直接用Vins-Fuison中人家给的参考值；后者注意相机和imu的时间对齐。 参考链接：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/issues/114#issuecomment-818438875
最后记录：
（1）安装terminator终端
运行相机通常需要启动很多窗口，一个个终端打开很不方便的
sudo apt-get install terminator
快捷键：Ctrl+Shift+O 和 Ctrl+Shift+E，实现一个窗口纵向和横向分栏。
可能遇到的小问题： 1.可能遇到Ctrl+Shift+E在terminator中失效的情况，这是因为安装的搜狗输入法快捷键冲突了。 2.有时候快捷键打不开terminator或者直接失灵了，检查一下你的Python版本是不是更换了，因为安装的时候默认绑定到了你的当前Python上。
（2）关于imu标定参数
我们发现在最后的配置文件中，并没有使用我们的标定参数，而是直接使用了参考值公制参数。也就意味着在传参过程中没有用imu标定结果。这是不是就意味着不需要imu标定了呢？
对于手机的话我觉得影响不大，但是ZED相机的调试还是需要的。多次联合标定的结果说明了如果给的imu标定参数不好，效果是比较差的
参考博客
1.ZED2双目相机运行VINS-FUSION初试 这篇博客开头讲了kalibr标定的效果好坏和实际slam运行起来没有直接的关系，博主本人说标定误差接近到5个像素点，其实这就很大了， 跟许许多多因素有关，博主也说建议使用topics话题下或者settings中config的标定信息。 在联合标定的时候，最好还是自己来标定
精彩链接
发表评论