cv2.solvepnp 相机的位姿估计
2024-09-26 23:15:15
预备知识
图像坐标系:
理想的图像坐标系原点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
最新文章
- Dynamics AX 2012 R2 RemoteApp导出项目报错
- sql语句感想
- 转载:Python中的new style class机制实现
- Android Activity之间通信
- 手机自动化培训:Appium介绍
- django-xadmin ModelAdmin中定义object_list_template无效的问题
- ListBox多列显示,原来比较简单
- 编程菜鸟的日记-初学尝试编程-C++ Primer Plus 第4章编程练习1
- min-max 容斥
- 电脑cpu100%的原因
- Spring框架的Bean管理的配置文件方式
- iOS 数组和字典排序
- mysql关于“异步复制”“同步复制”“半同步复制”“无损复制”的概念与区别
- linux查看及设置别名,权限,生成ssh秘钥
- linux软硬链接
- setitemdata 32位 or 64位
- 《gdb调试之基础篇》
- 简单使用postman
- python分布式爬虫--房天下
- C# 流