cv2.solvepnp 相机的位姿估计

预备知识

 

图像坐标系：

 

理想的图像坐标系原点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就代表世界坐标系的原点在摄像机坐标系里的“深度”。

 

函数原型

 

 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表示该坐标定义在相机坐标系内），求相机或者说是人位于世界坐标系的哪里。

 

 improt cv2 as cv
 import numpy as np
 objPoints = np.array([[],[],[]])
 imgPoints = np.array([])
 cameraMatrix = np.array()
 distCoeffs = np.array()
 retval,rvec,tvec = cv.solvePnP(objPoints,imgPoints,cameraMatrix,distCoeffs)

 

 

应用

 

应用在robomaster比赛里，就是通过测量装甲实际宽高（单位mm），通过摄像机识别到装甲，然后得到装甲在图像中的坐标，进而利用solvePnP得到rvec，这正好是世界坐标系原点相对于摄像机坐标系的坐标(单位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