ORBSLAM2之单目初始化(1)

文章目录

    • ORB_SLAM2单目初始化步骤
    • 第一阶段:通过匹配选取起始两帧
    • 参考

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 mvbPrevMatched;
            //mvbPrevMatched最大的情况就是所有特征点都被跟踪上
            //mvKeysUn:校正mvKeys后的特征点std::vector 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 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 
#include 
#include 
 
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源码中文详解

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

你可能感兴趣的:(c++,计算机视觉,ros,opencv,SLAM)