我对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;
}
1条答案
按热度按时间ghg1uchk1#
通过将OP链接中的整个示例翻译成C++,并将DLT的答案与cv::triangulatePoints()的答案进行比较,我已经验证了我的原始翻译是正确的。