【ORB-SLAM2】提升算法速度的优化手段列举

chouwa

1. 跟踪初始化求本质矩阵和单应矩阵时，开双线程进行并行加速

1. // 构造线程来计算H矩阵及其得分
2. // thread方式斗劲特殊，在传递引用的时候，外层需要用ref来进行引用传递，否则就是浅拷贝
3. thread threadH(&Initializer::FindHomography,        //该线程的主函数
4.                                    this,                                       
5.                                    ref(vbMatchesInliersH),                        
6.                                    ref(SH),                                
7.                                    ref(H));
8. // 计算fundamental matrix并打分
9. thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

复制代码

2. 计算关键点灰度质心标的目的时，操作圆的对称性每次循环求解对称的两组值。最后使用FastAtan2函数加速计算角度

1. static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
2. {
3. //……部门代码略
4.     for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
5.     {
6.         int v_sum = 0;
7.         int d = u_max[v];
8.         // 在坐标范围内挨个像素遍历，实际是一次遍历2个
9.         // 假设每次措置的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y)
10.         // 对于某次待措置的两个点：m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
11.         // 对于某次待措置的两个点：m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
12.         for (int u = -d; u <= d; ++u)
13.         {
14.             int val_plus = center[u + v*step], val_minus = center[u - v*step];
15.             //在v（y轴）上，2行所有像素灰度值之差
16.             v_sum += (val_plus - val_minus);
17.             //u轴（也就是x轴）标的目的上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
18.             m_10 += u * (val_plus + val_minus);
19.         }
20.         m_01 += v * v_sum;
21.     }
22.     //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
23.     return fastAtan2((float)m_01, (float)m_10);
24. }

复制代码

3. 通过近邻窗口半径（提取特征时预先分配至网格）、投影后坐标窗口附近、Bow词袋等几种方式加速特征匹配

1. void Frame::AssignFeaturesToGrid()
2. int SearchByBoW()
3. //……
4. //等等

复制代码

4. 每次先将某些变量的倒数值计算出来存放至inverse变量，减少计算时除法的计算量

1. //例如：1
2. mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
3. //2
4. Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,……)
5. {
6. ……
7. // 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算成果
8.     invfx = 1.0f/fx;
9.     invfy = 1.0f/fy;
10. ……
11. }
12. //3
13. int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th)
14. {
15. ……
16.      const float invz = 1/p3Dc.at<float>(2);
17.      const float x = p3Dc.at<float>(0)*invz;
18.      const float y = p3Dc.at<float>(1)*invz;
19. ……
20. }
21. //等等

复制代码

5. 循环或函数开始前先把vector等变量reserve扩容至足够大，防止循环时因容量不够发生临时扩容

1. //例如：1
2. int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
3. {
4. ……
5.     // 特征点角度旋转差统计用的直方图
6.     vector<int> rotHist[HISTO_LENGTH];
7.     for(int i=0;i<HISTO_LENGTH;i++)
8.         rotHist[i].reserve(500);
9. ……
10. }
11. //2
12. void Frame::AssignFeaturesToGrid()
13. {
14. ……
15.     // 给存储特征点的网格数组 Frame::mGrid 预分配空间
16.     int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
17.     // 开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间
18.     for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
19.         for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
20.             mGrid[i][j].reserve(nReserve);
21. ……
22. }
23. //等等

复制代码

6. 计算双目特征匹配时，只在某近邻像素行范围内查找，不全局暴力匹配

1. void Frame::ComputeStereoMatches()
2. {
3.     /*两帧图像稀疏立体匹配（即：ORB特征点匹配，非逐像素的密集匹配，但依然满足行对齐）
4.      * 输入：两帧立体矫正后的图像img_left 和 img_right 对应的orb特征点集
5.      * 过程：
6.           1. 行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断.
7.           2. 粗匹配. 按照法式1的成果，对img_left第i行的orb特征点pi，在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi
8.           3. 精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配成果
9.           4. 亚像素精度优化. 法式3得到的视差为uchar/int类型精度，并不必然是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差
10.           5. 最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点。
11.           6. 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就必然是正确匹配，比如光照变化、弱纹理等会造成误匹配
12.      * 输出：稀疏特征点视差图/深度图（亚像素精度）mvDepth 匹配成果 mvuRight
13.      */
14. ……
15.     // 右图特征点数量，N暗示数量 r暗示右图，且不能被改削
16.     const int Nr = mvKeysRight.size();
17.     // Step 1. 行特征点统计. 考虑到尺度金字塔特征，一个特征点可能存在于多行，而非独一的一行
18.     for(int iR = 0; iR < Nr; iR++) {
19.         // 获取特征点ir的y坐标，即行号
20.         const cv::KeyPoint &kp = mvKeysRight[iR];
21.         const float &kpY = kp.pt.y;
22.         // 计算特征点ir在行标的目的上，可能的偏移范围r，即可能的行号为[kpY + r, kpY -r]
23.         // 2 暗示在全尺寸(scale = 1)的情况下，假设有2个像素的偏移，随着尺度变化，r也跟着变化
24.         const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave];
25.         const int maxr = ceil(kpY + r);
26.         const int minr = floor(kpY - r);
27.         // 将特征点ir保证在可能的行号中
28.         for(int yi=minr;yi<=maxr;yi++)
29.             vRowIndices[yi].push_back(iR);
30.     }
31. ……
32. }

复制代码

7. 访谒多线程加锁数据时，快速解锁，避免长时间独有共享数据

1. //多线程编程注意，例如
2. void LocalMapping::ProcessNewKeyFrame()
3. {
4.     {
5.         unique_lock<mutex> lock(mMutexNewKFs);
6.         mpCurrentKeyFrame = mlNewKeyFrames.front();
7.         mlNewKeyFrames.pop_front();
8.     }
9.     //上锁措置数据后，迅速解锁
10. ……//do anotherthing
11. }

复制代码

8. 为分歧感化的数据变量选用合适的容器

例如：
1. 存储特征点的vector容器（频繁随机访谒，变化较小：提取完成后很少再进行插入和删除）
2. 存储共视关键帧mConnectedKeyFrameWeights的map容器。 mConnectedKeyFrameWeights是一个用于保留当前关键帧与其连接的其他关键帧之间权重信息的数据布局，此中键是连接的关键帧 ID，值是权重。此变量在 ORB-SLAM2 中被频繁地使用，因此它需要撑持高效的查找和插入操作。
个人猜测，由于该变量的大小凡是斗劲小，因此使用 std::map 。
std::map 是一种有序关联容器，它使用红黑树实现。由于它是一个有序容器，因此它提供了按键排序的功能，这使得它在查找和插入操作方面具有较高的效率。此外，由于红黑树的高效性能和可预测的内存占用，std::map 凡是比 std::unordered_map 在措置小规模数据时表示更好。
3. 四叉树均匀化分配特征点以及记录连接关键帧、局部地图点（如lLocalMapPoints）时使用list链表容器（不经常随机访谒，而是遍历迭代访谒，经常需要插入，list插入效率高）
4. Map中的地图点和关键帧用set容器，因为经常需要做插入删除动作（印象中orbslam2这里地图点的删除没实现，可能会造成内存泄漏）set容器有序，无反复元素，便利转vector（返回关键帧调集转化的vector给可视化和BA优化法式用）
9. 待补充