pythonc++opencvcomputer-visionsvd

Direct Linear Transformation in C++ from Python


I'm not experienced with Python, but I wanted to use this Python function that performs direct linear transformation that I got from this blog post:

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

I've attempted to convert this function into C++. Below is the original Python function as well as my C++ version. Is my C++ version correct? P1 and P2 are 3x4 matrices, and point1 and point2 are two-dimensional vectors.

The issue is that I'm not getting the results I expect, and I don't know if it's because I'm not properly reproducing this DLT function, or if it's something else.

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]

And here is my C++ version that I'm hoping is correct:

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

Solution

  • By translating the entire example in the OP link into C++ and comparing the answer from my DLT with the answer from cv::triangulatePoints(), I've verified that my original translation is correct.