SLAM定位建图和轨迹评估

Posted on | In
Slam定位、建图、轨迹评估。RealSense D435i、D455，Rolling-Shutter、Global-Shutter等不同相机对于slam计算影响对比。
一、概述

SLAM：即时定位与地图构建，Simultaneous Location And Mapping，也称为CML (Current Mapping and Localization)。分开来看就是两个部分：

  • 定位部分，主要是依据周围图像，提取图像特征点(或光流法)，依据图像差异计算轨迹，以确定自身位置。
  • 建图部分，通过自身回环轨迹，不断更新当前地图和全局地图，最终以点云形式，计算出周边环境轮廓地图。

          开源框架Orb-Slam3是典型的以图像特征点法为主的slam计算框架，基于此可以评估不同光照条件、不同相机、不同外部环境、不同曝光、不同图像等各种因素对于传感器运动轨迹的影响。
          对于仅基于图像的slam定位建图算法来说，获取到的图像质量对于计算精度和准确率的影响是极为重要的（尤其在没有附加惯性测量单元(imu)或外部GPS定位的条件下）。同时也可以看到，不同相机的帧率/曝光方式/相机性质/相机标定的内外参…，所带来的差异影响也是非常大的。

          本文分为三个部分：
1.卷帘曝光相机(Rolling-Shutter)和全局曝光相机(Global-Shutter)对比
          global shutter：全局曝光，感光元件的所有像素点同时曝光一定时间，进而成像。没有Rolling Shutter的“果冻现象”。曝光时间比Rolling Shutter短，曝光整帧后输出像素数据。
          rolling shutter：卷帘曝光，感光元件的所有像素点逐行轮流曝光一定时间，进而成像。会出现“果冻现象”。曝光时间比Global Shutter长，但是曝光一行输出一行。
2.RealSense D435i的彩色相机(rgb-camera)和红外相机(infrared-camera)对比
          RealSense 是立体视觉深度相机，集成了2个红外传感器(IR Stereo Camera)、1个红外激光发射器(IR Projector)、1个彩色相机(Color Camera)。IntelRealSense D435i提供完整深度相机模块，集成视觉处理器、立体深度模块、RGB传感器、彩色图像信号处理模块。深度模块采用立体视觉的左右成像器、可选红外激光发射器、 RGB色彩传感器。
3.RealSense D455的彩色相机(rgb-camera)和红外相机(infrared-camera)对比
          RealSense D455和RealSense D435i其实本质差异不太大，除了相机摄像头分布位置、摄像头内外参等。

二、Rolling-Shutter & Global-Shutter相机轨迹评估
1.实验设备

Global Shutter相机 (2M)
Rolling Shutter相机 (630)

2.实验方法

          两个摄像头同时固定设备，沿相同轨迹移动一段时间后，得到视频。两个相机运动轨迹近乎重合，有细微差别。预处理视频后，得到帧图像数据集（室内indoor/室外outdoor/走廊slides），标注为TUM数据集格式，定位建图框架运行环境为Orb-Slam3，按关键帧提取图像特征点，得到的轨迹文件KeyFrameTrajectory.txt，为运动位姿数据，包含：时间戳timestamp、6dof位姿数据(xyz和rpy)。
          参照轨迹Ground Truth为global shutter轨迹，rolling shutter相对于global shutter的。

3.轨迹实验

3.1.openCV录制视频
          openCV直接调起“pc摄像头”或“usb外接摄像头”录制数据。openCV读视频原理：其实就是不停地拍照，不停地写入到一个文件。视频本身也是由一张一张照片组成的。部分代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
string video_path = "/home/admin/video.mp4";
VideoCapture capture(2);
capture.set(CAP_PROP_FOURCC, CV_FOURCC('M','J','P','G'));
capture.set(CV_CAP_PROP_FRAME_WIDTH, 640);
capture.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
capture.set(CV_CAP_PROP_FPS, 30);
VideoWriter videoWriter;
videoWriter.open(video_path, CV_FOURCC('M','J','P','G'), 30.0, Size(frameWidth,frameHeight), true);
Mat frameImg;
while(1) {
    videoWriter << frameImg;
    if(char(waitKey(40))=='q' or char(waitKey(40))==27) {
            break;
    }
}
capture.release();
frameImg.release();
videoWriter.release();

3.2.实验视频 (点击自动播放)
(1) 室内(indoor)

视频1 室内(indoor)轨迹实验

(2) 室外(outdoor)

视频2 室外(outdoor)轨迹实验

(3) 走廊(slides)

视频3 走廊(slides)轨迹实验

3.3.视频制作数据集
          将视频以一定帧率采样，得到每帧图像，加上时间戳(timestamp)，用作Orb-Slam3系统的输入。常见的输入格式有TUM、EuRoC，此处以TUM为例，用openCV将视频mp4文件转换为TUM格式数据集。
          mp4提取图片帧，保存为TUM的“rgb文件夹”和“rgb.txt”。部分代码片如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
string video_path = "/home/admin/video.mp4";
Mat frame;
double time;
VideoCapture capture(video_path);
ofstream of;
of.open(path+"video/rgb.txt", std::ios_base::app);
...
while(true) {
    capture >> frame;
    time = capture.get(CV_CAP_PROP_POS_MSEC);  //时间戳timestamp
    ...
    std::ostringstream osf;
    osf << path+"video/rgb/" << std::fixed << time << ".png";
    cv::imwrite(osf.str(), frame);
    of << std::fixed << time << " " << "rgb/" << time << ".png" << endl;
}
of.close();
capture.release();

3.4.相机标定
          标定得到实际相机内外参(fx,fy,cx,cy)、相机转移参数(T_c1_c0)、畸变参数(distortion_coeffs)，写入Orb-Slam3参数配置文件cam0/1-camchain.yaml。
          相机标定部分内容比较多，从搭建ros标定环境、到标定工具Kalibr使用、相机内外参+imu标定，详见另外本系列另外博客《相机标定》。用到的工具：Kalibr工具imu+camera联合标定。

3.5.Slam计算
          修改配置文件中的相机内外参(ps:相机参数需要自行先标定好)，Orb-Slam3计算轨迹，得到轨迹文件FrameTrajectory.txt。

4.评估指标

          轨迹、绝对位姿误差、相对位姿误差。

Ape: 绝对位姿误差，APE w.r.t. translation part (m), (with Sim(3) Umeyama alignment)。估计轨迹全局一致性，比较被估计值和真值轨迹之间的绝对距离。针对所有时刻定义平移分量的 均方根误差(RMSE)和方差(SSE)。
Rpe: 相对位姿误差，RPE w.r.t. translation part (m), (with Sim(3) Umeyama alignment), for delta = 1 (frames) using consecutive pairs。测量轨迹在固定时间区间Δt内的局部准确度。对应轨迹的漂移，计算相隔固定时间差Δ两帧位姿差。

各指标信息汇总对比如下：

SceneTraj-Info
室内(Indoor)name: f_dataset-indoor_rolling
infos: 968 poses, 6.247m path length, 322333333330000.000s duration
name: f_dataset-indoor_global
infos: 986 poses, 5.679m path length, 328333333330000.000s duration
室外(Outdoor)name: f_dataset-outdoor_rolling
infos: 391 poses, 0.695m path length, 130000000000000.000s duration
name: f_dataset-outdoor_global
infos: 696 poses, 1.511m path length, 231666666670000.000s duration
走廊(Slides)name: f_dataset-slides_rolling
infos: 466 poses, 11.648m path length, 155000000000000.062s duration
name: f_dataset-slides_global
infos: 1455 poses, 40.405m path length, 494333333330000.062s duration

各误差汇总对比如下：

指标室内(indoor)室外(outdoor)走廊(slides)
APE max: 0.374459
mean: 0.062394
median: 0.029402
min: 0.003393
rmse: 0.099173
sse: 9.520567
std: 0.077086		 max: 0.132285
mean: 0.008305
median: 0.006561
min: 0.000379
rmse: 0.012343
sse: 0.059569
std: 0.009131		 max: 2.565694
mean: 0.654031
median: 0.608664
min: 0.038379
rmse: 0.792056
sse: 292.346650
std: 0.446762
RPE max: 0.104738
mean: 0.005775
median: 0.003462
min: 0.000161
rmse: 0.010518
sse: 0.106973
std: 0.008790		 max: 0.135044
mean: 0.006286
median: 0.004057
min: 0.000303
rmse: 0.013238
sse: 0.068347
std: 0.011650		 max: 0.679385
mean: 0.040339
median: 0.020645
min: 0.002755
rmse: 0.077982
sse: 2.827775
std: 0.066738

各轨迹图统计汇总对比如下：(点击查看大图)

指标室内(indoor)室外(outdoor)走廊(slides)
Trajectory
xyz_view
rpy_view
APE
APE-Map
RPE
RPE-Map
5.结论

1.Orb-Slam3完全是基于图像特征点(ORB)的方法。

  • 在特定光照、参照物明确的情景下，轨迹完整，误差也较小。
  • 在图像过于相似(走廊往返)、光照过于强烈(室外)的情况，会存在轨迹跳跃不连续现象。

2.图像变化过快的情形，导致特征点提取过慢或丢失部分特征点，会出现跑数据时的帧卡顿现象，对Altas地图集localMap构建和地图合并有影响。

          如上图所示，走廊实验中的现象，当沿着走廊尽头翻转180°走回来时，Orb-Slam3系统会因为图像过于相似(走廊顺向/逆向)而分辨不清，于是定位建图时直接跳回原点开始重新建图，随机选定一个方向角度开始重新绘制轨迹，即出现红色箭头所示方向。
          其本质原因是基于特征点法的slam算法本身存在的缺陷，当图像质量不清晰、或者图像中能检出的特征点十分有限，导致不能slam系统很好区分图像时，就没法定位了。
3.rolling shutter会存在轨迹不完全的情况(丢失轨迹)，global shutter则不会存在。

  • 轨迹重叠度：室内>室外>走廊
  • 在轨迹存在的部分，误差：走廊>室外>室内

此处详见上述“指标信息汇总对比”、“误差汇总对比”部分。

三、RealSense D435i: rgb & infrared-camera轨迹评估
1.实验设备

RealSense D435i: RGB-camera
RealSense D435i: Infrared-left-camera

2.实验方法

          RealSense D435i的2个摄像头：rgb-camera, infrared_left-camera，分别单独沿重复轨迹，实时运行Orb-Slam3，场景为 (室内indoor/室外outdoor/走廊slides)，得到轨迹文件KeyFrameTrajectory.txt和CameraTrajectory.txt，为各自运动位姿数据，包含：时间戳timestamp、6dof位姿数据(xyz和rpy)。
          由于2个摄像头是单独实时按相同既定路线运行，不存在同一时空统一性（时间戳timestamp、运行时长during length不完全统一），故轨迹为单独各自绘制。

3.轨迹实验

3.1.相机驱动
          RealSense深度相机系列，自带驱动调起摄像头，可实时录视频+slam定位建图，不是openCV录制预存离线数据集。
          关于RealSense驱动安装+ros环境安装+配置&校准相机内外参+imu参数，详见RealSense官网。

  • (1)realsense-sdk-2.0驱动安装：(下载链接见“参考文献”部分)
  • (2)获取相机内外参数：读取配置写到自定义文件(config_D435i.yaml、config_D455.yaml)，详见sdk源码：librealsense/tools/enumerate-devices/rs-enumerate-devices.cpp，可直接编译运行。

          把Orb-Slam3配置文件中的相机参数, , , ，替换为相机配置的, , , 参数。
3.2.实验视频
          如前所述，仍旧是：室内(indoor)、室外(outdoor)、走廊(slides)。此处不再给出实验视频。
3.3.相机标定
          驱动RealSense SDK可以完成，一定注意保证相机参数的准确性。
3.4.实时slam计算
          运行Orb-Slam3，计算轨迹+定位建图。

4.评估指标

          轨迹、绝对位姿误差、相对位姿误差。

各指标信息汇总对比如下：

SceneTraj-Info
Mono_IMURgbd_IMU
室内(Indoor) name: kf_dataset-monoi_realsense_d435i_indoor
infos: 276 poses, 36.352m path length, 101989215488.000s duration
name: f_dataset-monoi_realsense_d435i_indoor
infos: 3152 poses, 42.172m path length, 105055297792.000s duration		 name: kf_dataset-rgbd_imu_realsense_d435i_indoor
infos: 280 poses, 29.703m path length, 49206745088.000s duration
name: f_dataset-rgbd_imu_realsense_d435i_indoor
infos: 1587 poses, 37.269m path length, 52942709760.000s duration
室外(Outdoor) name: kf_dataset-monoi_realsense_d435i_outdoor
infos: 1179 poses, 257.977m path length, 236662186496.000s duration
name: f_dataset-monoi_realsense_d435i_outdoor
infos: 7252 poses, 275.178m path length, 241763062528.000s duration		 name: kf_dataset-rgbd_imu_realsense_d435i_outdoor
infos: 1230 poses, 267.810m path length, 267213810176.000s duration
name: f_dataset-rgbd_imu_realsense_d435i_outdoor
infos: 8166 poses, 381.485m path length, 272350868224.000s duration
走廊(Slides) name: kf_dataset-monoi_realsense_d435i_slides
infos: 283 poses, 45.609m path length, 173703452160.000s duration
name: f_dataset-monoi_realsense_d435i_slides
infos: 2581 poses, 71.888m path length, 175361714176.000s duration		 name: kf_dataset-rgbd_imu_realsense_d435i_slides
infos: 233 poses, 35.413m path length, 52588330496.000s duration
name: f_dataset-rgbd_imu_realsense_d435i_slides
infos: 1729 poses, 48.788m path length, 57625243904.000s duration

各误差汇总对比如下：

指标室内(indoor)室外(outdoor)走廊(slides)
Mono_IMURgbd_IMUMono_IMURgbd_IMUMono_IMURgbd_IMU
APE max:0.000004
mean:0.000001
median:0.000001
min:0.000000
rmse:0.000001
sse:0.000000
std:0.000001		max:0.000006
mean:0.000002
median:0.000001
min:0.000000
rmse:0.000002
sse:0.000000
std:0.000001		 max:0.000067
mean:0.000013
median:0.000011
min:0.000000
rmse:0.000017
sse:0.000000
std:0.000010		max:0.000094
mean:0.000015
median:0.000010
min:0.000001
rmse:0.000020
sse:0.000001
std:0.000013		 max:0.000007
mean:0.000002
median:0.000001
min:0.000000
rmse:0.000002
sse:0.000000
std:0.000001		max:0.000013
mean:0.000002
median:0.000001
min:0.000000
rmse:0.000003
sse:0.000000
std:0.000002
RPE max:0.000006
mean:0.000001
median:0.000001
min:0.000000
rmse:0.000002
sse:0.000000
std:0.000001		max:0.000010
mean:0.000002
median:0.000002
min:0.000000
rmse:0.000003
sse:0.000000
std:0.000002		 max:0.000108
mean:0.000017
median:0.000014
min:0.000000
rmse:0.000023
sse:0.000001
std:0.000015		max:0.000110
mean:0.000019
median:0.000013
min:0.000000
rmse:0.000028
sse:0.000001
std:0.000020		 max:0.000011
mean:0.000002
median:0.000002
min:0.000000
rmse:0.000003
sse:0.000000
std:0.000002		max:0.000015
mean:0.000003
median:0.000002
min:0.000000
rmse:0.000004
sse:0.000000
std:0.000003

各轨迹图统计汇总对比如下：(点击查看大图)

指标室内(indoor)室外(outdoor)走廊(slides)
Mono_IMURgbd_IMUMono_IMURgbd_IMUMono_IMURgbd_IMU
Trajectory
xyz_view
rpy_view
APE
APE-Map
RPE
RPE-Map
5.结论

1.infrared-camera整体效果好于rgb-camera，其中rgb-camera运行过程中，会出现的问题：
(1) 跳帧(找不到当前活跃地图current active-map就跳回原点、随机初始化角度方向，重画跟踪轨迹)：存在多次。bad case触发条件: 场景移动变化过快(>1m/s)、场景过于相似(走廊翻转180°)；
(2) 卡顿(图像帧卡住, 无法活动轨迹)：存在少量，停下重新移动camera会继续运行；而infrared-camera较流畅，不会出现上述问题。

2.场景对比：
(1) 轨迹完整度：室内>室外>走廊；

  • 室内、室外：整体轨迹保留比较完整，虽然在运行过程中会出现卡顿、跳帧，但最终轨迹结果是完整的；
  • 走廊：infrared-camera没有丢失轨迹，rgb-camera丢失部分轨迹；

(2) 轨迹总体误差：走廊>室外>室内；

四、RealSense D455: rgb & infrared-camera轨迹评估

          和RealSense D435i差异不大，此处不再赘述，注意获取准确对应的相机内参。

五、参考文献

[1] ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM, Dec, 2021.
[2] Orb-Slam3源码：https://github.com/UZ-SLAMLab/ORB_SLAM3
[3] RealSense-sdk-2.0：https://github.com/IntelRealSense/librealsense.git