艰难地运行了三天,终于算是搞定了,主要的时间和精力都花在标定上了, 总结一些需要注意的点,希望能对同样使用VINS-MONO的人有帮助.
先说一下我的初衷,其实是想要使用ORBSLAM3的Mono_Inertial的,但是总是会初始化失败,图像可以初始化成功,但是在添加IMU数据后整个track就会LOST, 觉得可能是IMU和相机的标定问题,所以想要试一下VINS-MONO的标定效果;
我采用的是彩色相机+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; 本着做最坏的打算的原则,采用了新方法;
个人觉得时间足够的话可以多采集几组数据对比一下, 毕竟新旧两个使用的数据包是不一样的,新方法在数据包的采集上也有改进,所以只进行一次这样的对比可能不够严谨;
运行之前,要注意自己电脑上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的时候我和前面标定所采用的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: 1411
stamp:
secs: 1602851363
nsecs: 898209810
frame_id: "camera_imu_optical_frame"
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 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.00178953423165
y: 0.00692256260663
z: -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.539947092533
y: -9.76134586334
z: 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-matrix
rows: 3
cols: 3
dt: d
data: [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-matrix
rows: 3
cols: 1
dt: d
data: [ -0.0026241,-0.00538847, 0.01327439]数据和结果都没有问题，但是在ＶＩＮＳ中的代码里有这样的设定
G.z() = fsSettings["g_norm"];即重力加速度是ｚ轴上的， 其实很多的ＩＭＵ预积分公式中也是这样假设的， 所以我分别在外参和读取参数的结果出做了一个转换， 如下：
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [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-matrix
rows: 3
cols: 1
dt: d
data: [ 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轴, 路径出错,回环无法纠正,同时极其容易飘走;
忽略飘走的现象,整体路径上确实差一个旋转矩阵.