（二）solvepnp 单目三维位姿估计--------利用二维码求解相机世界坐标

本文是通过相机拍摄二维码得到世界坐标和图像坐标点对，求解相机的姿态或目标（二维码）的姿态。

文章转自：https://blog.csdn.net/cocoaqin/article/details/77848588

博主代码：https://github.com/xukeqin/PnP

实现平台：

IDE：visual studio 2013 
使用库：Eigen opencv2.4.9 
文档版本：1.1

（一）solvepnp 单目三维位姿估计--------理论

matlab相机标定获取内参

旋转矩阵到旋转角之间的换算

以下为正文内容：

---

前言
原理简介
输入参数准备
1 objectPoints特征点世界坐标
2 imagePoints特征点在摄像头下的像素点坐标
3cameraMatrixdistCoeffs内参矩阵和畸变矩阵
相机世界坐标的求解
1求世界坐标中的点在相机坐标系下的坐标
2求相机在世界坐标中的坐标
3效果求解相机在世界坐标系下的坐标点
4求解代码

---

1.前言

　　很多时候，当我们利用单目相机获取图像时，往往只能获取图像中特征物体的像素坐标。而在空间三维点计算时，我们也第一时间会想到深度相机（Kinect、Realsense等）、双目相机等。 
　　但是在实际中，我们亦可以通过单目相机来求得空间三维点，但这个往往有一个条件，已知特征物体的三个以上特征点参数。

在本章中，我们将使用二维码作为标志物，通过二维码四个角点，通过P4P求解相机的位姿及空间坐标。 
特别说明：在本章以及未来的几篇博文中都将以右手坐标系为基准，Z轴垂直于二维码平面往里。

  
　　 
　　　　　　　　　　　　　　　二维码参数：长140mm，宽140mm

在求解时不一定要使用二维码，也可以用其它标志物代替，使用二维码的好处是，通过识别二维码内部的信息可以更精准的匹配以及识别，同时也能更容易的确定四个角点的顺序。 
　　

2.原理简介

bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )

　　objectPoints：特征点的世界坐标，坐标值需为float型，不能为double型，可以为mat类型，也可以直接输入vector 
　　imagePoints：特征点在图像中的像素坐标，可以输入mat类型，也可以直接输入vector，注意输入点的顺序要与前面的特征点的世界坐标一一对应 
　　cameraMatrix：相机内参矩阵 
　　distCoeffs：相机的畸变参数【Mat_(5, 1)】 
　　rvec：输出的旋转向量 
　　tvec：输出的平移矩阵 
　　最后的输入参数有三个可选项： 
　　CV_ITERATIVE，默认值，它通过迭代求出重投影误差最小的解作为问题的最优解。 
　　CV_P3P则是使用非常经典的Gao的P3P问题求解算法。 
　　CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。 
　　详见 solvepnp三维位姿估算

　　因此我们只要获得 特征点的世界坐标（三维坐标）、2D坐标（像素坐标）、相机内参矩阵、相机畸变参数矩阵 以上四个参数即可以解得相机与标志物之间的外参（R、T），并以此求得相机的世界坐标（以标志物为世界坐标平面，且原点为标志物已知某一点）。

3.输入参数准备

       在上一节中我们已知solvePnP需要输入四个参数，在这一节中，将介绍如何获得这四个参数。

3.1 objectPoints特征点世界坐标

       以特征点所在平面为世界坐标XY平面，并在该平面中确定世界坐标的原点，以我设计的二维码为例，我设定二维码的中心为世界坐标的原点，并按照顺时针方向逐个输入四个角点的世界坐标。

注意，输入一定要按照顺序输入

    m_markerCorners3d.push_back(cv::Point3f(-70.0f, -70.0f, 0));
    m_markerCorners3d.push_back(cv::Point3f(+70.0f, -70.0f, 0));    
    m_markerCorners3d.push_back(cv::Point3f(+70.0f, +70.0f, 0));
    m_markerCorners3d.push_back(cv::Point3f(-70.0f, +70.0f, 0));

3.2 imagePoints特征点在摄像头下的像素点坐标

       在这儿将获得四个特征点对应2D的像素点坐标，而这个过程你可以人为的从图像中逐个点获得，也可以通过二维码检测程序检测出，详见 PnP 单目相机位姿估计（三）：二维码四个角点的检测

注意，这儿检测到的四个像素点的输入顺序要和输入的世界坐标的顺序相同

3.3cameraMatrix，distCoeffs内参矩阵和畸变矩阵

      我们这儿采用matlab的标定工具标定，（参考文章：matlab相机标定获取内参）。并形成以下矩阵输入solvePnP

camMatrix = (Mat_(3, 3) << 598.29493, 0, 304.76898, 0, 597.56086, 233.34673, 0, 0, 1);
distCoeff = (Mat_(5, 1) << -0.53572,1.35993,-0.00244,0.00620,0.00000);

4.相机世界坐标的求解

       在上一节中，我们准备好了四个参数，把这四个参数输入solvePnP中，即可获得求得的R旋转矩阵、T平移矩阵，拿到这两个矩阵的值，我们就可以获得许多我们想要的空间信息啦~

已知 Pc=R*Po+T 
定义Pc为相机坐标系的点值，Po为世界坐标系的点值，R、T为世界坐标系和相机坐标系的相对外参。

而我们利用solvePnP解得的R和T正是相机坐标系和世界坐标系的相对外参。

注意，solvePnP返回的raux是旋转向量，可通过罗德里格斯变换成旋转矩阵R。

4.1求世界坐标中的点在相机坐标系下的坐标

        当Po=[0;0;0;]时，Pc=T即世界坐标原点（二维码的中心）在相机坐标系下的坐标就为T 
同样的道理，当我们已知一点的世界坐标Po，我们就可以求得Pc

4.2求相机在世界坐标中的坐标

       因为相机在相机坐标中相当于Pc=[0;0;0]，故Po=-R’*T 即可解得相机在世界坐标系下的坐标 
其中R’为R的逆或者转置矩阵（R是正交矩阵，R的逆等于R的转置）

4.3效果（求解相机在世界坐标系下的坐标点）：

 

4.4求解代码

        Marker& m = detectedMarkers[i];
        cv::Mat Rvec;
        cv::Mat_ Tvec;
        cv::Mat raux, taux;

        cv::solvePnP(m_markerCorners3d, m.points, camMatrix, distCoeff, raux, taux, false, CV_P3P);

        raux.convertTo(Rvec, CV_32F);    //旋转向量
        taux.convertTo(Tvec, CV_32F);   //平移向量

        cv::Mat_ rotMat(3, 3);
        cv::Rodrigues(Rvec, rotMat);  //由于solvePnP返回的是旋转向量，故用罗德里格斯变换变成旋转矩阵

        //格式转换
        Eigen::Matrix R_n;
        Eigen::Matrix T_n;
        cv2eigen(rotMat, R_n);
        cv2eigen(Tvec, T_n);
        Eigen::Vector3f P_oc;

        P_oc = -R_n.inverse()*T_n;