ORBSLAM2之单目初始化(1)

ORB_SLAM2单目初始化步骤

ORB_SLAM2单目模式的初始化过程可以分为以下阶段：

1 通过匹配选取两个可以作为起始两帧的初始帧

2 根据匹配计算两帧之间的位姿，获得初始位姿

3 三角化测量得到初始的特征点深度，包装成MapPoint，构建初始地图。

4 BA优化初始化结果

第一阶段：通过匹配选取起始两帧

这一阶段做的工作是:选取是两个特征点数目大于100的两个连续帧，并进行匹配，

只有当前后帧匹配点对>100，才认为这两帧可以进行初始化并记录下来两帧的匹配关系，

接下来开始尝试求取两帧之间的位姿。否则如果当前帧特征点太少，重新构造初始器。

**此阶段输入条件：**单目图像

**此阶段目标：**得到初始两帧的匹配点（大于100对）

1.如果是单目初始器mpInitializer为空，即第一次进行初始化，并且特征点数>100，得到用于初始化的第一帧：

ORB_SLAM2/src/Tracking.cpp--void Tracking::MonocularInitialization()
...
// 单目初始器Initializer* mpInitializer
if(!mpInitializer)
    {
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if(mCurrentFrame.mvKeys.size()>100)
        {
            // 得到用于初始化的第一帧，初始化需要两帧
            mInitialFrame = Frame(mCurrentFrame);
            // 记录最近的一帧
            mLastFrame = Frame(mCurrentFrame);
            //std::vector<cv::Point2f> mvbPrevMatched;
            //mvbPrevMatched最大的情况就是所有特征点都被跟踪上
            //mvKeysUn:校正mvKeys后的特征点std::vector<cv::KeyPoint> mvKeysUn；
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
                //记录匹配的特征点的位置
                mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;


            if(mpInitializer)
                delete mpInitializer;

            // 由当前帧构造初始器 sigma:1.0 iterations:200
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
			//std::vector<int> mvIniMatches;用于跟踪初始化时前两帧之间的匹配
            //std::fill()
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

            return;
        }
    }

...

cv::keyPoint

Opencv中KeyPoint类中的默认构造函数如下：
CV_WRAP KeyPoint() : pt(0,0), size(0), angle(-1), response(0), octave(0), class_id(-1) {}
现分析各项属性
pt(x,y):关键点的点坐标；
size():该关键点邻域直径大小；
angle:角度，表示关键点的方向，值为[零,三百六十)，负值表示不使用。
response:响应强度
octacv:从哪一层金字塔得到的此关键点。
class_id:当要对图片进行分类时，用class_id对每个关键点进行区分，默认为-1

单目初始器mpInitializer

ORB_SLAM2/src/Initializer.cpp
/**
 * @brief 给定参考帧构造Initializer
 * 
 * 用reference frame来初始化，这个reference frame就是SLAM正式开始的第一帧
 * @param ReferenceFrame 参考帧
 * @param sigma          测量误差
 * @param iterations     RANSAC迭代次数
 */
Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
{
    mK = ReferenceFrame.mK.clone();

    mvKeys1 = ReferenceFrame.mvKeysUn;

    mSigma = sigma;
    mSigma2 = sigma*sigma;
    mMaxIterations = iterations;
}

...

std::fill

std::fill函数的作用是：将一个区间的元素都赋予指定的值，即在[first, last)范围内填充指定值

#include <algorithm>
#include <vector>
#include <iostream>
 
int main()
{
    std::vector<int> v{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
 
    std::fill(v.begin(), v.end(), -1);
 
    for (auto elem : v) {
        std::cout << elem << " ";
    }
    std::cout << "\n";
}

2.如果当前帧特征点数大于100，则得到用于单目初始化的第二帧：

ORB_SLAM2/src/Tracking.cpp--void Tracking::MonocularInitialization()
 ...
    
// mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配
 // mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点
ORBmatcher matcher(0.9,true);
//获得匹配点对个数
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

...

3.如果当前帧特征点数不大于100，重新初始化：

ORB_SLAM2/src/Tracking.cpp--void Tracking::MonocularInitialization()
 ...
    
if((int)mCurrentFrame.mvKeys.size()<=100)
{
    delete mpInitializer;
    mpInitializer = static_cast<Initializer*>(NULL);
    fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
    return;
}

...

4.如果初始化的两帧之间的匹配点对太少（小于100），重新初始化：

ORB_SLAM2/src/Tracking.cpp--void Tracking::MonocularInitialization()
 ...
    
if(nmatches<100)
 {
     delete mpInitializer;
     mpInitializer = static_cast<Initializer*>(NULL);
     return;
 }

...

参考

https://www.zhihu.com/question/50385799/answer/120902345

https://blog.csdn.net/zhubaohua_bupt/article/details/78560966

ORB-SLAM: A Versatile and Accurate Monocular SLAM System

http://webdiis.unizar.es/~raulmur/orbslam/

https://github.com/raulmur/ORB_SLAM2

https://en.cppreference.com/w/cpp/algorithm/fill

ORB-SLAM2源码中文注释

ORB-SLAM2源码中文详解

《计算机视觉中的多视图几何》