ORB-SLAM2源码笔记(4)——帧Frame和关键帧KeyFrame

Frame类中的相机参数为static类型,表示所有Frame对象共享一份相机参数

特征点提取ExtractORB

Frame类构造函数中调用成员变量mpORBextractorLeftmpORBextractorRight()运算符进行特征点提取.

畸变矫正前的左目特征点是mvKeys[i].

畸变矫正后的左目特征点是mvKeysUn[i].

在右目图片中对应特征点的横坐标为mvuRight[i],纵坐标与mvKeys[i]的纵坐标相同.

特征点的深度是mvDepth[i].

void Frame::ExtractORB(int flag, const cv::Mat &im) {
    if (flag == 0)		// flag==0, 表示对左图提取ORB特征点
        (*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);
    else				// flag==1, 表示对右图提取ORB特征点
        (*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, mDescriptorsRight);
}

定义函数ExtractORB,调用通过ORBextractor创建出来的对象mpORBextractorLeft和mpORBextractorRight进行特征提取。

  • 对于双目相机,通过双目特征点匹配关系计算特征点的深度值.
  • 对于RGBD相机,根据特征点深度构造虚拟的右目图像特征点.

通过这种预处理,在后面SLAM系统中不再区分双目特征点和RGBD特征点,它们以双目特征点的形式被处理.(仅通过判断mvuRight[idx]判断某特征点是否有深度) 

int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoints, const float th) {
	// ...
    for (size_t idx : vIndices) {
        if (F.mvuRight[idx] > 0) {		// 通过判断 mvuRight[idx] 判断该特征点是否有深度
            // 针对有深度的特征点特殊处理
        } else {
            // 针对单目特征点的特殊处理
        }
    }
    // ...
}

双目特征点匹配

粗匹配: 根据特征点描述子距离和金字塔层级判断匹配.粗匹配关系是按行寻找的,对于左目图像中每个特征点,在右目图像对应行上寻找匹配特征点.
精匹配: 根据特征点周围窗口内容相似度判断匹配.
亚像素插值: 将特征点相似度与匹配坐标之间拟合成二次曲线,寻找最佳匹配位置(得到的是一个小数).
记录右目匹配mvuRight和深度mvDepth信息.
离群点筛选: 以平均相似度的2.1倍为标准,筛选离群点.

 

 

void Frame::ComputeStereoMatches() {

    mvuRight = vector(N, -1.0f);
    mvDepth = vector(N, -1.0f);
    
    // step0. 右目图像特征点逐行统计: 将右目图像中每个特征点注册到附近几行上
    vector > vRowIndices(nRows, vector());	// 图像每行的1右目特征点索引
    for (int iR = 0; iR < mvKeysRight.size(); iR++) {
        const cv::KeyPoint &kp = mvKeysRight[iR];
        const float &kpY = kp.pt.y;
        const int maxr = ceil(kpY + 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
        const int minr = floor(kpY - 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
        for (int yi = minr; yi <= maxr; yi++)
            vRowIndices[yi].push_back(iR);
    }

    // step1. + 2. 粗匹配+精匹配
    const float minZ = mb, minD = 0, maxD = mbf / minZ;		// 根据视差公式计算两个特征点匹配搜索的范围
	const int thOrbDist = (ORBmatcher::TH_HIGH + ORBmatcher::TH_LOW) / 2;

    vector > vDistIdx;		// 保存特征点匹配

    for (int iL = 0; iL < N; iL++) {

        const cv::KeyPoint &kpL = mvKeys[iL];
        const int &levelL = kpL.octave;
        const float &vL = kpL.pt.y, &uL = kpL.pt.x;

        const vector &vCandidates = vRowIndices[vL];
        if (vCandidates.empty()) continue;
		
        // step1. 粗匹配,根据特征点描述子和金字塔层级进行粗匹配
        int bestDist = ORBmatcher::TH_HIGH;
        size_t bestIdxR = 0;
        const cv::Mat &dL = mDescriptors.row(iL);
        for (size_t iC = 0; iC < vCandidates.size(); iC++) {

            const size_t iR = vCandidates[iC];
            const cv::KeyPoint &kpR = mvKeysRight[iR];
            if (kpR.octave < levelL - 1 || kpR.octave > levelL + 1)
                continue;
            const float &uR = kpR.pt.x;

            if (uR >= minU && uR <= maxU) {
                const cv::Mat &dR = mDescriptorsRight.row(iR);
                const int dist = ORBmatcher::DescriptorDistance(dL, dR);
                if (dist < bestDist) {
                    bestDist = dist;
                    bestIdxR = iR;
                }
            }
        }

        // step2. 精匹配: 滑动窗口匹配,根据匹配点周围5✖5窗口寻找精确匹配
        if (bestDist < thOrbDist) {
            const float uR0 = mvKeysRight[bestIdxR].pt.x;
            const float scaleFactor = mvInvScaleFactors[kpL.octave];
            const float scaleduL = round(kpL.pt.x * scaleFactor);
            const float scaledvL = round(kpL.pt.y * scaleFactor);
            const float scaleduR0 = round(uR0 * scaleFactor);
            const int w = 5;
            cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduL - w, scaleduL + w + 1);
            IL.convertTo(IL, CV_32F);
            IL = IL - IL.at(w, w) * cv::Mat::ones(IL.rows, IL.cols, CV_32F);
            int bestDist = INT_MAX;
            int bestincR = 0;
            const int L = 5;
            vector vDists;
            vDists.resize(2 * L + 1);
            const float iniu = scaleduR0 + L - w;
            const float endu = scaleduR0 + L + w + 1;
            for (int incR = -L; incR <= +L; incR++) {
                cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduR0 + incR - w, scaleduR0 + incR + w + 1);
                IR.convertTo(IR, CV_32F);
                IR = IR - IR.at(w, w) * cv::Mat::ones(IR.rows, IR.cols, CV_32F);
                float dist = cv::norm(IL, IR, cv::NORM_L1);
                if (dist < bestDist) {
                    bestDist = dist;
                    bestincR = incR;
                }
                vDists[L + incR] = dist;
            }
            
            // step3. 亚像素插值: 将特征点匹配距离拟合成二次曲线,寻找二次曲线最低点(是一个小数)作为最优匹配点坐标
            const float dist1 = vDists[L + bestincR - 1];
            const float dist2 = vDists[L + bestincR];
            const float dist3 = vDists[L + bestincR + 1];
            const float deltaR = (dist1 - dist3) / (2.0f * (dist1 + dist3 - 2.0f * dist2));

            // step4. 记录特征点的右目和深度信息
            float bestuR = mvScaleFactors[kpL.octave] * ((float) scaleduR0 + (float) bestincR + deltaR);
            float disparity = (uL - bestuR);
            if (disparity >= minD && disparity < maxD) {
                mvDepth[iL] = mbf / disparity;
                mvuRight[iL] = bestuR;
                vDistIdx.push_back(pair(bestDist, iL));
            }
        }
    }
    
    // step5. 删除离群点: 匹配距离大于平均匹配距离2.1倍的视为误匹配
    sort(vDistIdx.begin(), vDistIdx.end());
    const float median = vDistIdx[vDistIdx.size() / 2].first;
    const float thDist = 1.5f * 1.4f * median;
    for (int i = vDistIdx.size() - 1; i >= 0; i--) {
        if (vDistIdx[i].first < thDist)
            break;
        else {
            mvuRight[vDistIdx[i].second] = -1;
            mvDepth[vDistIdx[i].second] = -1;
        }
    }
}

对于RGBD特征,根据深度信息构造虚拟右目图像。

特征点分配AssigenFeaturesToGrid()

在对特征点进行预处理后,将特征点分配到4864列的网格中以加速匹配

对于双目和RGBD相机,分别执行以下Frame构造函数

// 双目相机Frame构造函数
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor *extractorLeft, ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    : mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mpReferenceKF(static_cast(NULL)) {
    
        
	// step0. 帧ID自增
    mnId = nNextId++;

    // step1. 计算金字塔参数
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

    // step2. 提取双目图像特征点
    thread threadLeft(&Frame::ExtractORB, this, 0, imLeft);
    thread threadRight(&Frame::ExtractORB, this, 1, imRight);
    threadLeft.join();
    threadRight.join();

    N = mvKeys.size();
    if (mvKeys.empty())
        return;

    // step3. 畸变矫正,实际上UndistortKeyPoints()不对双目图像进行矫正
    UndistortKeyPoints();

    // step4. 双目图像特征点匹配
    ComputeStereoMatches();
        
	// step5. 第一次调用构造函数时计算static变量
    if (mbInitialComputations) {
        ComputeImageBounds(imLeft);
        mfGridElementWidthInv = static_cast(FRAME_GRID_COLS) / static_cast(mnMaxX - mnMinX);
        mfGridElementHeightInv = static_cast(FRAME_GRID_ROWS) / static_cast(mnMaxY - mnMinY);
        fx = K.at(0, 0);
        fy = K.at(1, 1);
        cx = K.at(0, 2);
        cy = K.at(1, 2);
        invfx = 1.0f / fx;
        invfy = 1.0f / fy;
		
        // 计算完成,标志复位
        mbInitialComputations = false;
    }
	
	mvpMapPoints = vector(N, static_cast(NULL));	// 初始化本帧的地图点
    mvbOutlier = vector(N, false);	// 标记当前帧的地图点不是外点
    mb = mbf / fx;		// 计算双目基线长度

    // step6. 将特征点分配到网格中
    AssignFeaturesToGrid();
}
// RGBD相机Frame构造函数
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    : mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {
    // step0. 帧ID自增
    mnId = nNextId++;

    // step1. 计算金字塔参数
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

    // step2. 提取左目图像特征点
    ExtractORB(0, imGray);

    N = mvKeys.size();
    if (mvKeys.empty())
        return;

    // step3. 畸变矫正
    UndistortKeyPoints();
    
    // step4. 根据深度信息构造虚拟右目图像
    ComputeStereoFromRGBD(imDepth);

    mvpMapPoints = vector(N, static_cast(NULL));
    mvbOutlier = vector(N, false);

    // step5. 第一次调用构造函数时计算static变量
    if (mbInitialComputations) {
        ComputeImageBounds(imLeft);
        mfGridElementWidthInv = static_cast(FRAME_GRID_COLS) / static_cast(mnMaxX - mnMinX);
        mfGridElementHeightInv = static_cast(FRAME_GRID_ROWS) / static_cast(mnMaxY - mnMinY);
        fx = K.at(0, 0);
        fy = K.at(1, 1);
        cx = K.at(0, 2);
        cy = K.at(1, 2);
        invfx = 1.0f / fx;
        invfy = 1.0f / fy;
		
        // 计算完成,标志复位
        mbInitialComputations = false;
    }
        
	mvpMapPoints = vector(N, static_cast(NULL));	// 初始化本帧的地图点
    mvbOutlier = vector(N, false);	// 标记当前帧的地图点不是外点
    mb = mbf / fx;		// 计算双目基线长度


    // step6. 将特征点分配到网格中
    AssignFeaturesToGrid();
}

Tracking线程每收到一帧图像,就调用函数Tracking::GrabImageMonocular()Tracking::GrabImageStereo()Tracking::GrabImageRGBD()创建一个Frame对象,赋值给mCurrentFrame.

函数跟踪结束后,会将mCurrentFrame赋值给mLastFrame

关键帧KeyFrame 

​​​​​​ORB-SLAM2代码详解05: 关键帧KeyFrame_ncepu_Chen的博客-CSDN博客_slam 关键帧 

 能看到同一地图点的两关键帧之间存在共视关系,共视地图点的数量被称为权重mConnectedKeyFrameWeights记录了当前关键帧的共视关键帧及权重

mvpOrderedConnectedKeyFrames记录了所有共视关键帧,按权重从大到小排序

mvOrderedWeights记录了所有共视权重,按从大到小排序

基于当前关键帧对地图点的观测构造共视图UpdateConnections()

void KeyFrame::UpdateConnections() {
    
    // 1. 通过遍历当前帧地图点获取其与其它关键帧的共视程度,存入变量KFcounter中
    vector vpMP;
    {
        unique_lock lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }
    map KFcounter; 
    for (MapPoint *pMP : vpMP) {
        map observations = pMP->GetObservations();
        for (map::iterator mit = observations.begin(); mit != observations.end(); mit++) {
            if (mit->first->mnId == mnId)		// 与当前关键帧本身不算共视
                continue;
            KFcounter[mit->first]++;
        }
    }
  
    // step2. 找到与当前关键帧共视程度超过15的关键帧,存入变量vPairs中
    vector > vPairs;
    int th = 15;
    int nmax = 0;
    KeyFrame *pKFmax = NULL;   
    for (map::iterator mit = KFcounter.begin(), mend = KFcounter.end(); mit != mend; mit++) {
        if (mit->second > nmax) {
            nmax = mit->second;
            pKFmax = mit->first;
        }
        if (mit->second >= th) {
            vPairs.push_back(make_pair(mit->second, mit->first));
            (mit->first)->AddConnection(this, mit->second);				// 对超过阈值的共视边建立连接
        }
    }

    //  step3. 对关键帧按照共视权重降序排序,存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中
    sort(vPairs.begin(), vPairs.end());
    list lKFs;
    list lWs;
    for (size_t i = 0; i < vPairs.size(); i++) {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }
    {
        unique_lock lockCon(mMutexConnections);
        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector(lKFs.begin(), lKFs.end());
        mvOrderedWeights = vector(lWs.begin(), lWs.end());

        // step4. 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
        if (mbFirstConnection && mnId != 0) {
            mpParent = mvpOrderedConnectedKeyFrames.front();
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }
    }
}

当关键帧和地图点的连接关系发生变化,UpdateConnections()就会被调用

生成树mpParent、mspChildrens

在ORB-SLAM2中,保存所有关键帧构成的最小生成树(优先选择权重大的边作为生成树的边),在回环闭合时只需对最小生成树做BA优化就能以最小代价优化所有关键帧和地图点的位姿,相比于优化共视图大大减少了计算量

 新关键帧的父关键帧会被设为其共视程度最高的共视关键帧

void KeyFrame::UpdateConnections() {
    
    // 更新共视图信息
	// ...
    
    // 更新关键帧信息: 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
    // 该操作会改变当前关键帧的成员变量mpParent和父关键帧的成员变量mspChildrens
    unique_lock lockCon(mMutexConnections);
    if (mbFirstConnection && mnId != 0) {
        mpParent = mvpOrderedConnectedKeyFrames.front();
        mpParent->AddChild(this);
        mbFirstConnection = false;
    }
}

KeyFrame的删除过程也采取先标记再清除的方式,参与回环检测的关键帧具有不被删除的特权

当一个关键帧被删除时,其父关键帧所有子关键帧的生成树信息也会受到影响,需要为其所有子关键帧寻找新的父关键帧,如果父关键帧找的不好的话,就会产生回环,导致生成树就断开.

采用类似于最小生成树算法中的加边法重新构建生成树结构: 每次循环取权重最高的候选边建立父子连接关系,并将新加入生成树的子节点到加入候选父节点集合sParentCandidates

在回环检测中回环矫正函数在调用本质图BA优化前会先添加当前关键帧和闭环匹配关键帧之间的回环关系。

 

你可能感兴趣的:(计算机视觉,人工智能,学习,算法,c++)