How to correctly use cv::triangulatePoints()

Question

I am trying to triangulate some points with OpenCV and I found this cv::triangulatePoints() function. The problem is that there is almost no documentation or ex

Answer 1

I tried cv::triangulatePoints, but somehow it calculates garbage. I was forced to implement a linear triangulation method manually, which returns a 4x1 matrix for the triangulated 3D point:

Mat triangulate_Linear_LS(Mat mat_P_l, Mat mat_P_r, Mat warped_back_l, Mat warped_back_r)
{
    Mat A(4,3,CV_64FC1), b(4,1,CV_64FC1), X(3,1,CV_64FC1), X_homogeneous(4,1,CV_64FC1), W(1,1,CV_64FC1);
    W.at(0,0) = 1.0;
    A.at(0,0) = (warped_back_l.at(0,0)/warped_back_l.at(2,0))*mat_P_l.at(2,0) - mat_P_l.at(0,0);
    A.at(0,1) = (warped_back_l.at(0,0)/warped_back_l.at(2,0))*mat_P_l.at(2,1) - mat_P_l.at(0,1);
    A.at(0,2) = (warped_back_l.at(0,0)/warped_back_l.at(2,0))*mat_P_l.at(2,2) - mat_P_l.at(0,2);
    A.at(1,0) = (warped_back_l.at(1,0)/warped_back_l.at(2,0))*mat_P_l.at(2,0) - mat_P_l.at(1,0);
    A.at(1,1) = (warped_back_l.at(1,0)/warped_back_l.at(2,0))*mat_P_l.at(2,1) - mat_P_l.at(1,1);
    A.at(1,2) = (warped_back_l.at(1,0)/warped_back_l.at(2,0))*mat_P_l.at(2,2) - mat_P_l.at(1,2);
    A.at(2,0) = (warped_back_r.at(0,0)/warped_back_r.at(2,0))*mat_P_r.at(2,0) - mat_P_r.at(0,0);
    A.at(2,1) = (warped_back_r.at(0,0)/warped_back_r.at(2,0))*mat_P_r.at(2,1) - mat_P_r.at(0,1);
    A.at(2,2) = (warped_back_r.at(0,0)/warped_back_r.at(2,0))*mat_P_r.at(2,2) - mat_P_r.at(0,2);
    A.at(3,0) = (warped_back_r.at(1,0)/warped_back_r.at(2,0))*mat_P_r.at(2,0) - mat_P_r.at(1,0);
    A.at(3,1) = (warped_back_r.at(1,0)/warped_back_r.at(2,0))*mat_P_r.at(2,1) - mat_P_r.at(1,1);
    A.at(3,2) = (warped_back_r.at(1,0)/warped_back_r.at(2,0))*mat_P_r.at(2,2) - mat_P_r.at(1,2);
    b.at(0,0) = -((warped_back_l.at(0,0)/warped_back_l.at(2,0))*mat_P_l.at(2,3) - mat_P_l.at(0,3));
    b.at(1,0) = -((warped_back_l.at(1,0)/warped_back_l.at(2,0))*mat_P_l.at(2,3) - mat_P_l.at(1,3));
    b.at(2,0) = -((warped_back_r.at(0,0)/warped_back_r.at(2,0))*mat_P_r.at(2,3) - mat_P_r.at(0,3));
    b.at(3,0) = -((warped_back_r.at(1,0)/warped_back_r.at(2,0))*mat_P_r.at(2,3) - mat_P_r.at(1,3));
    solve(A,b,X,DECOMP_SVD);
    vconcat(X,W,X_homogeneous);
    return X_homogeneous;
}

the input parameters are two 3x4 camera projection matrices and a corresponding left/right pixel pair (x,y,w).