通过二维码计算6D位置姿态（基于Stag码和ROS2实现）

简介

6D位置姿态在工业自动化、机器人、AGV等领域中有广泛的应用，6D位置姿态是指在三维空间中描述物体的位置和方向，即x、y、z和Roll（滚转）、Yaw（偏航）、Pitch（俯仰）。在slam的精确定位、机器人工具夹爪的快换、模拟末端进行遥操作、物体的定位等方面均可得到应用。这里提供一份示例代码，以供研究者和工程师交流。

算法思路

1、相机取流，这里以笔记本自带的Webcam为例
2、获取RGB对象
3、识别二维码角点
4、根据PnP（Perspective-n-Point）计算位姿
5、通过ROS2将位姿数据发出

依赖

stag、opencv-python、numpy、ROS2

二维码获取

通过该链接下载电子版：https://drive.google.com/drive/folders/0ByNTNYCAhWbIV1RqdU9vRnd2Vnc?resourcekey=0-9ipvecbezW8EWUva5GBQTQ
之后自行打印，贴在所需位置。

代码

在此之前，需要获取相机的内参和畸变矩阵，可以通过相机出场的标定内参获取，也可使用标定板进行标定。如果使用标定板进行标定，需要用相机拍摄若干不同位置的标定板图片。下面给出使用12*9的棋盘格标定板进行标定的代码：

import numpy as np
import glob
import cv2

# 准备棋盘格的3D点
objp = np.zeros((11 * 8, 3), np.float32)
objp[:, :2] = np.mgrid[0:11, 0:8].T.reshape(-1, 2)
objp = 3 * objp  # 假设每个方格的大小为3个单位

# 存储棋盘格的3D点和图像点
obj_points = []
img_points = []

# 获取所有图片的路径
images = glob.glob("/home/lz/Documents/desk_camera/*.png")
print(images)

# 遍历每张图片，检测棋盘格角点
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (11, 8), None)
    if ret:
        obj_points.append(objp)
        corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
                                    (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))
        if [corners2]:
            img_points.append(corners2)
        else:
            img_points.append(corners)
        cv2.drawChessboardCorners(img, (11, 8), corners, ret)
        cv2.waitKey(1)

# 相机标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)

# 计算重投影误差
total_error = 0
for i in range(len(obj_points)):
    img_points2, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(img_points[i], img_points2, cv2.NORM_L2) / len(img_points2)
    total_error += error

mean_error = total_error / len(obj_points)

Camera_intrinsic = {"mtx": mtx, "dist": dist, "reprojection_error": mean_error}

print("Camera Intrinsic Parameters:")
print(Camera_intrinsic)
print(f"Mean Reprojection Error: {mean_error}")

下面是通过Stag码识别6D位姿的代码：

import stag
import cv2
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
import numpy as np

libraryHD = 15

# 0是代表摄像头编号，只有一个的话默认为0
capture = cv2.VideoCapture(0)
# 设置分辨率
width = 1280  # 设置宽度，例如1280像素
height = 720  # 设置高度，例如720像素

capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, height)

objp = np.zeros((2 * 2, 3), np.float32)
grid = np.mgrid[0:2, 0:2].T.reshape(-1, 2)
grid_sorted = np.zeros_like(grid)
grid_sorted[:, 0] = grid[:, 0]
grid_sorted[:, 1] = grid[:, 1]
grid_sorted[2, :], grid_sorted[3, :] = grid[3, :], grid[2, :]

# 将重排后的坐标赋值给 objp
objp[:, :2] = grid_sorted
worldpoint = objp * 62  # 棋盘格的宽度为15mm (array(70,3))

mtx = np.array([[996.73279988,   0.        , 664.57187967],
       [  0.        , 994.24447866, 389.01798004],
       [  0.        ,   0.        ,   1.        ]], dtype=np.float32)
dist = np.array([[ 0.15361005, -0.5340305 ,  0.00569068,  0.01081487,  0.82260219]], dtype=np.float32)


rclpy.init()
node = Node("eef_pose")
publisher_left = node.create_publisher(Pose, '/left_eff_pose', 10)
publisher_right = node.create_publisher(Pose, '/right_eff_pose', 10)

# 初始化上一帧的位姿为 None
previous_pose_left = None
previous_pose_right = None


def rotation_matrix_to_quaternion(R):
    q = np.empty((4,))
    t = np.trace(R)
    if t > 0.0:
        t = np.sqrt(t + 1.0)
        q[3] = 0.5 * t
        t = 0.5 / t
        q[0] = (R[2, 1] - R[1, 2]) * t
        q[1] = (R[0, 2] - R[2, 0]) * t
        q[2] = (R[1, 0] - R[0, 1]) * t
    else:
        i = 0
        if R[1, 1] > R[0, 0]:
            i = 1
        if R[2, 2] > R[i, i]:
            i = 2
        j = (i + 1) % 3
        k = (j + 1) % 3
        t = np.sqrt(R[i, i] - R[j, j] - R[k, k] + 1.0)
        q[i] = 0.5 * t
        t = 0.5 / t
        q[3] = (R[k, j] - R[j, k]) * t
        q[j] = (R[j, i] + R[i, j]) * t
        q[k] = (R[k, i] + R[i, k]) * t
    return q

while True:
    # 调用摄像机
    ref, frame = capture.read()
    (corners, ids, rejected_corners) = stag.detectMarkers(frame, libraryHD)
    if len(ids) >= 1:
        for i, id in enumerate(ids):
            if id[0] == 1:
                _, rvec, tvec = cv2.solvePnP(worldpoint, corners[i], mtx, dist)  # 解算位姿
                # Convert rvec to rotation matrix
                rotation_matrix, _ = cv2.Rodrigues(rvec)

                transformation_matrix = np.eye(4)

                # 将旋转矩阵放入变换矩阵的左上角
                transformation_matrix[:3, :3] = rotation_matrix

                # 将平移向量放入变换矩阵的最后一列
                transformation_matrix[:3, 3] = tvec.flatten()


                pose = Pose()
                # 平移部分
                pose.position.x = transformation_matrix[0, 3]
                pose.position.y = transformation_matrix[1, 3]
                pose.position.z = transformation_matrix[2, 3]

                # 旋转部分 (转换为四元数)
                rot = transformation_matrix[:3, :3]
                quat = rotation_matrix_to_quaternion(rot)
                pose.orientation.x = quat[0]
                pose.orientation.y = quat[1]
                pose.orientation.z = quat[2]
                pose.orientation.w = quat[3]

                publisher_left.publish(pose)

            if id[0] == 2:
                _, rvec, tvec = cv2.solvePnP(worldpoint, corners[i], mtx, dist)  # 解算位姿
                # Convert rvec to rotation matrix
                rotation_matrix, _ = cv2.Rodrigues(rvec)

                transformation_matrix = np.eye(4)

                # 将旋转矩阵放入变换矩阵的左上角
                transformation_matrix[:3, :3] = rotation_matrix

                # 将平移向量放入变换矩阵的最后一列
                transformation_matrix[:3, 3] = tvec.flatten()

                pose = Pose()
                # 平移部分
                pose.position.x = transformation_matrix[0, 3]
                pose.position.y = transformation_matrix[1, 3]
                pose.position.z = transformation_matrix[2, 3]

                # 旋转部分 (转换为四元数)
                rot = transformation_matrix[:3, :3]
                quat = rotation_matrix_to_quaternion(rot)
                pose.orientation.x = quat[0]
                pose.orientation.y = quat[1]
                pose.orientation.z = quat[2]
                pose.orientation.w = quat[3]

                publisher_right.publish(pose)