PnP 单目相机位姿估计（二）：solvePnP利用二维码求解相机世界坐标

---

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

---

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：输出的旋转向量