[轉]OpenCV三角測量重建triangulatePoints原了解析

opencv源代碼注釋

附上opencv三角測量函數的主要代碼和注釋

cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D)
{
 // preallocate SVD matrices on stack
    cv::Matx<double, 4, 4> matrA;
    cv::Matx<double, 4, 4> matrU;
    cv::Matx<double, 4, 1> matrW;
    cv::Matx<double, 4, 4> matrV;

    CvMat* projPoints[2] = {projPoints1, projPoints2};
    CvMat* projMatrs[2] = {projMatr1, projMatr2};

    /* Solve system for each point */
    for( int i = 0; i < numPoints; i++ )/* For each point */
    {
        /* Fill matrix for current point */
        for( int j = 0; j < 2; j++ )/* For each view */
        {
            double x,y;
            x = cvmGet(projPoints[j],0,i);
            y = cvmGet(projPoints[j],1,i);
            // 注1：構造系數矩陣A
            for( int k = 0; k < 4; k++ )
            {
                matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k);
                matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k);
            }
        }
        // 注2：SVD分解A矩陣
        /* Solve system for current point */
        cv::SVD::compute(matrA, matrW, matrU, matrV);

        /* Copy computed point */
        cvmSet(points4D,0,i,matrV(3,0));/* X */
        cvmSet(points4D,1,i,matrV(3,1));/* Y */
        cvmSet(points4D,2,i,matrV(3,2));/* Z */
        cvmSet(points4D,3,i,matrV(3,3));/* W */
    }
}