巧用 OpenCV solvePnP() 函数完成由图像坐标系到机器人坐标系的转换（二维坐标系之间的转换）

工程中巧用（借用）OpenCV solvePnP()函数完成由图像坐标系到机器人坐标系的转换（二维坐标系之间的转换）

---

---

前言

在使用长广溪机器人画画时，由图像处理得到素描图，将素描图截断为若干段坐标簇，我需要将接受到的图像坐标系下的点簇转换为机器人坐标系下的点，即完成由图像坐标系到机器人坐标系之间的转换。

---

一、分析需求

图像坐标系的点是二维坐标点，我想转换为机器人坐标系下的三维点，因为绘画任务仅在同一平面执行，因此可以简化为二维坐标之间的转换。

二、使用方法

1.solvePnP()函数说明

官网对于solvePnP()函数的说明如下：

cv.solvePnP( objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]] ) ->	retval, rvec, tvec

输入参数：

objectPoints为物体坐标系下物体的三维坐标，可以是N3的一通道向量，或者是1N/N*1的三通道向量，N是点的个数

imagePoints为对应的图像坐标系下的二维坐标，可以是N2的一通道向量，也可以是1N/N*1的二通道向量，N是点的个数

cameraMatrix为相机的内参矩阵

distCoeffs为相机的畸变系数

输出参数：

rvec为旋转向量

tvec为平移向量

输出的旋转向量和平移向量可将三维坐标系的物体点坐标转换到图像坐标系下的像素坐标。

2.solvePnP()使用示例

项目中坐标转换部分的代码如下：

import cv2.cv2 as cv
import numpy as np


class Trans():
    def __init__(self):
        self.image_points_3D = np.array([
            (0, 0, 0),
            (0, 512, 0),
            (512, 512, 0),
            (512, 0, 0)
        ], dtype="double")
        self.robot_points_2D = np.array([
           (0, 0),
            (0, 512),
            (512, 512),
            (512, 0)
        ], dtype="double")
        self.distortion_coeffs = np.zeros((4, 1))
        # 焦距设置为其他值也可以，这里只是借用求转换矩阵的功能，原理可进一步讨论
        self.focal_length = 1
        self.center = (256, 256)
        self.matrix_camera = np.array(
            [[self.focal_length, 0, self.center[0]],
             [0, self.focal_length, self.center[1]],
             [0, 0, 1]], dtype="double"
        )
        
        self.success, self.vector_rotation, self.vector_translation = cv.solvePnP(objectPoints=self.image_points_3D,
                                                                                  imagePoints=self.robot_points_2D,
                                                                                  cameraMatrix=self.matrix_camera,
                                                                                  distCoeffs=None, flags=0)
        # print(self.vector_rotation)
        # print(self.vector_translation)

    def transform(self, point, z):
        trans_point, _ = cv.projectPoints(point, self.vector_rotation, self.vector_translation, self.matrix_camera,
                                          None)
        z_ = [z, 0, 0, 0]
        trans_point = np.append(trans_point[0], z_)
        # trans_point=np.round(trans_point,3)
        return trans_point


# trans = Trans()
# point = trans.transform(np.array([256.0, 256.0, 0.0]), 1)
# print(point)

因为需求是将图像坐标系下的点坐标转换为机器人坐标系下的二维坐标，因此这里将图像坐标增加一个维度z，将其设置为0，整个过程并没有用到相机，因此将相机内参矩阵中焦距设置为1（实测将焦距设置为不为0的其他值也可以），畸变系数设置为全零向量。再使用projectPoints()函数（官网）即可将图像坐标系下的点坐标转换到机器人坐标系下的点坐标，发送给机器人即可完成绘图任务。

---

总结

这里借用了OpenCV的solvePnP函数的坐标转换功能完成相关任务，如有错误，望指正。
关于焦距的设定可以依据官网Perspective-n-Point (PnP) pose computation进一步讨论。

---

如果觉得内容不错，请点赞、收藏、关注