眼在手上标定结果应用：像素坐标转机械臂世界坐标

像素坐标转世界坐标主要用到如下的公式：其中boar2camera矩阵可由通过拍摄的标定板图片直接求解，为相机内参矩阵
camera_matrix:
rows: 3
cols: 3
data: [428.3066849046146, 0, 675.2344606795484, 0, 431.0838735333736, 405.3373367752419, 0, 0, 1]

1、手眼标定结束后，得到了相机的手眼标定矩阵，之后即可获得相机在世界坐标系下的位置和姿态。

    def updateCameraRT(self):
        cal_file = 	  rospy.get_param('/camera_cal_file','/home/nvidia/bitzz_acupuncture_ros/src/bit_acupuncture/config/camera_eyeinhand_cal.yaml')
        R,T = load_camera_cal(cal_file)
        self.Matrix_cam2end=np.column_stack((R,T))#手眼标定结果
        self.Matrix_cam2end=np.row_stack((self.Matrix_cam2end,np.array([0,0,0,1])))
        RMat=Rotation.from_euler('xyz',[self.robot_pos.rx,self.robot_pos.ry,self.robot_pos.rz],degrees=True)
        Matrix_end2base = np.column_stack((RMat.as_matrix(),[self.robot_pos.x,self.robot_pos.y,self.robot_pos.z]))
        Matrix_end2base=np.row_stack((Matrix_end2base,np.array([0,0,0,1])))
        # print("机械臂末端到世界坐标的矩阵")
        # print(Matrix_end2base)
        Matrix_camera2Base = np.dot(Matrix_end2base,self.Matrix_cam2end)
        Matrix_base2Cam=np.linalg.inv(Matrix_camera2Base)
        # print("机械臂到相机矩阵")
        # print(Matrix_base2Cam)
        # 相机外参
        with self.RLock:
            self.R = cv2.Rodrigues(Matrix_base2Cam[:3,:3])[0]
            self.T = Matrix_base2Cam[:3, 3].reshape((3, 1))

2、获取到相机在世界坐标系下的位置和姿态后，通过以下代码即可获取目标的世界坐标，控制机械臂进行移动。
转换原理参考https://www.jianshu.com/p/4566a1281066

def camera_to_world(cam_mtx, r, t, img_points,scale=0):
    '''
    cam_mtx,相机内参
    r:相机外参旋转向量
    t:相机外参平移向量
    img_points:像素点集合
    scale：相机缩放比例，即相机坐标系下Zc值，通过距离传感器获得

    '''
    
    inv_k = np.asmatrix(cam_mtx).I
    r_mat = np.zeros((3, 3), dtype=np.float64)
    cv2.Rodrigues(r, r_mat)
    # invR * T
    inv_r = np.asmatrix(r_mat).I  # 3*3
    transPlaneToCam = np.dot(inv_r, np.asmatrix(t))  # 3*3 dot 3*1 = 3*1
    world_pt = []
    coords = np.zeros((3, 1), dtype=np.float64)
    for img_pt in img_points:
        coords[0][0] = img_pt[0][0]
        coords[1][0] = img_pt[0][1]
        coords[2][0] = 1.0
        # 将像素坐标转换为相机坐标系下的坐标
        worldPtCam = np.dot(inv_k, coords)  # 3*3 dot 3*1 = 3*1
        # print("相机坐标")
        # print(worldPtCam)
        # [x,y,1] * invR
        worldPtPlane = np.dot(inv_r, worldPtCam)  # 3*3 dot 3*1 = 3*1
        # zc
        # print("Scale0",scale)
        if scale==0:
            scale = transPlaneToCam[2][0] / worldPtPlane[2][0]
        # print("transPlaneToCam",transPlaneToCam)
        # print("worldPtPlane",)

        # print("Scale",scale)
        # scale=128
        # zc * [x,y,1] * invR
        scale_worldPtPlane = np.multiply(scale, worldPtPlane)
        # [X,Y,Z]=zc*[x,y,1]*invR - invR*T
        worldPtPlaneReproject = np.asmatrix(scale_worldPtPlane) - np.asmatrix(transPlaneToCam)  # 3*1 dot 1*3 = 3*3
        pt = np.zeros((3, 1), dtype=np.float64)
        pt[0][0] = worldPtPlaneReproject[0][0]
        pt[1][0] = worldPtPlaneReproject[1][0]
        pt[2][0] = 0
        world_pt.append(pt.T.tolist())
    return world_pt