cpp
复制代码
import cv2
import sys
import copy
import zmq
import numpy as np
import math
import keyboard
import threading
import time
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
class Aruco_Process:
def __init__(self, camera_dev = 0, marker_size = 0.051) -> None:
# 相机设置
self.cap = cv2.VideoCapture(camera_dev)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) # 设置视频帧宽度
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 960) # 设置视频帧高度
self.position_pre = np.zeros(3) # 上一帧的位置， 用于平滑
self.euler_angles_pre = np.zeros(3) # 上一帧的姿态欧拉角， 用于平滑
self.k = 0.4
self.position_rate = 0.8
if not self.cap.isOpened():
print("无法打开摄像头")
exit()
self.marker_size = marker_size
# 创建检测参数
self.parameters = cv2.aruco.DetectorParameters_create()
# 调整检测参数以提高检测率
self.parameters.adaptiveThreshWinSizeMin = 3
self.parameters.adaptiveThreshWinSizeMax = 23
self.parameters.adaptiveThreshWinSizeStep = 10
self.parameters.adaptiveThreshConstant = 7
self.parameters.minMarkerPerimeterRate = 0.03
self.parameters.maxMarkerPerimeterRate = 4.0
self.parameters.polygonalApproxAccuracyRate = 0.05
self.parameters.minCornerDistanceRate = 0.05
self.parameters.minMarkerDistanceRate = 0.05
self.parameters.minDistanceToBorder = 3
frame = self.update_frame()
# 获取图像尺寸
height, width = frame.shape[:2]
# 估计相机内参（因为没有实际标定数据）
# 假设相机的焦距约为图像宽度，主点在图像中心
focal_length = width
center = (width / 2, height / 2)
self.camera_matrix = np.array(
[[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1]], dtype=np.float32
)
# 假设没有镜头畸变
self.dist_coeffs = np.zeros((4, 1), dtype=np.float32)
def update_frame(self):
# 读取一帧
ret, frame = self.cap.read()
if not ret:
print("无法获取视频帧")
exit()
else:
return frame
def detect_aruco_markers_with_pose(self, image, is_show = True):
"""
检测图像中的ArUco标记并计算摄像头位姿
参数:
image: 图像
camera_matrix: 相机内参矩阵
dist_coeffs: 畸变矩阵
返回:
标记后的图像和位姿信息
"""
# 实际应用中记得替换为您的实际相机内参。YOUR_FX,YOUR_FY为焦距.YOUR_CX,YOUR_CY为主点.
# camera_matrix = np.array([
# [YOUR_FX, 0, YOUR_CX],
# [0, YOUR_FY, YOUR_CY],
# [0, 0, 1]
# ], dtype=np.float32)
# 实际应用替换为实际的5个畸变系数
# dist_coeffs = np.array([[k1], [k2], [p1], [p2], [k3]], dtype=np.float32)
# 或者如果只有4个：
# dist_coeffs = np.array([[k1], [k2], [p1], [p2]], dtype=np.float32)
# 转换为灰度图像
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# 定义要尝试的ArUco字典列表
dict_type = cv2.aruco.DICT_6X6_50
# 字典名称映射（用于输出）
dict_names = {
cv2.aruco.DICT_4X4_50: "DICT_4X4_50",
cv2.aruco.DICT_6X6_50: "DICT_6X6_50",
}
# 创建结果图像的副本
result_image = image.copy()
try:
# 适配新版本OpenCV API
aruco_dict = cv2.aruco.getPredefinedDictionary(dict_type)
# 检测ArUco标记
corners, ids, rejected = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=self.parameters)
# 如果检测到标记
if ids is not None and len(ids) > 0:
if len(ids) > 1:
print("Detect more than one aruco, Wrong !!!")
return False, result_image, None
if is_show:
# 绘制检测到的标记
cv2.aruco.drawDetectedMarkers(result_image, corners, ids)
# 计算标记中心点
c = corners[0][0]
center_point = np.mean(c, axis=0).astype(int)
# 添加ID文本
marker_id = ids[0][0]
cv2.putText(
result_image,
f"ID: {marker_id}",
(center_point[0]-20, center_point[1]),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(0, 255, 0),
1
)
# 估计姿态
# 重新整理角点格式以适应estimatePoseSingleMarkers
marker_corners = np.array([corners[0]], dtype=np.float32)
# 估计姿态
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(marker_corners, self.marker_size, self.camera_matrix, self.dist_coeffs)
if is_show:
# 绘制坐标轴
cv2.drawFrameAxes(result_image, self.camera_matrix, self.dist_coeffs, rvecs[0], tvecs[0], self.marker_size / 2)
# 获取平移向量（米）
translation = tvecs[0][0]
translation[1] = -translation[1] # y 和 z 反向
translation[2] = -translation[2]
translation = translation * self.position_rate # 位置变化率
# 将旋转向量转换为旋转矩阵
rmat, _ = cv2.Rodrigues(rvecs[0])
trans_x_180 = np.array([[1, 0, 0],
[0, -1, 0],
[0, 0, -1]])
rmat = np.dot(trans_x_180, rmat) # 转换为相机坐标系到aruco坐标系的姿态变换
# 计算欧拉角
euler_angles = self.rotation_matrix_to_euler_angles(rmat)
# 平滑
translation = self.k * self.position_pre + (1-self.k) * translation # 位置平滑
euler_angles = self.euler_angles_pre * self.k + (1-self.k) * euler_angles # 姿态平滑
self.euler_angles_pre = copy.deepcopy(euler_angles)
self.position_pre = copy.deepcopy(translation)
# 转成控制的实际旋转矩阵
pos_result = np.vstack((np.hstack((self.euler_zyx_to_matrix(*euler_angles), translation.reshape(3, 1))), np.array([[0,0,0,1]])))
if is_show:
# 在图像上显示位置和角度信息
pos_text = f"Position: X:{translation[0]:.3f} Y:{translation[1]:.3f} Z:{translation[2]:.3f}"
rot_text = f"Rotation: R:{euler_angles[0]*180/3.14:.1f} P:{euler_angles[1]*180/3.14:.1f} Y:{euler_angles[2]*180/3.14:.1f}"
cv2.putText(
result_image,
pos_text,
(center_point[0]-20, center_point[1] + 30),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
(0, 0, 255),
1
)
cv2.putText(
result_image,
rot_text,
(center_point[0]-20, center_point[1] + 60),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
(0, 0, 255),
1
)
# 返回检测状态、图像和位姿结果
return True, result_image, pos_result
# 没有检测到aruco码
return False, result_image, None
except Exception as e:
print(f"使用字典 {dict_names[dict_type]} 检测时出错: {e}")
return False, result_image, None
def rotation_matrix_to_euler_angles(self, R):
"""
将旋转矩阵转换为欧拉角（弧度）
参数:
R: 3x3旋转矩阵
返回:
欧拉角 (roll, pitch, yaw) 弧度
"""
# 检查是否是有效的旋转矩阵
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
if sy > 1e-6:
x = math.atan2(R[2, 1], R[2, 2])
y = math.atan2(-R[2, 0], sy)
z = math.atan2(R[1, 0], R[0, 0])
else:
x = math.atan2(-R[1, 2], R[1, 1])
y = math.atan2(-R[2, 0], sy)
z = 0
return np.array([x, y, z])
import numpy as np
def euler_zyx_to_matrix(self, roll, pitch, yaw):
"""
将 ZYX 欧拉角 (yaw, pitch, roll) 转换为旋转矩阵。
参数:
roll: 绕 X 轴的旋转角度（弧度）
pitch: 绕 Y 轴的旋转角度（弧度）
yaw: 绕 Z 轴的旋转角度（弧度）
返回:
3x3 的旋转矩阵 (numpy.ndarray)
"""
cr = np.cos(roll)
sr = np.sin(roll)
cp = np.cos(pitch)
sp = np.sin(pitch)
cy = np.cos(yaw)
sy = np.sin(yaw)
R = np.array([
[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
[sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
[-sp, cp*sr, cp*cr]
])
return R
def close_cap(self):
self.cap.release()