Chirality Check for decomposeHomographyMat

C++
1

Hi everyone. I’m searching for an equal function like recoverPose() for Homography Matrix.
In particular, i’m interested to obtain the number of inlier that pass the cheirality check.

2

I’ve tried to implement my self. Is it correct?

int recoverPoseHomography(Mat H, KpAsPoint2f_Match inlier, Mat cameraMatrix, Mat& R, Mat& t)
{
    //Decompose Homography Matrix
    vector<Mat> R_candidates, t_candidates;
    int solutions = decomposeHomographyMat(H, cameraMatrix, R_candidates, t_candidates, noArray());

    //n_solutions usually 4
    //Cheirality Check: triangulated_points.z > 0

    Mat eye_m = (Mat1d(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
    Mat zero_v = (Mat1d(3, 1) << 0, 0, 0);
    Mat proj_std = projectionMatrix(eye_m, zero_v, cameraMatrix);

    vector<Mat> triangulateCandidates(solutions);
    vector<int> n_goodTP(solutions);

    for(int i = 0; i < solutions; i++)
    {
        triangulatePoints(proj_std, projectionMatrix(R_candidates[i], t_candidates[i], cameraMatrix), inlier.Kpoints1, inlier.Kpoints2, triangulateCandidates[i]);
        triangulateCandidates[i] = my_convertFromHom(triangulateCandidates[i]);

        for(int j = 0; j < triangulateCandidates[i].cols; j++)
        {
            if(triangulateCandidates[i].at<double>(2, j) > 0)
                n_goodTP[i]++;

        }

    }
    
    //extract index of best candidates
    int idx_max = max_element(n_goodTP.begin(), n_goodTP.end()) - n_goodTP.begin();

R = R_candidates[idx_max];
t = t_candidates[idx_max];

return n_goodTP[idx_max];
}