单目初始化从单应阵中恢复位姿、依据三角化检验位姿

ORB-SLAM2

---

---

---

代码

//在完成评分后，开始使用单应矩阵或者基础阵进行位姿的恢复
	if(RH>0.40)//在计算完评分后 选择利用那种方法来恢复位姿
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

首先，假设RH>0.4,使用单应阵来恢复位姿

bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
                      cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool>            &vbTriangulated, float minParallax, int minTriangulated)
//传参：匹配的内点、参考帧到当前帧的H阵，相机的内参阵、计算得出的相机旋转阵、计算出的平移、世界坐标系下三角化测量特征点对之后得到的特征点的空间坐标、特征点被三角化的标记、三角化测量时需要马努的最小视角、恢复运动需要最少三角化测量成功的点的个数
{
	int N=0;//统计内点匹配的个数
    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)//遍历内点匹配的size
        if(vbMatchesInliers[i])//查看该索引对应的匹配对是否为内点
            N++;

	cv::Mat invK = K.inv();
    cv::Mat A = invK*H21*K;//K-1*H21*KVV

    cv::Mat U,w,Vt,V;
    cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);//对A进行奇异值分解
    V=Vt.t();

	float s = cv::determinant(U)*cv::determinant(Vt);

    float d1 = w.at<float>(0);//获得奇异值
    float d2 = w.at<float>(1);
    float d3 = w.at<float>(2);

	if(d1/d2<1.00001 || d2/d3<1.00001)//奇异值的排布一般是从大到小的
    {
        return false;//如果不是，则返回
    }

	
	vector<cv::Mat> vR, vt, vn;
    vR.reserve(8);//定义8中情况的R，t
    vt.reserve(8);
    vn.reserve(8);

    //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
    float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
    float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
    float x1[] = {aux1,aux1,-aux1,-aux1};
    float x3[] = {aux3,-aux3,aux3,-aux3};
	
	 //case d'=d2 d>0与d<0的两种情况
    float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);

    float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
    float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};

    for(int i=0; i<4; i++)
    {
        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
        Rp.at<float>(0,0)=ctheta;
        Rp.at<float>(0,2)=-stheta[i];
        Rp.at<float>(2,0)=stheta[i];
        Rp.at<float>(2,2)=ctheta;

        cv::Mat R = s*U*Rp*Vt;
        vR.push_back(R);

        cv::Mat tp(3,1,CV_32F);
        tp.at<float>(0)=x1[i];
        tp.at<float>(1)=0;
        tp.at<float>(2)=-x3[i];
        tp*=d1-d3;

        cv::Mat t = U*tp;
        vt.push_back(t/cv::norm(t));

        cv::Mat np(3,1,CV_32F);
        np.at<float>(0)=x1[i];
        np.at<float>(1)=0;
        np.at<float>(2)=x3[i];

        cv::Mat n = V*np;
        if(n.at<float>(2)<0)
            n=-n;
        vn.push_back(n);
    }

    //case d'=-d2
    float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);

    float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
    float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};

    for(int i=0; i<4; i++)
    {
        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
        Rp.at<float>(0,0)=cphi;
        Rp.at<float>(0,2)=sphi[i];
        Rp.at<float>(1,1)=-1;
        Rp.at<float>(2,0)=sphi[i];
        Rp.at<float>(2,2)=-cphi;

		 cv::Mat R = s*U*Rp*Vt;
        vR.push_back(R);

        cv::Mat tp(3,1,CV_32F);
        tp.at<float>(0)=x1[i];
        tp.at<float>(1)=0;
        tp.at<float>(2)=x3[i];
        tp*=d1+d3;

        cv::Mat t = U*tp;
        vt.push_back(t/cv::norm(t));

        cv::Mat np(3,1,CV_32F);
        np.at<float>(0)=x1[i];
        np.at<float>(1)=0;
        np.at<float>(2)=x3[i];

        cv::Mat n = V*np;
        if(n.at<float>(2)<0)
            n=-n;
        vn.push_back(n);
    }
	//对8组解进行验证，选择在相机前方最多的3D最优解
	int bestGood = 0;
    int secondBestGood = 0;    
    int bestSolutionIdx = -1;
    float bestParallax = -1;
    vector<cv::Point3f> bestP3D;
    vector<bool> bestTriangulated;

    // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
    // We reconstruct all hypotheses and check in terms of triangulated points and parallax
    for(size_t i=0; i<8; i++)
    {
        float parallaxi;//第i组解对应较大的视交差
        vector<cv::Point3f> vP3Di//三角化后空间点的坐标
        vector<bool> vbTriangulatedi;//特征点是否被三角化
        int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);//计算good的数目点，调用Initializer::CheckRT

        if(nGood>bestGood)
        {
            secondBestGood = bestGood;
            bestGood = nGood;
            bestSolutionIdx = i;
            bestParallax = parallaxi;
            bestP3D = vP3Di;
            bestTriangulated = vbTriangulatedi;
        }
        else if(nGood>secondBestGood)
        {
            secondBestGood = nGood;
        }
    }

进入CheckRT（）

//利用R t来进行三角化，并看R t是否正确
int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
                       const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,
                       const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)
/*传参：当前组的旋转阵与平移向量
		特征点
		特征点匹配关系与内点标记
		相机内参阵
		存储三角化测量后特征点空间坐标
		三角化过程中允许最大的重投影误差
		特征点是否成功被三角化
		解这组特征点三角化测量比较大的视
*/
{
    // Calibration parameters
    const float fx = K.at<float>(0,0);//取出相机内参
    const float fy = K.at<float>(1,1);
    const float cx = K.at<float>(0,2);
    const float cy = K.at<float>(1,2);

    vbGood = vector<bool>(vKeys1.size(),false);//参考帧中的特征点是否是good
    vP3D.resize(vKeys1.size());//将三维点的大小重设

    vector<float> vCosParallax;//每对特诊点的视差
    vCosParallax.reserve(vKeys1.size());

    // Camera 1 Projection Matrix K[I|0]
    cv::Mat P1(3,4,CV_32F,cv::Scalar(0));//计算相机的投影阵第一帧
    K.copyTo(P1.rowRange(0,3).colRange(0,3));

    cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);//第一个相机的光心

    // Camera 2 Projection Matrix K[R|t]
    cv::Mat P2(3,4,CV_32F);//第二个相机投影阵
    R.copyTo(P2.rowRange(0,3).colRange(0,3));
    t.copyTo(P2.rowRange(0,3).col(3));
    P2 = K*P2;

    cv::Mat O2 = -R.t()*t;//第二个相机光心在世界坐标系下的位置

    int nGood=0;

    for(size_t i=0, iend=vMatches12.size();i<iend;i++)//遍历
    {
        if(!vbMatchesInliers[i])//如果不是内点的匹配点，跳过
            continue;

        const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];//是的化取出1，2中的keyponints
        const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
        cv::Mat p3dC1;

        Triangulate(kp1,kp2,P1,P2,p3dC1);//三角化计算三维点

        if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))//检查它里面的一些值是不是无限值即它合不合法
        {
            vbGood[vMatches12[i].first]=false;//记为false
            continue;
        }

        // Check parallax
        cv::Mat normal1 = p3dC1 - O1;//通过三维点的正负，以及两光心视差大小来检验是否合法
        float dist1 = cv::norm(normal1);//三维点到光心1的模长

        cv::Mat normal2 = p3dC1 - O2;
        float dist2 = cv::norm(normal2);//光心2到三维点的模长

        float cosParallax = normal1.dot(normal2)/(dist1*dist2);//求余弦夹角

        // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
        if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)//z是否为负的，夹角<0.99998
            continue;

        // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
        cv::Mat p3dC2 = R*p3dC1+t;//转到相机2坐标系下

        if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)//计算z值与夹角
            continue;

        // Check reprojection error in first image
        float im1x, im1y;//计算重投影误差
        float invZ1 = 1.0/p3dC1.at<float>(2);
        im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
        im1y = fy*p3dC1.at<float>(1)*invZ1+cy;

        float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);

        if(squareError1>th2)//重投影误差太大
            continue;

        // Check reprojection error in second image
        float im2x, im2y;//第二个相机
        float invZ2 = 1.0/p3dC2.at<float>(2);
        im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
        im2y = fy*p3dC2.at<float>(1)*invZ2+cy;

        float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);

        if(squareError2>th2)
            continue;

        vCosParallax.push_back(cosParallax);//统计检验的3点个数
        vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
        nGood++;

        if(cosParallax<0.99998)
            vbGood[vMatches12[i].first]=true;
    }

    if(nGood>0)
    {
        sort(vCosParallax.begin(),vCosParallax.end());//按照余弦值进行排序

        size_t idx = min(50,int(vCosParallax.size()-1));
        parallax = acos(vCosParallax[idx])*180/CV_PI;//转换为弧度制
    }
    else
        parallax=0;

    return nGood;
}