四.3D-2D：PnP问题求解 线性法

调用solvePnP函数

bool cv::solvePnP(objectPoints,imagePoints,cameraMatrix,distCoeffs,

OutputArray r, OutputArray t,

bool useExtrinsicGuess = false,int flags = SOLVEPNP_ITERATIVE )

缺点：P3P只利用了3个点信息（后续提出EPNP、UPNP利用更多信息）

           如果3D或2D点受到噪声影响，或者误匹配，算法失效。

主代码：

#include <iostream>

#include <vector>

#include <opencv2/opencv.hpp>

using namespace std;

using namespace cv;

 void find_feature_matches(

const Mat& img_1, const Mat& img_2,

std::vector<KeyPoint>& keypoints_1,

std::vector<KeyPoint>& keypoints_2,

std::vector< DMatch >& matches);

 Point2d pixel2cam(const Point2d& p, const Mat& K);

 int main()

{

Mat img1 = imread("1.png");

Mat img2 = imread("2.png");

Mat d1 = imread("1_depth.png");       // 深度图为16位无符号数，单通道图像

//1. 找到特征点和匹配关系

vector<KeyPoint> keypoint1, keypoint2;

vector<DMatch> matches;

find_feature_matches(img1,img2,keypoint1,keypoint2,matches);//自定义函数找特征匹配

        Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

//2. 第一张图像给出3D坐标，第二张给像素2d坐标

vector<Point3f> pts_3d;

vector<Point2f> pts_2d;

for (DMatch m : matches)

{

//3. 3D坐标由第一张的像素坐标转为相机坐标，再乘上深度，为世界坐标

       ushort d = d1.ptr<unsigned short> (int(keypoint1[m.queryIdx].pt.y))[int (keypoint1[m.queryIdx].pt.x)];

//d1是深度图，Mat类型的ptr是指针，获得像素点深度，首先得到行地址，之后列指针  (.pt.y)[.pt.x]

d = d / 1000;

Point2d p1 = pixel2cam(keypoint1[m.queryIdx].pt, K);//自定义函数像素坐标转相机坐标

/***************************************************************************************************************/

Point2d pixel2cam ( const Point2d& p, const Mat& K )

{

    return Point2d

           (

               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),

               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )

           );

}

/***************************************************************************************************************/

Point3f p2(p1.x*d, p1.y*d, d);

pts_3d.push_back(p2);

pts_2d.push_back(keypoint2[m.trainIdx].pt);

cout << "3d-2d pairs" << pts_3d.size() << endl;

//4. 调用函数p3p，求解r，t

Mat R,r, t;

solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false, SOLVEPNP_EPNP);

              Rodrigues(r, R); //solvepnp得到的是旋转向量r，需要通过罗德里格斯公式换为旋转矩阵R。即李代数向李群的转换。

 

}

}