ORB-SLAM2学习笔记——Triangulate三角化函数

1、 理论部分

看图然后能得到下式，用第三个与前两个结合求解：
u=λP0X
v=λP1X
1=λP2X

2、代码部分

//三角化
//将空间点投影到两个相机平面
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    //构造A矩阵，用于求解空间点 A*X = 0；
    cv::Mat A(4,4,CV_32F);
    
    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
    //对A矩阵进行SVD分解，所得右矩阵的最后一列就是所需的解
    cv::Mat u,w,vt;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();
    //保持解的齐次形式，让最后一个为1；
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}