ZED2运行VINS-MONO初试

上篇博客，已经完成ZED2的标定，获取相关标定参数，将在此基础上进行VINS的相关修改，实现使用ZED2运行VINS-MONO

一 VINS-MONO环境搭建

因为系统环境从ubuntu18.04更改为ubuntu20.04，故需重新搭建环境，可采用数据集测试环境。
具体可根据VINS-MONO实践

1）ceres 编译报错 error: ‘integer_sequence’ is not a member of ‘std’
在编译一些用到ceres的项目的时候会报错 error: ‘integer_sequence’ is not a member of ‘std’，这是因为较新版本中的ceres对c++版本有要求

在报错的项目的CMakeList里的
set(CMAKE_CXX_FLAGS “-std=c++11”)
改成
set(CMAKE_CXX_STANDARD 14)

2）error: ‘CV_RGB2GRAY’ was not declared in this scope
在报错头文件里添加

#include <opencv2/imgproc/types_c.h>

3）编译时遇到报错 error: ‘CV_FONT_HERSHEY_SIMPLEX’ was not declared in this scope
将报错文件上的 CV_FONT_HERSHEY_SIMPLEX 参数改为 cv::FONT_HERSHEY_SIMPLEX

4）将camera_model包改成兼容opencv4

在camera_model包中的头文件Chessboard.h中添加
#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d_c.h>
在CameraCalibration.h中添加
#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgproc/imgproc_c.h>

其余问题可参考https://zhuanlan.zhihu.com/p/432167383

编译成功后，运行roslaunch vins_estimator vins_rviz.launch时，报错

ERROR: cannot launch node of type [rviz/rviz]: rviz
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/sjj/Vins-mono/src
ROS path [2]=/opt/ros/noetic/share

安装rviz即可

sudo apt-get install ros-noetic-rviz

打开四个终端分别运行以下命令：

roscore
roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play MH_01_easy.bag

数据集运行成功：

二 修改VINS配置文件

修改realsense_color_config.yaml文件

1)订阅topics修改

imu_topic: "/zed2/zed_node/imu/data_raw"
image_topic: "/zed2/zed_node/left/image_rect_color"

2)左目相机内参修改

model_type: PINHOLE
camera_name: camera
image_width: 1280
image_height: 720
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 414.926283
   fy: 416.695084
   cx: 653.478486
   cy: 365.478913

这里使用的是校正后的图像，故设置畸变系数均为0；

3)IMU至cam的变换矩阵，参数修改为2，使用在线标定（设置为0，使用已有的标定参数也是可以运行的）：

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 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.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

这里IMU和camera之间的外参矩阵建议使用Kalibr工具进行离线标定，也可以改成1或者2让估计器自己标定和优化。

4)IMU参数，使用VINS-mono中给定的参数

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80       # gravity magnitude

5)不需要在线估计同步时差

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

6)相机改为全局曝光

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

三 运行VINS-MONO

roslaunch zed_wrapper zed2.launch
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

报错：

terminate called after throwing an instance of 'cv::Exception'
  what():  OpenCV(4.2.0) ../modules/core/src/matrix.cpp:423: error: (-215:Assertion failed) 0 <= _rowRange.start && _rowRange.start <= _rowRange.end && _rowRange.end <= m.rows in function 'Mat'

[feature_tracker-1] process has died [pid 4090, exit code -6, cmd /home/sjj/Vins-mono/devel/lib/feature_tracker/feature_tracker __name:=feature_tracker __log:=/home/sjj/.ros/log/ee50516c-4c0a-11ec-86e6-c7de7ab2b409/feature_tracker-1.log].
log file: /home/sjj/.ros/log/ee50516c-4c0a-11ec-86e6-c7de7ab2b409/feature_tracker-1*.log

原因是相机图片的大小，即高宽尺度问题，可查看相关topic查看。

修改后可成功运行，但是一旦没法实时跟踪，位置就开始飘移

怀疑原因是如果突然图像跟踪失败并且没有重定位，轨迹会一直往前方走，这是因为只用到IMU数据时，IMU积分得到的位移、速度和角度值一定会发生漂移。对IMU进行参数标定和适当修改重力加速度的值会减少偏移的程度但不能去除。

尝试降低IMU发布频率，位置跟踪效果变好，但是还需从算法角度提交匹配精度和系统鲁棒性。