ORB_SLAM2::Initializer::ComputeF21 (OpenCV 细节)

我们首先贴出代码:

cv::Mat Initializer::ComputeF21(const vector &vP1,const vector &vP2)
{
    const int N = vP1.size();

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

    for(int i=0; i(i,0) = u2*u1;
        A.at(i,1) = u2*v1;
        A.at(i,2) = u2;
        A.at(i,3) = v2*u1;
        A.at(i,4) = v2*v1;
        A.at(i,5) = v2;
        A.at(i,6) = u1;
        A.at(i,7) = v1;
        A.at(i,8) = 1;
    }

    cv::Mat u,w,vt;

    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

    cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列

    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

    w.at(2)=0; // 秩2约束,将第3个奇异值设为0

    return  u*cv::Mat::diag(w)*vt;
}

ORB_SLAM2::Initializer::ComputeF21 (OpenCV 细节)_第1张图片

你可能感兴趣的:(orb-slam)