solvePnp

贴一个完整版的对solvePnp函数的调用代码

#include<iostream>
#include<opencv2\opencv.hpp>
#include<vector>

using namespace std;
using namespace cv;



int main()
{
    float threeDim[5][3] = { {-11,153,857}, {-10,104,865}, {171,62,708}, {172,50,714}, {-10,28,880} };
    float twoDim[5][2] = { {73,169}, {226,173}, {313,850}, {359,850}, {461,173} };

    vector<Point3f>outDim;
    vector<Point2f>inDim;
    vector<float> distCoeff(0);

    for (int i = 0; i < 5;i++)
    {
        outDim.push_back(Point3f(threeDim[i][0], threeDim[i][1], threeDim[i][2]));
        inDim.push_back(Point2f(threeDim[i][0], threeDim[i][1]));
    }

    Mat cameraMatrix(3,3,CV_32F);
    float tempMatrix[3][3] = { { 2697.6,0 ,597.4 }, { 0, 2682,515.6 }, { 0, 0 ,1} };
    for (int i = 0; i < 3;i++)
    {
        for (int j = 0; j < 3;j++)
        {
            cameraMatrix.at<float>(i, j) = tempMatrix[i][j];
        }
    }

    Mat rvec, tvec;
    solvePnP(outDim, inDim, cameraMatrix, distCoeff, rvec, tvec);

    Rodrigues(rvec, rvec);

    cout << rvec<< endl;
    cout << tvec << endl;

    return 0;

}