从 Python 使用 C++ 进行直接线性变换

问：

我对 Python 没有经验，但我想使用这个 Python 函数来执行我从这篇博文中获得的直接线性转换：

https://temugeb.github.io/opencv/python/2021/02/02/stereo-camera-calibration-and-triangulation.html

我试图将这个函数转换为 C++。下面是原始的Python函数以及我的C++版本。我的 C++ 版本是否正确？P1 和 P2 是 3x4 矩阵，point1 和 point2 是二维向量。

问题是我没有得到我期望的结果，我不知道是因为我没有正确重现这个 DLT 函数，还是其他原因。

def DLT(P1, P2, point1, point2):

    A = [point1[1]*P1[2,:] - P1[1,:],
         P1[0,:] - point1[0]*P1[2,:],
         point2[1]*P2[2,:] - P2[1,:],
         P2[0,:] - point2[0]*P2[2,:]
        ]
    A = np.array(A).reshape((4,4))
    #print('A: ')
    #print(A)

    B = A.transpose() @ A
    from scipy import linalg
    U, s, Vh = linalg.svd(B, full_matrices = False)

    print('Triangulated point: ')
    print(Vh[3,0:3]/Vh[3,3])
    return Vh[3,0:3]/Vh[3,3]

这是我的 C++ 版本，我希望它是正确的：

// Perform direct linear transformation
cv::Point3f DLT(cv::Mat P1, cv::Mat P2, cv::Point2f point1, cv::Point2f point2)
{
    // The 3D point to return
    cv::Point3f Retv(0.0f, 0.0f, 0.0f);

    // Build the DLT A 4x4 matrix
    cv::Mat A(4, 4, CV_64F);

    // First row
    A.at<double>(0, 0) = ((point1.y * P1.at<double>(2, 0)) - P1.at<double>(1, 0));
    A.at<double>(0, 1) = ((point1.y * P1.at<double>(2, 1)) - P1.at<double>(1, 1));
    A.at<double>(0, 2) = ((point1.y * P1.at<double>(2, 2)) - P1.at<double>(1, 2));
    A.at<double>(0, 3) = ((point1.y * P1.at<double>(2, 3)) - P1.at<double>(1, 3));

    // Second row
    A.at<double>(1, 0) = (P1.at<double>(0, 0) - (point1.x * P1.at<double>(2, 0)));
    A.at<double>(1, 1) = (P1.at<double>(0, 1) - (point1.x * P1.at<double>(2, 1)));
    A.at<double>(1, 2) = (P1.at<double>(0, 2) - (point1.x * P1.at<double>(2, 2)));
    A.at<double>(1, 3) = (P1.at<double>(0, 3) - (point1.x * P1.at<double>(2, 3)));

    // Third row
    A.at<double>(2, 0) = ((point2.y * P2.at<double>(2, 0)) - P2.at<double>(1, 0));
    A.at<double>(2, 1) = ((point2.y * P2.at<double>(2, 1)) - P2.at<double>(1, 1));
    A.at<double>(2, 2) = ((point2.y * P2.at<double>(2, 2)) - P2.at<double>(1, 2));
    A.at<double>(2, 3) = ((point2.y * P2.at<double>(2, 3)) - P2.at<double>(1, 3));

    // Fourth row
    A.at<double>(3, 0) = (P2.at<double>(0, 0) - (point2.x * P2.at<double>(2, 0)));
    A.at<double>(3, 1) = (P2.at<double>(0, 1) - (point2.x * P2.at<double>(2, 1)));
    A.at<double>(3, 2) = (P2.at<double>(0, 2) - (point2.x * P2.at<double>(2, 2)));
    A.at<double>(3, 3) = (P2.at<double>(0, 3) - (point2.x * P2.at<double>(2, 3)));

    // Calculate A transpose
    cv::Mat ATranspose;
    cv::transpose(A, ATranspose);

    // Compute the final matrix on which to perform singular value decomposition
    cv::Mat B = ATranspose * A;

    // Compute singular value decomposition
    cv::Mat w, u, vt;
    cv::SVD::compute(B, w, u, vt);

    // If the result is of the expected size
    if ((4 == vt.rows) && (4 == vt.cols))
    {
        // Get the fourth in homogeneous coordinates
        const double dDivisor = vt.at<double>(3, 3);

        // If we have a non-zero fourth in the homogeneous coordinates
        if (dDivisor != 0.0)
        {
            // Fill in the point to return
            Retv.x = static_cast<float>(vt.at<double>(3, 0) / dDivisor);
            Retv.y = static_cast<float>(vt.at<double>(3, 1) / dDivisor);
            Retv.z = static_cast<float>(vt.at<double>(3, 2) / dDivisor);
        }
    }

    // Return the point we just calculated
    return Retv;
}