首先给出泡泡机器人的ORB-SLAM2的源码详解链接:【泡泡机器人公开课】第三十六课:ORB-SLAM2源码详解 by 吴博
这里主要介绍ORB MATCHER模块,ORBSLAM2对ORB MATCHER做了许多有意思的优化,比如加速的描述子距离计算,搜索范围的优化等等,这里简单介绍他的几个函数
DescriptorDistance函数
见Counting bits set, in parallel
这里摘一部分过来说明:
unsigned int v; // count bits set in this (32-bit value)
unsigned int c; // store the total here
static const int S[] = {1, 2, 4, 8, 16}; // Magic Binary Numbers
static const int B[] = {0x55555555, 0x33333333, 0x0F0F0F0F,
0x00FF00FF, 0x0000FFFF};
c = v - ((v >> 1) & B[0]);
c = ((c >> S[1]) & B[1]) + (c & B[1]);
c = ((c >> S[2]) + c) & B[2];
c = ((c >> S[3]) + c) & B[3];
c = ((c >> S[4]) + c) & B[4];
The B array, expressed as binary, is:
B[0] = 0x55555555 = 01010101 01010101 01010101 01010101
B[1] = 0x33333333 = 00110011 00110011 00110011 00110011
B[2] = 0x0F0F0F0F = 00001111 00001111 00001111 00001111
B[3] = 0x00FF00FF = 00000000 11111111 00000000 11111111
B[4] = 0x0000FFFF = 00000000 00000000 11111111 11111111
We can adjust the method for larger integer sizes by continuing with the patterns for the Binary Magic Numbers, B and S. If there are k bits, then we need the arrays S and B to be ceil(lg(k)) elements long, and we must compute the same number of expressions for c as S or B are long. For a 32-bit v, 16 operations are used.
The best method for counting bits in a 32-bit integer v is the following:
v = v - ((v >> 1) & 0x55555555); // reuse input as temporary
v = (v & 0x33333333) + ((v >> 2) & 0x33333333); // temp
c = ((v + (v >> 4) & 0xF0F0F0F) * 0x1010101) >> 24; // count
The best bit counting method takes only 12 operations, which is the same as the lookup-table method, but avoids the memory and potential cache misses of a table. It is a hybrid between the purely parallel method above and the earlier methods using multiplies (in the section on counting bits with 64-bit instructions), though it doesn’t use 64-bit instructions. The counts of bits set in the bytes is done in parallel, and the sum total of the bits set in the bytes is computed by multiplying by 0x1010101 and shifting right 24 bits.
CheckDistEpipolarLine函数
已知基础矩阵F,两幅图像中的点
x′
和
x
,根据关系
从而定义出对应的epipolar line直线: l=x′F ,和另外一幅图像中的点到该直线的距离 d <script type="math/tex" id="MathJax-Element-5">d</script> 可以作为误差,因此有CheckDistEpipolarLine函数,判断两个点的对应关系是否满足机和约束关系
// Epipolar line in second image l = x1'F12 = [a b c]
const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);
const float num = a*kp2.pt.x+b*kp2.pt.y+c;
const float den = a*a+b*b;
if(den==0)return false;
//计算kp2点到直线的l = x1'F12距离
const float dsqr = num*num/den;
//返回距离误差是否在允许范围内
return dsqr<3.84*pKF2->mvLevelSigma2[kp2.octave];
整体采用特征点匹配的方式,先进性搜索范围缩小,在逐个匹配,并进行旋转一致性检测。此外还要求最优匹配要比次优匹配好一定程度才算匹配成功。
在其余的函数介绍如下:
SearchByProjection和SearchBySim3函数
具体流程如下:
SearchByBoW函数
主要流程和SearchByProjection类似,主要是匹配变换成为BOW的特征搜索与匹配
SearchForTriangulation函数
主要流程和SearchByProjection类似,搜索范围为整张图的点,主要是加了CheckDistEpipolarLine函数,用来验证最近的匹配点是否合格
Fuse函数
除了上面函数的作用,主要可以更新匹配的MapPoint,获取更优的匹配
具体流程如下: