// P3P has up to 4 solutions, and the solutions are sorted by reprojection errors(lowest to highest).

    int solveP3P (

        InputArray     objectPoints,     // object points, 3x3 1-channel or 1x3/3x1 3-channel. vector<Point3f> can be also passed

        InputArray      imagePoints,     // corresponding image points, 3x2 1-channel or 1x3/3x1 2-channel. vector<Point2f> can be also passed

        InputArray     cameraMatrix,     // camera intrinsic matrix

        InputArray       distCoeffs,     // distortion coefficients.If NULL/empty, the zero distortion coefficients are assumed.

        OutputArrayOfArrays   rvecs,     // rotation vectors

        OutputArrayOfArrays   tvecs,     // translation vectors

        int                   flags      // solving method

    ); 　　

bool solvePnP(InputArray opoints, InputArray ipoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags)

{

    CV_INSTRUMENT_REGION();

    vector<Mat> rvecs, tvecs;

    int solutions = solvePnPGeneric(opoints, ipoints, cameraMatrix, distCoeffs, rvecs, tvecs, useExtrinsicGuess, (SolvePnPMethod)flags, rvec, tvec);

    if (solutions > 0)

    {

        int rdepth = rvec.empty() ? CV_64F : rvec.depth();

        int tdepth = tvec.empty() ? CV_64F : tvec.depth();

        rvecs[0].convertTo(rvec, rdepth);

        tvecs[0].convertTo(tvec, tdepth);

    }

    return solutions > 0;

}　　

    bool solvePnPGeneric (

        InputArray         objectPoints,    // object points, Nx3 1-channel or 1xN/Nx1 3-channel, N is the number of points. vector<Point3d> can be also passed

        InputArray          imagePoints,    // corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, N is the number of points. vector<Point2d> can be also passed

        InputArray         cameraMatrix,    // camera intrinsic matrix

        InputArray           distCoeffs,    // distortion coefficients

        OutputArrayOfArrays        rvec,    // rotation vector

        OutputArrayOfArrays        tvec,    // translation vector

        bool  useExtrinsicGuess = false,    // used for SOLVEPNP_ITERATIVE. If true, use the provided rvec and tvec as initial approximations, and further optimize them.

        SolvePnPMethod  flags = SOLVEPNP_ITERATIVE, // solving method

        InputArray               rvec = noArray(),  // initial rotation vector when using SOLVEPNP_ITERATIVE and useExtrinsicGuess is set to true

        InputArray               tvec = noArray(),  // initial translation vector when using SOLVEPNP_ITERATIVE and useExtrinsicGuess is set to true

        OutputArray reprojectionError = noArray()   // optional vector of reprojection error, that is the RMS error

    ); 　　

    bool solvePnPRansac (

        InputArray         objectPoints,    // object points, Nx3 1-channel or 1xN/Nx1 3-channel, N is the number of points. vector<Point3d> can be also passed

        InputArray          imagePoints,    // corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, N is the number of points. vector<Point2d> can be also passed

        InputArray         cameraMatrix,    // camera intrinsic matrix

        InputArray           distCoeffs,    // distortion coefficients

        OutputArray                rvec,    // rotation vector

        OutputArray                tvec,    // translation vector

        bool  useExtrinsicGuess = false,    // used for SOLVEPNP_ITERATIVE. If true, use the provided rvec and tvec as initial approximations, and further optimize them.

        int       iterationsCount = 100,    // number of iterations

        float   reprojectionError = 8.0,    // inlier threshold value. It is the maximum allowed distance between the observed and computed point projections to consider it an inlier

        double        confidence = 0.99,    // the probability that the algorithm produces a useful result

        OutputArray inliers = noArray(),    // output vector that contains indices of inliers in objectPoints and imagePoints

        int  flags = SOLVEPNP_ITERATIVE     // solving method

    );　　

  void solvePnPRefineLM (

      InputArray      objectPoints,  // object points, Nx3 1-channel or 1xN/Nx1 3-channel, N is the number of points

      InputArray       imagePoints,  // corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel

      InputArray      cameraMatrix,  // camera intrinsic matrix

      InputArray        distCoeffs,  // distortion coefficients

      InputOutputArray      rvec,      // input/output rotation vector

      InputOutputArray      tvec,      // input/output translation vector

      TermCriteria  criteria = TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON) // Criteria when to stop the LM iterative algorithm

); 　　

#include "opencv2/imgproc.hpp"

#include "opencv2/highgui.hpp"

#include "opencv2/calib3d.hpp"

using namespace std;

using namespace cv;

Size kPatternSize = Size(9, 6);

float kSquareSize = 0.025;

// camera intrinsic parameters and distortion coefficient

const Mat cameraMatrix = (Mat_<double>(3, 3) << 5.3591573396163199e+02, 0.0, 3.4228315473308373e+02,

                                                0.0, 5.3591573396163199e+02, 2.3557082909788173e+02,

                                                0.0, 0.0, 1.0);

const Mat distCoeffs = (Mat_<double>(5, 1) << -2.6637260909660682e-01, -3.8588898922304653e-02, 1.7831947042852964e-03,

                                              -2.8122100441115472e-04, 2.3839153080878486e-01);

int main()

{

    // 1) read image

    Mat src = imread("left07.jpg");

    if (src.empty())

        return -1;

    // prepare for subpixel corner

    Mat src_gray;

    cvtColor(src, src_gray, COLOR_BGR2GRAY);

    // 2) find chessboard corners and subpixel refining

    vector<Point2f> corners;

    bool patternfound = findChessboardCorners(src, kPatternSize, corners);

    if (patternfound) {

        cornerSubPix(src_gray, corners, Size(11, 11), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.1));

    }

    else {

        return -1;

    }

    // 3) object coordinates

    vector<Point3f> objectPoints;

    for (int i = 0; i < kPatternSize.height; i++)

    {

        for (int j = 0; j < kPatternSize.width; j++)

        {

            objectPoints.push_back(Point3f(float(j * kSquareSize), float(i * kSquareSize), 0));

        }

    }

    // 4) Rotation and Translation vectors

    Mat rvec, tvec;

    solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec);

    // 5) project estimated pose on the image

    drawFrameAxes(src, cameraMatrix, distCoeffs, rvec, tvec, 2*kSquareSize);

    imshow("Pose estimation", src);

    waitKey();

}　　

    class PnPProblem

    {

    public:

        explicit PnPProblem(const double param[]);  // custom constructor

        virtual ~PnPProblem();

        cv::Point2f backproject3DPoint(const cv::Point3f& point3d);

        void estimatePoseRANSAC(const std::vector<cv::Point3f>& list_points3d, const std::vector<cv::Point2f>& list_points2d,

                                int flags, cv::Mat& inliers, int iterationsCount, float reprojectionError, double confidence);

        // ...

    }

    // Custom constructor given the intrinsic camera parameters

    PnPProblem::PnPProblem(const double params[])

    {

        // intrinsic camera parameters

        _A_matrix = cv::Mat::zeros(3, 3, CV_64FC1);

        _A_matrix.at<double>(0, 0) = params[0];       //  [ fx   0  cx ]

        _A_matrix.at<double>(1, 1) = params[1];       //  [  0  fy  cy ]

        _A_matrix.at<double>(0, 2) = params[2];       //  [  0   0   1 ]

        _A_matrix.at<double>(1, 2) = params[3];

        _A_matrix.at<double>(2, 2) = 1;

        // rotation matrix, translation matrix, rotation-translation matrix

        _R_matrix = cv::Mat::zeros(3, 3, CV_64FC1);

        _t_matrix = cv::Mat::zeros(3, 1, CV_64FC1);

        _P_matrix = cv::Mat::zeros(3, 4, CV_64FC1);

    }

    // Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use

    void PnPProblem::estimatePoseRANSAC (

        const std::vector<Point3f>& list_points3d,        // list with model 3D coordinates

        const std::vector<Point2f>& list_points2d,        // list with scene 2D coordinates

        int flags, Mat& inliers, int iterationsCount,     // PnP method; inliers container

        float reprojectionError, float confidence)        // RANSAC parameters

    {

        // distortion coefficients, rotation vector and translation vector

        Mat distCoeffs = Mat::zeros(4, 1, CV_64FC1);

        Mat rvec = Mat::zeros(3, 1, CV_64FC1);

        Mat tvec = Mat::zeros(3, 1, CV_64FC1);

        // no initial approximations

        bool useExtrinsicGuess = false;           

        // PnP + RANSAC

        solvePnPRansac(list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec, useExtrinsicGuess, iterationsCount, reprojectionError, confidence, inliers, flags);

        // converts Rotation Vector to Matrix

        Rodrigues(rvec, _R_matrix);

        _t_matrix = tvec;                            // set translation matrix

        this->set_P_matrix(_R_matrix, _t_matrix);    // set rotation-translation matrix

    }

    // Backproject a 3D point to 2D using the estimated pose parameters

    cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f& point3d)

    {

        // 3D point vector [x y z 1]'

        cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);

        point3d_vec.at<double>(0) = point3d.x;

        point3d_vec.at<double>(1) = point3d.y;

        point3d_vec.at<double>(2) = point3d.z;

        point3d_vec.at<double>(3) = 1;

        // 2D point vector [u v 1]'

        cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);

        point2d_vec = _A_matrix * _P_matrix * point3d_vec;

        // Normalization of [u v]'

        cv::Point2f point2d;

        point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2));

        point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2));

        return point2d;

    }

// Intrinsic camera parameters: UVC WEBCAM

double f = 55;                     // focal length in mm

double sx = 22.3, sy = 14.9;       // sensor size

double width = 640, height = 480;  // image size

double params_WEBCAM[] = { width * f / sx,   // fx

                           height * f / sy,  // fy

                           width / 2,      // cx

                           height / 2 };   // cy

// instantiate PnPProblem class

PnPProblem pnp_detection(params_WEBCAM);

// RANSAC parameters

int iterCount = 500;        // number of Ransac iterations.

float reprojectionError = 2.0;    // maximum allowed distance to consider it an inlier.

float confidence = 0.95;          // RANSAC successful confidence.

// OpenCV requires solvePnPRANSAC to minimally have 4 set of points

if (good_matches.size() >= 4)

{

    // -- Step 3: Estimate the pose using RANSAC approach

    pnp_detection.estimatePoseRANSAC(list_points3d_model_match, list_points2d_scene_match,

        pnpMethod, inliers_idx, iterCount, reprojectionError, confidence);

    // ... ..

}

// ... ...　

float fp = 5;

vector<Point2f> pose2d;

pose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(0, 0, 0))); // axis center

pose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(fp, 0, 0))); // axis x

pose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(0, fp, 0))); // axis y

pose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(0, 0, fp))); // axis z

draw3DCoordinateAxes(frame_vis, pose2d); // draw axes

// ... ...