SolvePnP - pose estimation for planar object - ambiguous case

Question

I have an asymmetric circular dot pattern similar to this. I use opencv solvePnP function to estimate the pose of the pattern based on a) known 3D pattern point; b) detected pattern points in 2D image

I found that in some cases (when the pattern frontally face to the camera and tilted a bit), the estimated pose returned by the solvePnP function is unstable. There are two results for almost same pattern pose (no moving). I guess it is due to ambiguous planar poses have same 2D projection.

Is there any solution to it except the temporal filtering as suggested here: Similar issue

Update: I tried to apply this paper which solve the ambiguous case but it didn't work. The code I used is from here. I could get almost same result as the cv::solvePNP by using the algorithm, but the unstable case still exist.

Answer 1

You can set to true the setting useExtrinsicGuess of solvePnP function; with this parameter, solvePnP uses the last computed pose in the first iteration of the solver inside solvePnP, see https://en.wikipedia.org/wiki/Levenberg–Marquardt_algorithm. For example:

void getPose(cv::Mat& t_vec, cv::Mat& r_vec)
{
    static cv::Mat raux, taux;
    static bool useExtrinsicGuess = false
    cv::Mat objPoints  (4, 3, CV_32FC1); // Your 3d points
    cv::Mat imagePoints(4, 2, CV_32FC1); // Your 2d points

    cv::solvePnP (objPoints, 
                  imagePoints, 
                  camera.getCameraMatrix(), // Camera matrix
                  camera.getDistorsion(),   // Camera distorsion
                  raux, taux, useExtrinsicGuess, CV_ITERATIVE);
    if (!useExtrinsicGuess) {
        useExtrinsicGuess = true;
    }
    raux.convertTo(r_vec, CV_32F);
    taux.convertTo(t_vec, CV_32F);
}

Also you could use more points, in different planes.

Answer 2

As has been already suggested, use findHomography, then decompose the result. See Zengyou Zhang's paper here. See, in particular, equations 2.18 for the concept, and 2.26 for the solution - the case you want is the one with one image only.