opencv SolvePnPRansac使用

参考
https://blog.csdn.net/xuelangwin/article/details/80847337

1.传归一化坐标使用

下面代码验证将坐标点转到归一化平面使用SolvePnPRansac解出来的位置是否正确。

#include
 #include 
#include
#include
#include
#include 

using namespace cv;
using namespace std;
using namespace Eigen;

void generate3DPointCloud(std::vector& points)
{
    cv::Point3f pmin = cv::Point3f(-1, -1, 5);
    cv::Point3f pmax = cv::Point3f(1, 1, 10);
    cv::RNG rng = cv::theRNG(); // fix the seed to use "fixed" input 3D points

    for (size_t i = 0; i < points.size(); i++)
    {
        float _x = rng.uniform(pmin.x, pmax.x);
        float _y = rng.uniform(pmin.y, pmax.y);
        float _z = rng.uniform(pmin.z, pmax.z);
        points[i] = cv::Point3f(_x, _y, _z);
    }
}

void generateCameraMatrix(cv::Mat& cameraMatrix, cv::RNG& rng)
{
    const double fcMinVal = 1e-3;
    const double fcMaxVal = 100;
    cameraMatrix.create(3, 3, CV_64FC1);
    cameraMatrix.setTo(cv::Scalar(0));
    cameraMatrix.at(0, 0) = rng.uniform(fcMinVal, fcMaxVal);
    cameraMatrix.at(1, 1) = rng.uniform(fcMinVal, fcMaxVal);
    cameraMatrix.at(0, 2) = rng.uniform(fcMinVal, fcMaxVal);
    cameraMatrix.at(1, 2) = rng.uniform(fcMinVal, fcMaxVal);
    cameraMatrix.at(2, 2) = 1;
}

void generateDistCoeffs(cv::Mat& distCoeffs, cv::RNG& rng)
{
    distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
    for (int i = 0; i < 3; i++)
        distCoeffs.at(i, 0) = rng.uniform(0.0, 1.0e-6);
}

void generatePose(cv::Mat& rvec, cv::Mat& tvec, cv::RNG& rng)
{
    const double minVal = 1.0e-3;
    const double maxVal = 1.0;
    rvec.create(3, 1, CV_64FC1);
    tvec.create(3, 1, CV_64FC1);
    for (int i = 0; i < 3; i++)
    {
        rvec.at(i, 0) = rng.uniform(minVal, maxVal);
        tvec.at(i, 0) = rng.uniform(minVal, maxVal / 10);
    }
}

int main()
{
    std::vector points;
    points.resize(500);
    generate3DPointCloud(points);
    cv::Mat trueRvec, trueTvec;
    //cv::Mat intrinsics, distCoeffs;
    cv::RNG rng = cv::RNG();
    //generateCameraMatrix(intrinsics, rng);
    //generateDistCoeffs(distCoeffs, rng);
    double fx = 200,fy=200,cx=500, cy =500;
    cv::Mat intrinsics = (Mat_(3, 3) <<
                   fx, 0, cx,
                   0, fy, cy,
                   0, 0, 1);
    cv::Mat K = (cv::Mat_(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0);
    cv::Mat distCoeffs;
    //cv::Mat distCoeffs =(cv::Mat_(4, 1) << 0.0, 0, 0, 0);
    generatePose(trueRvec, trueTvec, rng);
    std::vector opoints;
    //opoints = std::vector(points.begin(), points.begin() + 4);
    opoints = std::vector(points.begin(), points.end());
    std::vector projectedPoints;
    projectedPoints.resize(opoints.size());
    projectPoints(cv::Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
     std::vector projectedPointsNorm;
    for (size_t i = 0; i < projectedPoints.size(); i++)
    {
        cv::Point2f p_norm;
        p_norm.x=(projectedPoints[i].x-cx)/fx;
        p_norm.y=(projectedPoints[i].y-cy)/fy;
        projectedPointsNorm.push_back(p_norm);
    }
    std::cout << "intrinsics: " << intrinsics << std::endl;
    std::cout << "distcoeffs: " << distCoeffs << std::endl;
    //std::cout << "oPoint: " << opoints << std::endl;
    //std::cout << "projectedPoints: " << projectedPoints << std::endl;
    cv::Mat rvec, tvec;
    solvePnP(opoints, projectedPointsNorm, K, distCoeffs, rvec, tvec, false,cv::SOLVEPNP_EPNP);
    std::cout << "trueRvec: " << trueRvec << std::endl;
    std::cout << "trueTvec: " << trueTvec << std::endl;
    std::cout <<"SOLVEPNP_EPNP result_rvec"<< rvec <<"---" <

2.传uv参数使用

#include
#include 
#include
#include 
using namespace cv;
using namespace std;
void generate3DPointCloud(std::vector& points)
{
    cv::Point3d pmin = cv::Point3d(-1, -1, 5);
    cv::Point3d pmax = cv::Point3d(1, 1, 10);
    cv::RNG rng = cv::theRNG(); // fix the seed to use "fixed" input 3D points
    
    for (size_t i = 0; i < points.size(); i++)
    {
        double _x = rng.uniform(pmin.x, pmax.x);
        double _y = rng.uniform(pmin.y, pmax.y);
        double _z=rng.uniform(pmin.z, pmax.z);
        points[i]=(cv::Point3d(_x, _y, _z));
    }
}
int main( int argc, char** argv )
{
    //1.1 get para intrinsic
    double f = 200, w = 1000, h = 1000;
    cv::Mat K = (Mat_(3, 3) <<
                 f, 0, w/2,
                 0, f, h/2,
                 0, 0, 1);
    std::vector vp3d;
    vp3d.resize(500);
    generate3DPointCloud(vp3d);
    vector prj2d;
    Mat true_R=(cv::Mat_(3, 3) <<
                1,0,0,
                0,1,0,
                0,0,1);
    Mat true_rvec;
    cv::Mat true_t = (cv::Mat_(3, 1) << 1,1,0);
    Rodrigues(true_R,true_rvec);
    cv::projectPoints(vp3d,true_rvec,true_t,K,Mat(),prj2d);
    for(int i=0;i