大家好，我是小鱼。
今天分享几行祖传代码，是小鱼一两年前写的了，当时就是用来将yolo识别结果的位姿转换成相机坐标系下的位姿，最终让机械臂抓取。
代码很简单，就一个函数，输入像素坐标xy和深度z即可求出对应的空间坐标，计算过程中还需要相机的内参和畸变参数，这个在相机标定时即可获取。
import cv2
import numpy as np
K = [[602.7175003324863, 0, 351.305582038406],
[0, 601.6330312976042, 240.0929104708551],
[0, 0, 1]]
D = [0.06712174262966401, -0.2636999208734844, 0.006484443443073637, 0.01111161327049835, 0]
MK = np.array(K, dtype=np.float)
MD = np.array(D, dtype=np.float)
def covert(point=[[0.0, 0.0]], z=1):
point = np.array(point, dtype=np.float)
pts_uv = cv2.undistortPoints(point, MK, MD) * z
return pts_uv[0][0]
print(covert([0,0],1))
为什么要从像素坐标转换成三维的相机坐标系呢？
一般我们使用yolo等识别出物体在图像中的像素位置，像素位置并不能用于机械臂抓取或者3D的位姿计算，所以我们还需要将其转换成相机坐标系下的坐标使用~
重点函数：cv2.undistortPoints
需要注意的是，在opencv4.1版本中好像找不到该函数，后来小鱼安装了4.2版本的opencv-python库找到了该函数。
影响最终结果的主要是内参和畸变D，所以发现最终结果误差比较大，可以从这两个方面下手。
原文链接：
