ORB-SLAM2 单目初始化中随机选择点集、归一化原理、求单应阵、基础阵

ORB-SLAM2

---

---

一、代码

在特征点提取与匹配完成后，回到Tracking.cc中

		if(nmatches<100)//判断，如果特征匹配的数目小于100的话
        {
            delete mpInitializer;//那么就删除这个初始化器。重新开始初始化
            mpInitializer = static_cast<Initializer*>(NULL);
            return;
        }

		v::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches特征点是否成功三角化？

		//H或F进行初始化，得到相对运动，初始化地图点
		if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
        {
            for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
            {
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }

进入Initialize（）函数

bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
//传入的参数是：当前帧，当前帧与参考帧的匹配、R,t，三角化成功的点
{
    // Fill structures with current keypoints and matches with reference frame
    // Reference Frame: 1, Current Frame: 2
    mvKeys2 = CurrentFrame.mvKeysUn;//将当前帧去畸变后的图像特征点传给mvkeys2

    mvMatches12.clear();//清空mvMatches12，记录匹配特征点对的
    mvMatches12.reserve(mvKeys2.size());//将它的大小转化为mvkeys的大小
    mvbMatched1.resize(mvKeys1.size());//记录参考帧1中是否有匹配的特诊点
    for(size_t i=0, iend=vMatches12.size();i<iend; i++)//遍历匹配完的特征点数量，里面存的是当前帧与第一帧匹配点的索引值
    {
        if(vMatches12[i]>=0)//如果里面有值的化
        {
            mvMatches12.push_back(make_pair(i,vMatches12[i]));//把关键帧1中的索引与帧2中的索引pair起来
            mvbMatched1[i]=true;//；标记帧1中的特征点有匹配关系
        }
        else
            mvbMatched1[i]=false;//否则匹配不上
    }

	const int N = mvMatches12.size();//定义匹配点的个数，也就是F1中与F2中的匹配点的匹配关系的索引

    // Indices for minimum set selection
    vector<size_t> vAllIndices;//存储特征点的索引，并分配大小，8点法
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;//RANSAC中某次迭代，样本索引

    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);//索引对应第一帧与第二帧的匹配索引
    }

    // Generate sets of 8 points for each RANSAC iteration
    mvSets = vector< vector<size_t> >(mMaxIterations,
    vector<size_t>(8,0));//RANSAC最大迭代次数，

    DUtils::Random::SeedRandOnce(0);//随机选取样本。随机种子数

    for(int it=0; it<mMaxIterations; it++)//循环迭代次数<200次
    {
        vAvailableIndices = vAllIndices;//索引对应第一帧与第二帧的匹配索引，都是可用的

        // Select a minimum set
        for(size_t j=0; j<8; j++)//选取8个点
        {
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);//在0到最大匹配点的个数之间随机选取数目
            int idx = vAvailableIndices[randi];//将选择的点的索引传给idx

            mvSets[it][j] = idx;//本次迭代对应的第J个索引存到mvSet中，it表示第几次迭代，j表示选择点的索引

            vAvailableIndices[randi] = vAvailableIndices.back();//用最后一个元素将选中元素进行覆盖
            vAvailableIndices.pop_back();//删除最后一个元素
        }
    }//迭代200次，每次选择8个点

	vector<bool> vbMatchesInliersH, vbMatchesInliersF;//记录哪些点是内点
    float SH, SF;//记录计算后的评分
    cv::Mat H, F;

	//开两个线程来加速计算
    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

	//等待线程结束
	threadH.join();
    threadF.join();
	float RH = SH/(SH+SF);//这边是计算评分，
	
	if(RH>0.40)//如果评分大于0.4用H阵来恢复
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)否则F阵
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

    return false;
}

如何计算H阵F阵类似

//1.将当前帧与参考帧的特征点进行归一化
//2.选择8个归一化后的点进行迭代
//3.八点法计算
//4.利用重投影误差来计算RANSAC的评分
//5.保存最优结果  
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{//传参：标记是否为外点？单应阵的得分、单应阵结果
    // Number of putative matches
    const int N = mvMatches12.size();//匹配特诊点的总数

    // Normalize coordinates
    vector<cv::Point2f> vPn1, vPn2;//开始进行归一化
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);//进行归一化
    Normalize(mvKeys2,vPn2, T2);
    cv::Mat T2inv = T2.inv();//反归一化

    // Best Results variables
    score = 0.0;//计算最佳评分
    vbMatchesInliers = vector<bool>(N,false);//最佳评分是特诊点对应的内点，默认是false

    // Iteration variables
    vector<cv::Point2f> vPn1i(8);//某次迭代中的参考帧对应的坐标
    vector<cv::Point2f> vPn2i(8);//某次迭代中当前帧对应的坐标
    cv::Mat H21i, H12i;//H阵与其逆
    vector<bool> vbCurrentInliers(N,false);//当前帧内点 
    float currentScore;

    // Perform all RANSAC iterations and save the solution with highest score
    for(int it=0; it<mMaxIterations; it++)//开始迭代200个
    {
        // Select a minimum set
        for(size_t j=0; j<8; j++)
        {
            int idx = mvSets[it][j];//刚刚选取的八个点

            vPn1i[j] = vPn1[mvMatches12[idx].first];//帧1特征点索引，归一化的点
            vPn2i[j] = vPn2[mvMatches12[idx].second];//帧2的索引
        }

        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);//8点法进行计算，这边是归一化后的点进行单应矩阵的计算
        H21i = T2inv*Hn*T1;//keypoints去除归一化
        H12i = H21i.inv();//计算逆

        currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);//利用重投影误差来对这次进行评分，进入

        if(currentScore>score)
        {
            H21 = H21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

归一化函数

void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
//传参关键点、归一化点的坐标、变换阵
{
    float meanX = 0;
    float meanY = 0;
    const int N = vKeys.size();

    vNormalizedPoints.resize(N);//所有关键点数量大小

    for(int i=0; i<N; i++)
    {
        meanX += vKeys[i].pt.x;//关键点x坐标相加
        
        meanY += vKeys[i].pt.y;//y坐标相加
    }

    meanX = meanX/N;//取均值
    meanY = meanY/N;

    float meanDevX = 0;
    float meanDevY = 0;

    for(int i=0; i<N; i++)
    {
        vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;//所有的x点距离均值的大小
        vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;

        meanDevX += fabs(vNormalizedPoints[i].x);//将距离均值大小都加起来
        meanDevY += fabs(vNormalizedPoints[i].y);
    }

    meanDevX = meanDevX/N;//用这个距离总和再除以特征点数
    meanDevY = meanDevY/N;

    float sX = 1.0/meanDevX;//为变换做准备
    float sY = 1.0/meanDevY;

    for(int i=0; i<N; i++)
    {
        vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
        vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
    }

    T = cv::Mat::eye(3,3,CV_32F);//转换为矩阵形式
    T.at<float>(0,0) = sX;
    T.at<float>(1,1) = sY;
    T.at<float>(0,2) = -meanX*sX;
    T.at<float>(1,2) = -meanY*sY;
}

进入CheckHomography（）函数选择最佳的H阵

//对给定的H阵来进行评分，需要使用卡方检验
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)//传参：参考帧到当前帧的H、当前帧到参考帧的H、匹配好的特侦点对的Inliner的标记、估计误差方差，默认为1
{   


/*
输入  H21  H12  匹配点集mvkeys1

for p1（i） p2(i)  mvkeys1     p1  p2是进行逆归一化后的keypoints
e1=||p2(i)-H21*p2(i)||2           重投影误差
e2=||p1(i)-H12*p1(i)||2

w1=1/(sigma*sigma)
w2=1/(sigma*sigma)

if e1<th
score+=th-e1*w1
if e2<th
score+=th-e2*w2

*/ 
    const int N = mvMatches12.size();//特征点匹配的数目

    const float h11 = H21.at<float>(0,0);//获取参考帧到当前帧的H阵的各个元素
    const float h12 = H21.at<float>(0,1);
    const float h13 = H21.at<float>(0,2);
    const float h21 = H21.at<float>(1,0);
    const float h22 = H21.at<float>(1,1);
    const float h23 = H21.at<float>(1,2);
    const float h31 = H21.at<float>(2,0);
    const float h32 = H21.at<float>(2,1);
    const float h33 = H21.at<float>(2,2);

    const float h11inv = H12.at<float>(0,0);//获取当前帧到参考帧的H阵的各个元素
    const float h12inv = H12.at<float>(0,1);
    const float h13inv = H12.at<float>(0,2);
    const float h21inv = H12.at<float>(1,0);
    const float h22inv = H12.at<float>(1,1);
    const float h23inv = H12.at<float>(1,2);
    const float h31inv = H12.at<float>(2,0);
    const float h32inv = H12.at<float>(2,1);
    const float h33inv = H12.at<float>(2,2);
}
 

    vbMatchesInliers.resize(N);//给特诊点的outlier标记预分配的

    float score = 0;

    const float th = 5.991;//卡方检验的阈值（假设有一个像素的偏差）

    const float invSigmaSquare = 1.0/(sigma*sigma);//平方差的倒数

    for(int i=0; i<N; i++)
    {
        bool bIn = true;//默认都是inlier

        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];//提取参考帧与当前帧的匹配点对
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];

        const float u1 = kp1.pt.x;//keypoints的坐标
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;

        // Reprojection error in first image
        // x2in1 = H12*x2
      //计算F2到F1的重投影误差
        const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);//H阵的推导计算公式
        const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
        const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;

        const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);//重投影

        const float chiSquare1 = squareDist1*invSigmaSquare;//卡方？

        if(chiSquare1>th)//如果大于阈值就算作是离群点
            bIn = false;
        else
            score += th - chiSquare1;//否则内点累计得分，阈值-卡方值，误差越大得分越低

        // Reprojection error in second image
        // x1in2 = H21*x1
		//反向计算
        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;

        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);

        const float chiSquare2 = squareDist2*invSigmaSquare;

        if(chiSquare2>th)
            bIn = false;
        else
            score += th - chiSquare2;

        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }

    return score;
}