I have implemented a 2D->3D point triangulation method, which uses 2 poses P1 and P2 and 2 points 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.