SLAM机器人开发（四）Improved_ORBSLAM2算法
ORBSLAM算法简介
本专栏介绍的SLAM机器人使用的Improved_SLAM2算法基于ORBSLAM算法，建立稠密点云地图，是一种视觉SLAM算法。ORBSLAM算法使用ORB特征点，自2015年提出以来被开发者广泛使用。
2015年提出ORB-SLAM。
转载：R. Mur-Artal, J. M. M. Montiel ,J. D. Tardós. ORB-SLAM: A Versatile and Accurate Monocular
SLAM System[J]. IEEE/Transactions on Robotics, 2015, 31(5):1147-1163.
2017年提出ORB-SLAM2。
转载：R. Mur-Artal,J. D. Tardós. ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo,and RGB-D Cameras[J]. IEEE/Transactions on Robotics, 2017, 33(5):1255-1262.
2021年提出ORB-SLAM3。
转载：C. Campos, R. Elvira, et al.ORB-SLAM3: An Accurate Open-Source Library for Visual,
Visual-Inertial, and Multimap SLAM[J]. IEEE/Transactions on Robotics(Early Access), 2021:1-17.
Improved_SLAM2算法
与基本的视觉 SLAM 框架类似,本专栏中的 Improve-ORBSLAM2 算法分为 RGB-D 相机数据读取、前端 VO/VIO、回环检测、后端非线性优化、视觉建图、稠密重建、定位等部分。
本专栏使用的Improved_SLAM2算法代码托管在Gitee上：
funandi_Improved_ORBSLAM2
基于:
高翔的ORBSLAM2_with_pointcloud_map代码;
R. Mur-Artal,J. D. Tardós的ORB_SLAM2代码;
R. Mur-Artal, J. M. M. Montiel ,J. D. Tardós的ORB_SLAM代码;
RGB-D 相机数据读取
（1）本专栏中选用的 RGB-D 相机是 RealSense D415和Kinect V2,从 RealSense D415 读取的深度图分辨率最大为 1280×720,帧率60fps;彩色图分辨率最大为 1920×1080,帧率60fps;从Kinect V2读取的深度图分辨率最大为 512×424,帧率30fps;彩色图分辨率最大为 1920×1080，帧率30fps。以上图像中，每个像素占用的存储均为 2Byte,即 16 位数据。
（2）如果使用RealSense D415，可借助 RealSense 驱动,在消息位置“/camera/color/image_raw”读取彩色图,消息位置“/camera/depth/image_rect_raw”读取深度图。RealSense 测量深度图的原理融合了双目视觉和结构光原理。采用结构光可以解决双目视觉在环境低纹理或光照太强或太弱时无法准确获得深度数据的问题。由于使用双目视觉的原理,所以 RealSense 在使用时需要标定参数。其中,RealSense D415 的基线为 55mm,内参矩阵可以通过查阅 ROS 系统中 RealSense 节点发布的的话题“/camera/color/camera_info”获得。
（3）如果使用Kinect V2，可借助kinect驱动，在消息位置"/kinect2/qhd/image_color"读取彩色图，消息位置"/kinect2/qhd/image_depth_rect"读取深度图。
前端 VCO
Improve-ORBSLAM2 算法中使用了视觉里程计（含结构光）、惯性里程计和轮速里程计一同来估计物流机器人位姿和局部地图。其中,视觉里程计中结构光会主动发射激光图,然后测量激光图的视差,在无纹理的情况下也可以估计位姿;惯性里程计通过 IMU 测量三轴角速度和三轴角加速度等信息,可以在相机快速运动的情况下也准确估计位姿;轮速里程计则测量麦轮转速,但是由于物流机器人系统是非完整约束系统,对估计位姿的作用不是很大。由于主要里程计为视觉里程计和惯性里程计,本文称其为视觉综合例程计VCO(Visual-Comprehensive Odometry)。
ORB 特征
ORB 特征由改进后的 FAST 角点和 BRIEF 描述子构成。特征点是图像信息的又一种表达形式,对每张图像选择一组好的特征是至关重要的。计算机视觉领域的研究者们设计出了许多相对稳定的局部图像特征,比如 SIFT、ORB、SURF 等等。
FAST 角点是一种关键点,主要检测局部像素灰度变化明显的地方,以速度快著称,FAST 角点提供了特征点的位置。原始的 FAST 角点检测会对灰度图中的每一个像素点 p 进行检测,如果其周围的 16 个像素点有连续的 N 个点亮度明显不同于 p,即认为 p 为角点。其中, N 可取 12, 9,11。相较于原版的 FAST, ORB 中选取了最符合的 M 个 FAST 角点,同时计算了特征点的主方向,为后续的 BRIEF 描述子增加了旋转不变特性。它的检测过程如下:
BRIEF 是一种二进制描述子,它的描述向量由许多个 0 和 1 组成,这里的 0 和 1 编码了关键点附近两个像素(比如说 m 和 n)的大小关系:如果 m 比 n 大,则取 1,反之就取 0。 BRIEF使用了随机选点的比较和二进制表达,所以匹配起来非常快,存储起来也十分方便,适用于实时的图像匹配。由于考虑到了旋转和缩放,使得 ORB 在平移、旋转、缩放的变换下仍有良好的表现。
有测试表明,在同一幅图像中同时提取约 1000 个特征点的情况下, ORB 约要花费 15.3ms,SURF 约花费217.3 ms , SIFT 约花费 5228.7ms。
特征匹配
ORB-SLAM2 采用快速近似最近邻(FLANN)算法来适合于匹配点数量极多的情况。 FLANN算法使用了随机 K-D 树算法,先找出数据集中方差最高的维度,利用这个维度的数值将数据划分为两个部分,对每个子集重复相同的过程;然后,建立多棵随机 K-D 树,从具有最高方差的 D n维中随机选取若干维度,用来做划分。在对随机 K-D 森林进行搜索时,所有的随机 K-D 树将共享一个优先队列, K-D 森林中的树一般不超过 20。但是,对于需要高精度的情形,优先搜索 K-means树更加有效。
估计相机位姿
用四元数表示机器人姿态
后端非线性优化
回环检测
回环检测目的
词袋模型
相似度计算
视觉建图
稠密重建
定位模式
ORBSLAM算法展望
最新的ORB-SLAM3于今年5月正式提出， 是第一个基于特征点的紧耦合的 VIO 系统,仅依赖于最大后验估计,包括在 IMU 在初始化时。与旧版本相比,ORB-SLAM3 在大场景或小场景,室内或室外中都能鲁棒的实时运行。而且在精度上,相比于上一版提升了 2 到 5 倍。
C. Campos, R. Elvira等人提出的ORB-SLAM3代码。