计算机视觉大型攻略 —— SLAM(4) ORB SLAM2（二）跟踪线程

最新推荐文章于 2024-06-10 19:47:19 发布

linusyue

最新推荐文章于 2024-06-10 19:47:19 发布

阅读量1.1k

点赞数

本文链接：https://blog.csdn.net/plateros/article/details/103603796

版权

视觉定位专栏收录该内容

9 篇文章 11 订阅

订阅专栏

ORB SLAM是一套完整的基于特征的图优化单目SLAM系统，实现了地图创建，重用，定位，回环检测和重定位功能。上一篇文章写了架构，这一篇主要写系统的跟踪线程(Tracking thread)。

开源代码：https://github.com/raulmur/ORB_SLAM2.git

参考文献:
[1] ORB-SLAM: a Versatile and Accurate Monocular SLAM System
[2] ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras
[3] Double Window Optimization for Constant Time Visual SLAM
[4] Bags of Binary Words for Fast Place Recognition in Image Sequences
[5]ORB: an efficient alternative to SIFT or SURF

系统架构

论文[2]中给出的算法框架图。

跟踪线程(Tracking)

输入图像到跟踪线程。首先对图像做预处理(Preprocess-Input)

图像预处理

系统将图像预处理独立出来，将来自双目或RGB-D的数据单独处理，后续模块以其输出的特征为输入，不依赖于某种传感器。

输入图像后，首先提取ORB特征。

ORB(Oriented FAST and Rotated BRIEF)

ORB = Fast keypoint detector + BRIEF descriptor

FAST (Features from Accelerated Segment Test)

如上图，FAST探测器将点P周围的16个相邻像素分三类，亮度大于p, 小于p，接近p。如果第一类或第二类的像素个数大于8，那么可以判定为keypoints。然而，这样角点没有旋转和尺度不变性。

ORB采用多尺度金字塔算法，在每个尺度上寻找极值，一定程度上实现了尺度不变性。更多关于尺度不变形，图像金字塔的信息可参见这篇文章：特征的变与不变。

获得特征点后，计算方向特征方向。ORB采用灰度质心法(intensity centroid)，假设特征点的灰度与质心之间存在一个偏移，这个偏移可以用于表示方向。点p的邻域像素的矩(moment)为，

$m_{pq} = \sum _{x,y} x^{p}y^{q}I(x,y)$

q为质心，质心可求，

$C=(\frac{m_{10}}{m_{00}}, \frac{m_{01}}{m_{00}})$

最后求得特征点方向 $\theta$ 为

$\theta =atan2(m_{01}, m_{10})$

BRIEF (Binary Robust Independent Elementary Features)

BRIEF将FAST算法输出的特征点转换成长度为128-512bit的二进制特征向量(binary feature vector)。

ORB算法使用了256bit, 假设平滑image patch, 特征点p，二值化函数 $\tau$ 定义为，

p(x)是点x的光强。可定义 $f_{n}(p)$ 为描述符，n=256。

Rotation-aware BRIEF

然而此时FAST+BRIEF仍然不具备旋转不变性。利用之前获得的特征方向，把特征旋转 $\theta$ ，再计算特征描述符。此时就具备了旋转不变性。

令特征点P的邻域Patch内的坐标集为S，

旋转点P的特征方向 $\theta$

描述符函数可重新定义为，

Stereo keypoints

Stereo keypoints定义为 $X_{s}=(u_{L}, v_{L},u_{R})$ ， $u_{L}$ , $u_{R}$ 分别为左图和右图坐标系的列坐标。对于双目相机，在左右幅图像提取ORB特征，根据左图的特征点到右图上找匹配。由于输入的图像已经经过了双目校准，只需在同一行寻找匹配即可。

传送门：双目校准

远, 近特征点。双目特征点会根据距离分为远，近两类。小于40倍基线的距离为近特征点，否则为远特征点。近特征点深度精度高，提供准确的尺度，旋转和平移信息。远特征点由于在尺度和平移上的信息误差较大，只提供准确的旋转信息。

Monocular keypoints

定义为 $X_{m}=(u_{L}, v_{L})$ 。左相机图像坐标系。这些点无法通过ORB找到右图对应的匹配点。

Stereo keypoints与Monocular keypoints是图像预处理模块的输出结果。

地图初始化

对比ORB SLAM[1]和ORB SLAM[2]的论文我们可以发现，系统框架图稍有不同。使用双目或者RGB-D相机的ORB SLAM2系统无需额外的初始化。只需要在启动时以第一帧作为Keyframe，并设置其为位姿原点，创建初始化地图即可。

Pose Prediction or Relocalization

Pose Prediction

跟踪线程通过Motion-only BA优化相机Pose。Motioin-only BA仅优化相机Pose。

与之前基础算法的文章描述的完全相同，定义, 为相机Pose，计算投影误差,

$e=x_{\left ( \cdot \right )}^{i}-\pi_{\left ( \cdot \right )} (RX^{i}+t)$

$X^{i}$ 为点上一帧的3D坐标， $x^{i}$ 为本帧观测到的相应匹配点的图像坐标。 $X^{i}$ 通过旋转平移矩阵 R,t ，再经过投影函数 $\pi_{\left ( \cdot \right )}$ , 可以将上一帧的3D坐标 $X^{i}$ 投影到本帧的图像坐标系上，计算其与实际观测值 $x^{i}$ 的差，可以获得投影误差。BA算法优化这个投影误差。由于这里只是Motion-only，因此，只优化了R, t。