ORB-SLAM3源码阅读笔记（6）-MLPnP-CSDN博客

MLPnPsolver.cc用于在重定位时计算当前帧和候选帧的位姿变换，利用了3D-2D匹配。与ORB-SLAM2中的EPnP算法不同，MLPnP不依赖于相机模型，主要根据相机的反投影函数计算bearing vector。代码中的数学原理和公式都出自这篇论文，我仅对代码进行一下梳理。

1、构造函数

MLPnPsolver::MLPnPsolver(const Frame &F, const vector<MapPoint *> &vpMapPointMatches)://3D(地图点)-2D(Frame中匹配的关键点)匹配
            mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0), mpCamera(F.mpCamera){
        mvpMapPointMatches = vpMapPointMatches;//匹配的地图点
        mvBearingVecs.reserve(F.mvpMapPoints.size());//bearing vector，即3D点在相机坐标系下的方向向量
        mvP2D.reserve(F.mvpMapPoints.size());//匹配的2D点
        mvSigma2.reserve(F.mvpMapPoints.size());//卡方检验中的sigma值，根据每个关键点所在层数不同而有所不同
        mvP3Dw.reserve(F.mvpMapPoints.size());//3D点在世界坐标系下的坐标
        mvKeyPointIndices.reserve(F.mvpMapPoints.size());//匹配的关键点的索引，不连续(因为有的索引并没有匹配)
        mvAllIndices.reserve(F.mvpMapPoints.size());//连续的索引，从0到匹配数-1
        int idx = 0;
        //遍历所有匹配的地图点，计算MLPnP相关的量
        for(size_t i = 0, iend = mvpMapPointMatches.size(); i < iend; i++){
            MapPoint* pMP = vpMapPointMatches[i];//取出地图点
            if(pMP){
                if(!pMP -> isBad()){
                    if(i >= F.mvKeysUn.size()) continue;
                    const cv::KeyPoint &kp = F.mvKeysUn[i];//地图点对应的2D点
                    mvP2D.push_back(kp.pt);//保存2D点坐标
                    mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);//根据所在层保存2D点的sigma值
                    //Bearing vector should be normalized
                    cv::Point3f cv_br = mpCamera->unproject(kp.pt);//反投影，得到相机坐标系下3D点坐标(即bearing vector)
                    cv_br /= cv_br.z;//单位化
                    bearingVector_t br(cv_br.x,cv_br.y,cv_br.z);
                    mvBearingVecs.push_back(br);//保存bearing vector
                    //3D coordinates
                    cv::Mat cv_pos = pMP -> GetWorldPos();//地图点的3D坐标
                    point_t pos(cv_pos.at<float>(0),cv_pos.at<float>(1),cv_pos.at<float>(2));
                    mvP3Dw.push_back(pos);//保存3D点世界坐标系下的坐标
                    mvKeyPointIndices.push_back(i);//关键点索引，有的i是不会被保存的(当pMP坏的时候)
                    mvAllIndices.push_back(idx);//从0开始连续的索引，
                    idx++;
                }
            }
        }
        SetRansacParameters();//设置Ransac参数，包括最小集、迭代次数、最小内点数、每个点的最大误差等
    }

2、主函数–MLPnPsolver::iterate()

算法流程(Ransac框架)：
如果匹配的关键点数小于最小内点数，则直接返回空位姿，因为这时计算出的内点数不可能大于所需的最小内点数了。

 if(N<mRansacMinInliers)
	    {
	        bNoMore = true;
	        return cv::Mat();
	    }

当迭代次数没达到最大迭代次数，则进行循环求解位姿，知道迭代次数达到最大值，或者找到了内点数符合条件的位姿。
每次迭代时，首先随机选出6个点和bearing vector(求解所需的最小集为6)

for(short i = 0; i < mRansacMinSet; ++i)
{
    int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
    int idx = vAvailableIndices[randi];
    bearingVecs[i] = mvBearingVecs[idx];
    p3DS[i] = mvP3Dw[idx];
    indexes[i] = i;
    vAvailableIndices[randi] = vAvailableIndices.back();//选过的索引要删除
    vAvailableIndices.pop_back();
}

利用最小集计算位姿，并保存。

computePose(bearingVecs,p3DS,covs,indexes,result);

检查内点数

void MLPnPsolver::CheckInliers(){
        mnInliersi=0;//内点数
        for(int i=0; i<N; i++)//遍历所有的匹配点
        {
            point_t p = mvP3Dw[i];//3D点的世界坐标系坐标
            cv::Point3f P3Dw(p(0),p(1),p(2));
            cv::Point2f P2D = mvP2D[i];//2D点的坐标
            //将世界坐标系3D点利用计算出来的位姿R、t，变换到相机坐标系下
            float xc = mRi[0][0]*P3Dw.x+mRi[0][1]*P3Dw.y+mRi[0][2]*P3Dw.z+mti[0];
            float yc = mRi[1][0]*P3Dw.x+mRi[1][1]*P3Dw.y+mRi[1][2]*P3Dw.z+mti[1];
            float zc = mRi[2][0]*P3Dw.x+mRi[2][1]*P3Dw.y+mRi[2][2]*P3Dw.z+mti[2];
            cv::Point3f P3Dc(xc,yc,zc);
            cv::Point2f uv = mpCamera->project(P3Dc);//将相机坐标系3D点投影至2D图像平面
            float distX = P2D.x-uv.x;
            float distY = P2D.y-uv.y;
            float error2 = distX*distX+distY*distY;//计算投影坐标和观测值的误差
            if(error2<mvMaxError[i])//如果误差在范围之内，标记该点为内点，内点数+1
            {
                mvbInliersi[i]=true;
                mnInliersi++;
            }
            else
            {
                mvbInliersi[i]=false;//误差超过范围，标记为外点
            }
        }
    }