Python转C++的Direct Linear Transformation(DLT)函数正确性验证

本人Python经验有限，参考某博客的DLT（Direct Linear Transformation）Python函数，将其转换为C版本，但运行未得到预期结果，无法确定是DLT函数转换错误还是其他问题，请求验证C版本是否正确。

原Python实现 #

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;
}

代码正确性分析 #

从逻辑上看，C++版本的矩阵A构造与原Python代码完全一致，每一行的计算规则都严格对应。但需要注意几个可能导致结果异常的细节：

1. SVD输出匹配性：
   Python的scipy.linalg.svd返回的Vh是转置后的V矩阵，OpenCV的cv::SVD::compute返回的vt同样是V的转置，且两者默认都按奇异值从大到小排序，因此取最后一行向量的逻辑是正确的。

2. 数据类型一致性：
   C++版本用CV_64F创建矩阵，与Python的numpy默认float64类型匹配，但需确保传入的P1、P2参数确实是CV_64F类型，若传入CV_32F直接用at<double>会导致数据错误。

3. 零值判断的严谨性：
   直接判断dDivisor != 0.0存在浮点精度风险，建议改为fabs(dDivisor) > 1e-8，避免因微小浮点误差导致错误。

4. 外部输入验证：
   若转换逻辑无问题，需检查外部输入：比如P1、P2的标定参数是否正确，point1、point2的像素坐标是否严格对应同一空间点。

内容的提问来源于stack exchange，提问作者user2062604