VINS_MONO+D435i Realtime运行

艰难地运行了三天,终于算是搞定了,主要的时间和精力都花在标定上了, 总结一些需要注意的点,希望能对同样使用VINS-MONO的人有帮助.

先说一下我的初衷,其实是想要使用ORBSLAM3的Mono_Inertial的,但是总是会初始化失败,图像可以初始化成功,但是在添加IMU数据后整个track就会LOST, 觉得可能是IMU和相机的标定问题,所以想要试一下VINS-MONO的标定效果;

IMU+Camera 标定

我采用的是彩色相机+IMU的形式,所以只需要标定单目,单目标定和相机加IMU标定使用的都是kalibr;　Kalibr的标定方法这里不多说了, 详细的参考了两位博主的总结,其中博主强调的一些点一定一定要注意:古路－Intel RealSense D435i Calibration　Panda-like-me---VINSMONO+D435i

总结一下标定的时候需要注意的一些点:

1. 在不同终端分别运行如下的命令:　

roslaunch realsense2_camera rs_camera.launch
rosbag record -o calib.bag /camera/imu /camera/color/image_raw
rosrun topic_tools throttle messages /camera/imu  200 /imu

这里要注意给ＩＭＵ降频，不过我的降完之后是１５０ＨＺ，　而不是200HZ, 同时将相机的频率30HZ在降频之后也是15HZ,所以我干脆就在rs_camera.launch中设置为15HZ了, 并没有使用降频的命令;

在运行之前也要注意rs_camera.launch中下面两个变量的设置,第一个设置IMU和相机的同步, 第二个输出同步的IMU数据

<arg name="enable_sync"               default="true"/>，
<arg name="unite_imu_method"          default="copy"/>

这些细节注意到了之后就可以用采集好的包进行标定:

kalibr_calibrate_imu_camera --target rs_rgb_imu/aprilgrid.yaml --cam rs_rgb_imu/camchain.yaml --imu rs_rgb_imu/imu.yaml --bag /media/shinan/DATA/ROSbag/realsense/camera_imu_calibration.bag --bag-from-to 5 135 --max-iter 15  --show-extraction

我也是按照建议添加了

--max-iter 15  --show-extraction

第一个是最大迭代次数,第二个显示标定板提取的结果; 我个人觉得标定的过程还是要缓慢一点的移动,不要有过快地运动或抖动; 我的标定板比较小, 80X80的, 采集标定包的时候我一般距离标定板也就50-100cm, 大概采集了两分钟;

相对于我之前的标定方法(就是按照原来rs_camera.launch中的设定),这样标定出来的结果和之前标定出来的结果差距还是蛮大的,但是从标准差来看可能是之前的会好一些:

新的标定结果的标准差:　

旧的标定方法的标准差：

但是对于时间差的标定结果中，新的标定方法将时间差标定为：-0.0533094355641 而旧的标的方法将结果标定为: -0.0183391407762; 本着做最坏的打算的原则,采用了新方法;

个人觉得时间足够的话可以多采集几组数据对比一下, 毕竟新旧两个使用的数据包是不一样的,新方法在数据包的采集上也有改进,所以只进行一次这样的对比可能不够严谨;

运行VINS之前

运行之前,要注意自己电脑上Ceres和Eigen的版本问题, Ceres需要的是Eigen3.2, 我之前的版本是3.3 , 每一次运行VINS在初始化成功之后都会出现段错误, 提示 [vins_estimator-2] process has died, 与 VINS-MONO ISSUE70 一样, 这里把该ISSUE的图片贴上来, 方便没有VPN的朋友查看:

因此, 如果大家遇到这个问题,可以去

/usr/local/include/eigen3/Eigen/src/Core/util　打开其中的Macros.h文件

来查看自己安装的版本,或者在Ceres中message(${EIGEN_INCLUDE_DIR})来输出自己的Eigen包含路径;

运行VINS

这里需要注意的是运行VINS的时候我和前面标定所采用的rs_camera.launch的参数是一样的; 运行VINs-MONO所需命令如下:

roslaunch vins_estimator realsense_color.launch
roslaunch vins_estimator vins_rviz.launch

在VINS指定的config下的参数文件,也需要注意下面参数的设定:

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

ｇ_norm其实可以设定成自己所在区域对应的重力值，比如深圳是9.7, estimate_td 也可以设置成1, 查看一下VINS运行后的标定结果;

运行的时候有一个关于坐标系的问题, 因为SDK版本不同,所以Realsense给出的数据也会有所差别, 主要的差别在坐标轴上; 下面主要说一下这个问题;

以相机坐标系为基准, 为向右-->x, 向下-->y, 向前-->z; 我的Realsense的SDK出来的IMU数据与相机坐标系同向, 如下,为将Realsense正放于桌面上:

header: seq: 1411stamp: secs: 1602851363nsecs: 898209810frame_id: "camera_imu_optical_frame"
orientation: x: 0.0y: 0.0z: 0.0w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: x: -0.00178953423165y: 0.00692256260663z: -2.09028585232e-05
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: x: -0.539947092533y: -9.76134586334z: 0.401100456715
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
---

可以看到重力加速度在y的方向上,而非z向, 这就导致我的标定结果中, 相机和IMU之间的外参(在忽略掉一些轻微的旋转之后)是单位阵, 结果如下:　

extrinsicRotation: !!opencv-matrixrows: 3cols: 3dt: ddata: [0.99996341, -0.00296024, -0.00802547,0.00298187, 0.99999195 , 0.00268508 ,0.00801745, -0.00270891, 0.99996419 ]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrixrows: 3cols: 1dt: ddata: [ -0.0026241,-0.00538847, 0.01327439]

数据和结果都没有问题，但是在ＶＩＮＳ中的代码里有这样的设定

G.z() = fsSettings["g_norm"];

即重力加速度是ｚ轴上的，　其实很多的ＩＭＵ预积分公式中也是这样假设的，　所以我分别在外参和读取参数的结果出做了一个转换，　如下：

extrinsicRotation: !!opencv-matrixrows: 3cols: 3dt: ddata: [0.00801745, -0.00270891, 0.99996419,-0.99996341, 0.00296024 , 0.00802547 ,-0.00298187, -0.999991951, -0.00268508]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrixrows: 3cols: 1dt: ddata: [ 0.01327439,0.0026241, 0.00538847]

新的标定结果，是在第一个标定结果的基础上　左乘　了如下矩阵：

 [0，0，1，0，-1，0，0，0，0，-1，0，0，0，0，0，1］

同时在代码中的　/home/shinan/catkin_ws_VINS/src/VINS-Mono/vins_estimator/src/estimator_node.cpp 中的读取IMU数据的部分做出如下的修改: (代码中有三处类似的地方,记得都要修改)

double dx = imu_msg->linear_acceleration.z;
double dy = -imu_msg->linear_acceleration.x;
double dz = -imu_msg->linear_acceleration.y;
Eigen::Vector3d linear_acceleration{dx, dy, dz};double rx = imu_msg->angular_velocity.z;
double ry = -imu_msg->angular_velocity.x;
double rz = -imu_msg->angular_velocity.y;
Eigen::Vector3d angular_velocity{rx, ry, rz};

本质上也是将IMU数据左乘上面矩阵的3X3部分;

这些都完成之后,重新编译,再次运行;即可;

在运行的时候,有几个点需要注意:

1. 开始时在特征丰富的区域,缓慢地多方向移动;

2. 运行过程中可以缓慢移动, 被遮挡之后路径会飘走;

3, 在未初始化充分时,静置不动也会出现路径飘走的现象;

但是,如果注意上面的三点,最终的建图效果还是很标准的, 如下(我是在室内无阳光照的情况下, 5*10米的区域):

更新：

今天针对上面提到的坐标系问题进行了测试，也就是保持我的D435i的输出数据, 同时在VINS代码中读取的部分也保持原先的代码不变, 使用的标定结果也是原始的未经过旋转的标定结果,即IMU与CAM同向, 测试结果如下:

稳定的时候也可以正常运行,但是方向与上面不同,初始化都是朝向World坐标系的y轴, 路径出错,回环无法纠正,同时极其容易飘走;

忽略飘走的现象,整体路径上确实差一个旋转矩阵.