cv2.solvepnp 相机的位姿估计

预备知识
 
图像坐标系: cv2.solvepnp 相机的位姿估计_第1张图片

 

 

 
理想的图像坐标系原点O1和真实的O0有一定的偏差,由此我们建立了等式(1)和(2),可以用矩阵形式(3)表示。
相机坐标系(C)和世界坐标系(W):

cv2.solvepnp 相机的位姿估计_第2张图片

 

通过相机与图像的投影关系,我们得到了等式(4)和等式(5),可以用矩阵形式(6)表示。
我们又知道相机坐标系和世界坐标的关系可以用等式(7)表示:
cv2.solvepnp 相机的位姿估计_第3张图片

 

由等式(3),等式(6)和等式(7)我们可以推导出图像坐标系和世界坐标系的关系:
cv2.solvepnp 相机的位姿估计_第4张图片

 

其中M1称为相机的内参矩阵,包含内参(fx,fy,u0,v0)。M2称为相机的外参矩阵,包含外参(R:旋转矩阵,T:平移矩阵)。
众所周知,相机镜头存在一些畸变,主要是径向畸变(下图dr),也包括切向畸变(下图dt)等。

cv2.solvepnp 相机的位姿估计_第5张图片

 

上图右侧等式中,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
 

转载于:https://www.cnblogs.com/aoru45/p/9781540.html

你可能感兴趣的:(cv2.solvepnp 相机的位姿估计)