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