上篇文章讲到了可以用小孔成像原理得到图像中某点相对于摄像头的转角，可以用这个来对所需要转角的测量。

但是这个方法有很大的局限性，它只能得到相对于摄像头中心的转角。而在实际应用中摄像头肯定不会在云台的轴上，每次以相对摄像头中心的转角来指挥云台运动就会有误差。

如果摄像头是放在枪管上的，那其实这点误差没有大碍。因为一般会用闭环控制算法来控制云台跟踪目标，这样如果目标静止不动则稳态时摄像头中心点正对目标，枪管则稍偏下，可以根据实际情况调整摄像头所瞄准的中心位置使枪管对准目标。

如果摄像头是放在车上的，那这种测转角的方法就不能用了。上篇文章的方法最大的缺陷在于它缺失了深度信息，从而无法变换坐标系。这里讲另一种求解目标位置的算法，通过求解 Perspective N-Point (PNP) Problem 来得到两者间的坐标。

与上文的方法对比这个方法可以得到三维坐标，缺点在于需要4个点才能计算而上文算法只要1个点即可，所以目标距离太远适合用单点解算角度，目标距离合适的情况下使用PnP解算角度。

有关算法原理方面的讲解和一些使用案例可以参看以下几篇文章：

「Head Pose Estimation using OpenCV and Dlib」https://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/

「Real Time pose estimation of a textured object」 https://docs.opencv.org/3.4.6/dc/d2c/tutorial_real_time_pose.html

「《视觉SLAM十四讲》学习笔记-3D->2D: PnP问题的由来」 https://blog.csdn.net/luohuiwu/article/details/80722542

要用这个算法首先要标定摄像头，得到相机的内参矩阵和畸变参数。之后需要测量物体的尺寸，得到物体在世界坐标系中的坐标。

这个坐标是自定义的，由于装甲板可以看成是一个平板，其z轴坐标可以设为0，x轴和y轴的坐标分别可以设置为正负二分之一的长和宽，这样装甲板的中心就为原点了。OpenCV中的坐标系可参看下图： （图片来源：「OpenCV文档」https://docs.opencv.org/3.4.6/d9/d0c/group__calib3d.html）

在检测程序检测到装甲板后就可以得到装甲板在图像中的坐标，之后通过solvePnP函数即可得到平移向量和旋转向量。

旋转向量可以不用管，因为我们不需要得到姿态信息而只需要得到位置信息。求得的平移向量就是以当前摄像头中心为原点时物体原点所在的位置。

因为我们的目的是让枪管瞄准装甲板的中央，而得到的坐标是以摄像头中心为原点的，要得到正确的转角需要对这个坐标进行平移，将原点平移到云台的轴上。

一般而言x坐标可以不用动，因为摄像头一般放在枪管的上方这样x方向就没有偏差。y方向和z方向需要修正。修正后即可通过反三角函数得到角度值。

为啥用二维码不用装甲板呢，因为不做比赛了没有装甲板可以用。用二维码可以直接使用现成的库完成识别，而不需要自己写识别函数了。

二维码识别我是参看「Barcode and QR code Scanner using ZBar and OpenCV」https://www.learnopencv.com/barcode-and-qr-code-scanner-using-zbar-and-opencv/ 使用ZBar库来做的，如果用较新版的OpenCV也可以使用OpenCV自带的二维码识别函数。

参考的代码：https://github.com/spmallick/learnopencv/tree/master/barcode-QRcodeScanner

我只是在其基础上增加了读取摄像头内参和用solvePnP得到二维码的位置的代码。

其中解算位置的部分只需要以下几行即可：