ORB_SLAM2学习笔记——ReconstructH函数

1、理论部分 2、ReconstructH 代码部分

1、理论部分

本部分参考吴博的ppt，在此感谢吴博的无私分享

2、ReconstructH 代码部分

//从本质矩阵中恢复出R t；
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)
{
     
    int N=0;
    //；遍历所有匹配点
    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
        if(vbMatchesInliers[i])
            N++;

    // We recover 8 motion hypotheses using the method of Faugeras et al.
    // Motion and structure from motion in a piecewise planar environment.
    // International Journal of Pattern Recognition and Artificial Intelligence, 1988
    //相机内参的逆矩阵
    cv::Mat invK = K.inv();
    //p1 = invK * H21 * K * P2 定义两个像素点的转化关系
    cv::Mat A = invK*H21*K;
    
    //对A进行SVD分解
    cv::Mat U,w,Vt,V;
    cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
    //VT的专置 V
    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);
    //如果 d1 < d2 或者 d2 < d3 那么返回fales,出错；
    //SVD分解后的奇异值一般都是降序；排列的
    if(d1/d2<1.00001 || d2/d3<1.00001)
    {
     
        return false;
    }
    //给 vR（旋转矩阵）, vt（平移向量）, vn（空间向量） 预存 8 位的空间 
    vector<cv::Mat> vR, vt, vn;
    vR.reserve(8);
    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
    //表示X1，X3的绝对值数值
    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
    //角度的正弦函数表达式
    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};
    //四种情况依次赋值
    //根据不同的e1 e3组合所得出来的四种R t的解
    //      | ctheta      0   -aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0    aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       | aux3|

    //      | ctheta      0    aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0   -aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       | aux3|
    for(int i=0; i<4; i++)
    {
     
        // 给R赋值 （绕Y轴旋转了一定角度）
        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;
        //计算实际的R
        cv::Mat R = s*U*Rp*Vt;
        vR.push_back(R);
        //计算t
        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;
        //计算实际的t
        cv::Mat t = U*tp;
        //对t进行归一化存储
        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);
    }

    //最好点
    int bestGood = 0;
    //次好点
    int secondBestGood = 0;    
    //最好点索引
    int bestSolutionIdx = -1;
    //最大视差角
    float bestParallax = -1;
    //最好点的3D点
    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
    //对于 d'=d2 和 d'=-d2 分别对应8组(R t) 分别验证每一种解的情况
    for(size_t i=0; i<8; i++)
    {
     
        //第i组解对应的比较大的视差角
        float parallaxi;
        //三角化测量之后的特征点的空间坐标
        vector<cv::Point3f> vP3Di;
        //特征点对是否被三角化的标记
        vector<bool> vbTriangulatedi;
        //计算好点的个数
            int nGood = CheckRT(vR[i],vt[i],					//当前组解的旋转矩阵和平移向量
							mvKeys1,mvKeys2,				//特征点
							mvMatches12,vbMatchesInliers,	//特征匹配关系以及Inlier标记
							K,								//相机的内参数矩阵
							vP3Di, 							//存储三角化测量之后的特征点空间坐标的
							4.0*mSigma2,					//三角化过程中允许的最大重投影误差
							vbTriangulatedi,				//特征点是否被成功进行三角测量的标记
							parallaxi);						// 这组解在三角化测量的时候的比较大的视差角mSigma2, vbTriangulatedi, parallaxi);
        //如果好的解大于最好的进行替换，大于次好的也进行替换
        if(nGood>bestGood)
        {
     
            secondBestGood = bestGood;
            bestGood = nGood;
            bestSolutionIdx = i;
            bestParallax = parallaxi;
            bestP3D = vP3Di;
            bestTriangulated = vbTriangulatedi;
        }
        else if(nGood>secondBestGood)
        {
     
            secondBestGood = nGood;
        }
    }

    //最优解足够多 ； 最大视差角大于阈值 ； 能三角化的点大于阈值 ；好点的数量达到总数的90%
    if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N)
    {
     
        //把最好的R t 的索引 给 R21 t21
        vR[bestSolutionIdx].copyTo(R21);
        vt[bestSolutionIdx].copyTo(t21);
        //最好的3D点 赋值给 vP3D
        vP3D = bestP3D;
        //获取特征点的被成功进行三角化的标记
        vbTriangulated = bestTriangulated;
        //成功返回True
        return true;
    }
    //失败返回false
    return false;
}