I have implemented a 2D->3D point triangulation method, which uses 2 poses P1 and P2 and 2 points back-projected to z=1 point1 and point2 to recover depth:
Matx14d A1 = point1(0) * P1.row(2) - P1.row(0), A2 = point1(1) * P1.row(2) - P1.row(1), A3 = point2(0) * P2.row(2) - P2.row(0), A4 = point2(1) * P2.row(2) - P2.row(1); double normA1 = norm(A1), normA2 = norm(A2), normA3 = norm(A3), normA4 = norm(A4); Matx44d A(A1(0)/normA1, A1(1)/normA1, A1(2)/normA1, A1(3)/normA1, A2(0)/normA2, A2(1)/normA2, A2(2)/normA2, A2(3)/normA2, A3(0)/normA3, A3(1)/normA3, A3(2)/normA3, A3(3)/normA3, A4(0)/normA4, A4(1)/normA4, A4(2)/normA4, A4(3)/normA4); SVD svd(A, SVD::MODIFY_A); Mat_<double> svdVT = svd.vt; Mat smallest = svdVT.row(3); double scale = smallest.at<double>(0,3); if (scale == 0.0) scale = 0.00001; Point3d point3D(smallest.at<double>(0,0)/scale, smallest.at<double>(0,1)/scale, smallest.at<double>(0,2)/scale);
This is based on the book Multiple View Geometry in Computer Vision. However, this doesn't seem to provide the right results visually and in fact, when I calculate the difference between the projected 3D point and the original image point coordinates, there is an error of >100px.
What is the problem? Even with synthetic values and transformations I can't get a good enough triangulation.