ORB-SLAM2学习笔记——ComputeH21函数、ComputeF21函数

1、ComputeH21函数

cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{
    const int N = vP1.size();

    cv::Mat A(2*N,9,CV_32F);

    for(int i=0; i<N; i++)
    {
        //提取像素点坐标，归一化平面上的
        const float u1 = vP1[i].x;
        const float v1 = vP1[i].y;
        const float u2 = vP2[i].x;
        const float v2 = vP2[i].y;
        //Af = 0
        A.at<float>(2*i,0) = 0.0;
        A.at<float>(2*i,1) = 0.0;
        A.at<float>(2*i,2) = 0.0;
        A.at<float>(2*i,3) = -u1;
        A.at<float>(2*i,4) = -v1;
        A.at<float>(2*i,5) = -1;
        A.at<float>(2*i,6) = v2*u1;
        A.at<float>(2*i,7) = v2*v1;
        A.at<float>(2*i,8) = v2;

        A.at<float>(2*i+1,0) = u1;
        A.at<float>(2*i+1,1) = v1;
        A.at<float>(2*i+1,2) = 1;
        A.at<float>(2*i+1,3) = 0.0;
        A.at<float>(2*i+1,4) = 0.0;
        A.at<float>(2*i+1,5) = 0.0;
        A.at<float>(2*i+1,6) = -u2*u1;
        A.at<float>(2*i+1,7) = -u2*v1;
        A.at<float>(2*i+1,8) = -u2;

    }
    cv::Mat u,w,vt;
    //svd分解 a：要分解的 w：奇异矩阵 u，vt：分解矩阵 vt为v的转置
    //输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存
    //FULL_UV=把U和VT补充成单位正交方阵
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    //得到本质矩阵的解H， 后面用于求解t R
    return vt.row(8).reshape(0, 3);
}

ComputeF21函数

//计算基础矩阵
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
{
    const int N = vP1.size();
    //定义矩阵 N*9  
    cv::Mat A(N,9,CV_32F);

    for(int i=0; i<N; i++)
    {
        //提取像素点坐标，归一化平面上的
        const float u1 = vP1[i].x;
        const float v1 = vP1[i].y;
        const float u2 = vP2[i].x;
        const float v2 = vP2[i].y;
        //Af = 0
        A.at<float>(i,0) = u2*u1;
        A.at<float>(i,1) = u2*v1;
        A.at<float>(i,2) = u2;
        A.at<float>(i,3) = v2*u1;
        A.at<float>(i,4) = v2*v1;
        A.at<float>(i,5) = v2;
        A.at<float>(i,6) = u1;
        A.at<float>(i,7) = v1;
        A.at<float>(i,8) = 1;
    }

    cv::Mat u,w,vt;
    //svd分解 a：要分解的 w：奇异矩阵 u，vt：分解矩阵 vt为v的转置
    //输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存
    //FULL_UV=把U和VT补充成单位正交方阵
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    //vt第9行取出来 分成3行 也就是3*3 实际是v的第9列
    cv::Mat Fpre = vt.row(8).reshape(0, 3);
    //由于基础矩阵的秩为 2，而我们不敢保证计算得到的这个结果的秩为2，所以需要通过第二次奇异值分解,来强制使其秩为 2
    // 对初步得来的基础矩阵继续进行一次奇异值分解
    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    //奇异值约束 第三个为0 本质矩阵内在性质 尺度不确定
    w.at<float>(2)=0;
    // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回
    return  u*cv::Mat::diag(w)*vt;
}