预备知识
图像坐标系：
理想的图像坐标系原点O1和真实的O0有一定的偏差，由此我们建立了等式（1）和（2），可以用矩阵形式（3）表示。
相机坐标系（C）和世界坐标系（W）：
通过相机与图像的投影关系，我们得到了等式（4）和等式（5），可以用矩阵形式（6）表示。
我们又知道相机坐标系和世界坐标的关系可以用等式（7）表示：
由等式（3），等式（6）和等式（7）我们可以推导出图像坐标系和世界坐标系的关系：
其中M1称为相机的内参矩阵，包含内参（fx，fy，u0，v0）。M2称为相机的外参矩阵，包含外参（R:旋转矩阵，T:平移矩阵）。
众所周知，相机镜头存在一些畸变，主要是径向畸变（下图dr），也包括切向畸变（下图dt）等。
上图右侧等式中，k1,k2,k3,k4,k5,k6为径向畸变，p1,p2为切向畸变。在OpenCV中我们使用 张正友相机标定法通过10幅不同角度的棋盘图像来标定相机获得相机内参和畸变系数。
R的第i行表示摄像机坐标系中的第i个坐标轴方向的单位向量在世界坐标系里的坐标；R的第i列表示世界坐标系中的第i个坐标轴方向的单位向量在摄像机坐标系里的坐标；T正好是世界坐标系的原点在摄像机坐标系的坐标，特别的，Tz就代表世界坐标系的原点在摄像机坐标系里的“深度”。
函数原型
1 cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]) → retval, rvec, tvec
参数解释
-
objectPoints：世界坐标系中的3D点坐标，单位mm
-
imagePoints：图像坐标系中点的坐标，单位像素
-
cameraMatrix：相机内参矩阵
-
distCoeffs：畸变系数
-
rvec：旋转矩阵
-
tvec：平移矩阵
-
useExtrinsicGuess：是否输出平移矩阵和旋转矩阵，默认为false
-
flags：SOLVEPNP _ITERATIVE、SOLVEPNP _P3P、SOLVEPNP _EPNP、SOLVEPNP _DLS、SOLVEPNP _UPNP
内参矩阵和畸变系数都是要通过标定得到的，这个不细讲，opencv官方提供了有标定例子。函数输出的是旋转矩阵rvec和tvec。
代码例子
我们的问题就是，人站在某点拍摄世界坐标系的原点Ow（下标w表示该坐标是在世界坐标系中的），拍摄出的原点正好落在图像中心，现在我通过某种方法（解PNP问题）计算出Ow在相机坐标系下的坐标为（下标c表示该坐标定义在相机坐标系内），求相机或者说是人位于世界坐标系的哪里。
1 improt cv2 as cv 2 import numpy as np 3 objPoints = np.array([[],[],[]]) 4 imgPoints = np.array([]) 5 cameraMatrix = np.array() 6 distCoeffs = np.array() 7 retval,rvec,tvec = cv.solvePnP(objPoints,imgPoints,cameraMatrix,distCoeffs)
应用
应用在robomaster比赛里，就是通过测量装甲实际宽高（单位mm），通过摄像机识别到装甲，然后得到装甲在图像中的坐标，进而利用solvePnP得到tvec，这正好是世界坐标系原点相对于摄像机坐标系的坐标(单位mm)，那么z就是距离咯，我们标定装甲的中心为世界坐标系原点，那么就直接得到了装甲中心相对于摄像机坐标系的坐标，通过角度计算就拿到偏移角度，再调试得到摄像机坐标系坐标原点到云台坐标系的偏移值，通过简单运算就得到装甲中心相对云台坐标系的坐标，我们就可以攻击它啦。
参考：
OpenCV相机标定和姿态更新：https://www.cnblogs.com/mikewolf2002/p/5746667.html
Opencv249和Opencv3.0以上的 SolvePnp函数详解（附带程序、算例）：https://blog.csdn.net/qq_30547073/article/details/78656795
OpenCV官方文档：https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
根据四个特征点估计相机姿态：http://www.cnblogs.com/singlex/p/pose_estimation_1.html