1、aruco标记检测基础知识
1.1 aruco简介
aruco标记是可用于摄像机姿态估计的二进制方形基准标记，由宽黑色边框和确定其标识符（id）的内部二进制矩阵组成。黑色边框有助于其在图像中的快速检测，内部二进制编码用于识别标记和提供错误检测和纠正。
aruco标记尺寸的大小决定内部矩阵的大小，例如尺寸为 4×4 的标记由 16 位二进制数组成。由于单个aruco标记就可以提供足够的对应关系，例如有四个明显的角点及内部的二进制编码，所以aruco标记被广泛用来增加从二维世界映射到三维世界时的信息量，便于发现二维世界与三维世界之间的投影关系，从而实现姿态估计、相机矫正等等应用。
OpenCV中的ArUco模块包括了对aruco标记的创建和检测，以及将aruco标记用于姿势估计和相机矫正等应用的相关API，同时还提供了标记板等等。
1.2 aruco创建
为了做aruco的检测，首先需要提供标记样本，其获取方式可以有两种方式：一种是从其他地方拷贝，另一种就是自己生成。这里介绍如何自己生成。
首先我们创建aruco标记时，需要先指定一个字典，这个字典表示的是创建出来的aruco标记具有怎样的尺寸、怎样的编码等等内容，我们使用APIgetPredefinedDictionary（）来声明我们使用的字典。在OpenCV中，提供了多种预定义字典，我们可以通过PREDEFINED_DICTIONARY_NAME来查看有哪些预定义字典。而且字典名称表示了该字典的aruco标记数量和尺寸，例如DICT_7X7_50表示一个包含了50种7×7位标记的字典。
标记生成代码示例：
import cv2
import numpy as np
# 生成aruco标记
# 加载预定义的字典
dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
# 生成标记
markerImage = np.zeros((200, 200), dtype=np.uint8)
markerImage = cv2.aruco.drawMarker(dictionary, 22, 200, markerImage, 1)
cv2.imwrite("marker22.png", markerImage) opencv的aruco模块共有25个预定义的标记词典。每个词典中所有的Aruco标记均包含相同数量的块或位(例如4×4、5×5、6×6或7×7)，且每个词典中Aruco标记的数量固定(例如50、100、250或1000)。
cv2.aruco.Dictionary_get()函数会加载cv2.aruco.DICT_6X6_250包含250个标记的字典，其中每个标记都是6×6位二进制模式
cv2.aruco.drawMarker(dictionary, 22, 200, markerImage, 1)中的第二个参数22是aruco的标记id（0～249），第三个参数决定生成的标记的大小，在上面的示例中，它将生成200×200像素的图像，第四个参数表示将要存储aruco标记的对象(上面的markerImage），最后，第五个参数是边界宽度参数，它决定应将多少位（块）作为边界添加到生成的二进制图案中。
执行后将会生成这样的标记：标记id分别是22
1.3 aruco检测
import numpy as np
import time
import cv2
import cv2.aruco as aruco
#读取图片
frame=cv2.imread('IMG_3739.jpg')
#调整图片大小
frame=cv2.resize(frame,None,fx=0.2,fy=0.2,interpolation=cv2.INTER_CUBIC)
#灰度话
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
#设置预定义的字典
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
#使用默认值初始化检测器参数
parameters = aruco.DetectorParameters_create()
#使用aruco.detectMarkers()函数可以检测到marker，返回ID和标志板的4个角点坐标
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)
#画出标志位置
aruco.drawDetectedMarkers(frame, corners,ids)
cv2.imshow("frame",frame)
cv2.waitKey(0)
cv2.destroyAllWindows() 对于每次成功检测到标记，将按从左上，右上，右下和左下的顺序检测标记的四个角点。在C++中，将这4个检测到的角点存储为点矢量，并将图像中的多个标记一起存储在点矢量容器中。在Python中，它们存储为Numpy 数组。
detectMarkers()函数用于检测和确定标记角点的位置。
第一个参数image是带有标记的场景图像。
第二个参数dictionary是用于生成标记的字典。
第三个参数parameters： DetectionParameters 类的对象，该对象包括在检测过程中可以自定义的所有参数；
返回参数corners：检测到的aruco标记的角点列表，对于每个标记，其四个角点均按其原始顺序返回（从右上角开始顺时针旋转），第一个角是右上角，然后是右下角，左下角和左上角。
返回ids：检测到的每个标记的 id；
返回参数rejectedImgPoints：抛弃的候选标记列表，即检测到的、但未提供有效编码的正方形。
当我们检测到aruco标签之后,为了方便观察,我们需要进行可视化操作,把标签标记出来:使用drawDetectedMarkers（）这个API来绘制检测到的aruco标记，其参数含义如下：
参数image: 是将绘制标记的输入 / 输出图像（通常就是检测到标记的图像）
参数corners：检测到的aruco标记的角点列表
参数ids：检测到的每个标记对应到其所属字典中的id,可选（如果未提供）不会绘制ID。
参数borderColor：绘制标记外框的颜色,其余颜色（文本颜色和第一个角颜色）将基于该颜色进行计算，以提高可视化效果，可忽略。
2、实现环境说明
硬件平台：NVIDIA Jetson NX
python：2.7.17
opencv：3.3.1
numpy: 1.13.3
飞控版本： ArduCopter 4.6 Dev
3、aruco标记生成
在上面已经讲述了aruco标记生成的方法，但是上面的例子只是生成一张标记，通常情况下，需要对标识进行多重设计，即一张地标上包含多张大小、ID不同的标记，以便实现无人机在不同高度的识别要求，有时也需要应对在不同ID标识上降落的需要场景。
import numpy as np # 导入numpy库，用于处理数组和数值计算
import time # 导入time库，可用于时间相关操作
import cv2 # 导入OpenCV库，用于图像处理
import cv2.aruco as aruco # 导入ArUco模块，专门用于处理ArUco标记
import os # 导入os库，用于文件和目录操作
# 获取预定义的ArUco字典，这里使用的是DICT_4X4_250
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)
# 设置生成的ArUco标记的起始ID和结束ID
start_id = 0
end_id = 250
marker_size = 500 # 指定每个ArUco标记的大小（像素）
# 定义输出目录，用于保存生成的ArUco标记图像
output_dir = 'aurco_markers'
# 如果输出目录不存在，则创建该目录
if not os.path.exists(output_dir):
os.makedirs(output_dir)
# 遍历从start_id到end_id-1的所有ID，生成对应的ArUco标记
for marker_id in range(start_id, end_id):
# 使用指定的字典和ID生成ArUco标记图像
marker_image = cv2.aruco.drawMarker(aruco_dict, marker_id, marker_size)
# 将生成的灰度图像转换为三通道BGR图像
marker_image_rgb = cv2.cvtColor(marker_image, cv2.COLOR_GRAY2BGR)
# 创建一个更大的背景图像，尺寸为marker_size+200，以容纳ArUco标记及其周围留白
background_size = (marker_size + 200, marker_size + 200)
background_image = np.ones(background_size + (3,), dtype=np.uint8) * 255 # 白色背景
# 计算将ArUco标记放置在背景图像中心的位置
marker_center = (background_size[0] // 2, background_size[1] // 2)
marker_start = (marker_center[0] - marker_size // 2, marker_center[1] - marker_size // 2)
# 将ArUco标记图像放置在背景图像中央
background_image[marker_start[1]:marker_start[1] + marker_size,
marker_start[0]:marker_start[0] + marker_size] = marker_image_rgb
# 定义生成的图像文件名，格式为"marker_XXXX.png"
filename = 'marker_%04d.png' % marker_id
filepath = os.path.join(output_dir, filename) # 构建完整的文件路径
# 将合成后的图像写入文件
cv2.imwrite(filepath, background_image)
# 打印已保存的文件名信息
print('save %s' % filename)
# 所有标记生成完成后打印提示信息
print('All markers have been generated and saved')不同版本的opencv，函数的名字和参数均会有所区别，如果在代码运行过程中出现错误，一定仔细检查opencv版本。
生成的图片集包含了251张标识，可以从中筛选辨识度较高的标识作为地标标识。本人为了满足飞机不同高度的降落识别要求，设计了如下的地面标识板，降落原点位于右侧，后期将进行位置变换处理：
4、摄像头校准
摄像头校准是一项必须做、必须仔细做的工作，决定了最终位置检测的精度。整个过程三个步骤：（1）生成棋盘图，并打印出来；（2）拍摄多种角度的棋盘图片并保存；（3）运行校准算法，获取内参矩阵和畸变系数。
4.1 生成棋盘图
import cv2
import numpy as np
# 定义棋盘格的尺寸
size = 140
# 定义标定板尺寸
boardx = size * 10
boardy = size * 10
canvas = np.zeros((boardy, boardx, 1), np.uint8) # 创建画布
for i in range(0, boardx):
for j in range(0, boardy):
if (int(i/size) + int(j/size)) % 2 != 0: # 判定是否为奇数格
canvas[j, i] = 255
cv2.imwrite("./chessboard.png", canvas)最后生成如下的图片，当然也可以从网上下载该图片，只要保证图片的清晰度即可，尤其一些角点是清晰的。
4.2 拍摄照片
通过运行下面的程序，距离棋盘图片大于1米左右，拍摄不同角度的照片，并保存下来，总共拍摄约20~30张即可。代码运行后按“j”键保存一张图片，最后按“q”键退出程序。
import cv2
camera = cv2.VideoCapture(0)
#camera = cv2.VideoCapture("rtsp://admin:admin@192.168.144.10:554/stream=0")
print(camera.get(cv2.CAP_PROP_FRAME_WIDTH))
print(camera.get(cv2.CAP_PROP_FRAME_HEIGHT))
i = 0
ret, img = camera.read()
print('input j, save picture')
print('inut q, quit program')
while ret:
ret, img = camera.read()
cv2.imshow('img', img)
if cv2.waitKey(1) & 0xFF == ord('j'):
i += 1
firename = str('./img' + str(i) + '.jpg')
cv2.imwrite(firename, img)
print('save picture:', firename)
#cv2.imshow('video_720*720', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
print('quit')
break
camera.release()
cv2.destroyAllWindows()
最后生成一堆下面的图片：
4.3 校准
#!/usr/bin/python
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import glob
# 找棋盘格角点
# 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # 阈值
#棋盘格模板规格 角点数量
w = 9 # 10 - 1
h = 6 # 7 - 1
# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵
objp = np.zeros((w*h,3), np.float32)
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
objp = objp * 38 # 18.1 mm
# 储存棋盘格角点的世界坐标和图像坐标对
objpoints = [] # 在世界坐标系中的三维点
imgpoints = [] # 在图像平面的二维点
#加载pic文件夹下所有的jpg图像
images = glob.glob('./*.jpg') # 拍摄的十几张棋盘图片所在目录
i = 0
print (len(images))
for fname in images:
img = cv2.imread(fname)
# 获取画面中心点
# 获取图像的长宽
h1, w1 = img.shape[0], img.shape[1]
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
u, v = img.shape[:2]
# 找到棋盘格角点
ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
# 如果找到足够点对，将其存储起来
if ret == True:
print("i:", i)
i = i+1
# 在原角点的基础上寻找亚像素角点
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
# 追加进入世界三维点和平面二维点中
objpoints.append(objp)
imgpoints.append(corners)
# 将角点在图像上显示
cv2.drawChessboardCorners(img, (w,h), corners, ret)
cv2.namedWindow('findCorners', cv2.WINDOW_NORMAL)
cv2.resizeWindow('findCorners', 640, 480)
cv2.imshow('findCorners', img)
cv2.waitKey(200)
while True:
if cv2.waitKey(10) == ord("c"):
break
cv2.destroyAllWindows()
#%% 标定
print('正在计算')
#标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
print("ret:", ret)
print("mtx:
", mtx) # 内参数矩阵
print("dist畸变值:
", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
print("rvecs旋转（向量）外参:
", rvecs) # 旋转向量 # 外参数
print("tvecs平移（向量）外参:
", tvecs) # 平移向量 # 外参数
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0, (u, v))
print('newcameramtx外参', newcameramtx)
#校准输出 打开摄像机
camera = cv2.VideoCapture(0)
while True:
(grabbed, frame) = camera.read()
h1, w1 = frame.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w1, h1), 0, (w1, h1))
# 纠正畸变
dst1 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w1, h1), 5)
dst2 = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
# 裁剪图像，输出纠正畸变以后的图片
x, y, w1, h1 = roi
dst1 = dst1[y:y + h1, x:x + w1]
cv2.imshow('frame',frame)
cv2.imshow('dst1',dst1)
cv2.imshow('dst2', dst2)
if cv2.waitKey(1) & 0xFF == ord('q'): # 按q保存一张图片
#cv2.imwrite("../u4/frame.jpg", dst1)
break
camera.release()
cv2.destroyAllWindows()
校准完成最终输出mtx和dist两个参数是最终想要的结果，一个是内参数矩阵，另一个是畸变系数。
代码校准完成后将重新启动摄像头进行效果验证：
frame框显示原始视频画面
dst1显示经过校准处理后的画面
dst2是在dst1基础上进一步裁剪黑边的优化版本
当摄像头校准后出现黑边时，dst2版本会自动去除这些黑边。此时在dst1或dst2画面中，墙角或显示器边框应呈现为笔直的线条，而frame框中的相同物体则仍会显示为曲线。
5、标记检测实现
在机载计算机上运行标记检测程序，将生成的位置信息通过mavlink2.0协议发送给飞控。
5.1 Mavlink 2.0协议发送位置检测信息
Mavlink协议的关键参数 消息ID： 149 帧长： 60 CRC： 200
MAVPACKED(
typedef struct __mavlink_landing_target_t {
uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/
float angle_x; /*< [rad] X-axis angular offset of the target from the center of the image*/
float angle_y; /*< [rad] Y-axis angular offset of the target from the center of the image*/
float distance; /*< [m] Distance to the target from the vehicle*/
float size_x; /*< [rad] Size of target along x-axis*/
float size_y; /*< [rad] Size of target along y-axis*/
uint8_t target_num; /*< The ID of the target if multiple targets are present*/
uint8_t frame; /*< Coordinate frame used for following fields.*/
float x; /*< [m] X Position of the landing target in MAV_FRAME*/
float y; /*< [m] Y Position of the landing target in MAV_FRAME*/
float z; /*< [m] Z Position of the landing target in MAV_FRAME*/
float q[4]; /*< Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/
uint8_t type; /*< Type of landing target*/
uint8_t position_valid; /*< Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid).*/
}) mavlink_landing_target_t; 5.2 完整代码
#!/usr/bin/python
# -*- coding: utf-8 -*-
import numpy as np
import time
import cv2
import cv2.aruco as aruco
import math
import serial
import struct
ids_family=[18,19,20,21,65,61,232]
def rotation_matrix_to_quaternion(R):
"""
将旋转矩阵R转换为四元数。
参数: R -- 3x3的旋转矩阵，numpy数组形式
返回: q -- 四元数(q0, q1, q2, q3)，其中q0是实部，q1, q2, q3是虚部
"""
# 检查输入矩阵R是否为3x3
if R.shape != (3, 3):
raise ValueError("输入矩阵R必须是3x3")
# 旋转矩阵到四元数的转换
m00, m01, m02 = R[0, 0], R[0, 1], R[0, 2]
m10, m11, m12 = R[1, 0], R[1, 1], R[1, 2]
m20, m21, m22 = R[2, 0], R[2, 1], R[2, 2]
tr = m00 + m11 + m22
if tr > 0:
S = np.sqrt(tr + 1.0) * 2 # S=4*qw
qw = 0.25 * S
qx = (m21 - m12) / S
qy = (m02 - m20) / S
qz = (m10 - m01) / S
elif (m00 > m11) and (m00 > m22):
S = np.sqrt(1.0 + m00 - m11 - m22) * 2 # S=4*qx
qw = (m21 - m12) / S
qx = 0.25 * S
qy = (m01 + m10) / S
qz = (m02 + m20) / S
elif m11 > m22:
S = np.sqrt(1.0 + m11 - m00 - m22) * 2 # S=4*qy
qw = (m02 - m20) / S
qx = (m01 + m10) / S
qy = 0.25 * S
qz = (m12 + m21) / S
else:
S = np.sqrt(1.0 + m22 - m00 - m11) * 2 # S=4*qz
qw = (m10 - m01) / S
qx = (m02 + m20) / S
qy = (m12 + m21) / S
qz = 0.25 * S
q = np.array([qw, qx, qy, qz])
return q
def cal_aruco_area(markerCorner):
pts = markerCorner.reshape(4, 2)
rect = np.zeros((4, 2), dtype="float32")
# 计算矩形边界框的坐标
s = pts.sum(axis=1)
rect[0] = pts[np.argmin(s)]
rect[2] = pts[np.argmax(s)]
diff = np.diff(pts, axis=1)
rect[1] = pts[np.argmin(diff)]
rect[3] = pts[np.argmax(diff)]
# 计算面积
(tl, tr, br, bl) = rect
widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2))
widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2))
heightA = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2))
heightB = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2))
maxWidth = max(int(widthA), int(widthB))
maxHeight = max(int(heightA), int(heightB))
area = maxWidth * maxHeight
return area
def crc_accumulate(data,crc):
for b in data[1:]:
ch = (ord(b) ^ (crc & 0x00ff)) & 0xff
ch = (ch ^ (ch << 4)) & 0xff
crc = ((crc >> 8) ^ (ch << 8) ^ (ch << 3) ^ (ch >> 4)) & 0xffff
return crc
def build_mavlink_message(system_id, component_id, sequence, message_id, payload):
# Step 1: 构造头部
magic = 'xFD'
payload_length = chr(len(payload)) #转化为字符
incompat_flags = 'x00'
compat_flags = 'x00'
seq_bytes = chr(sequence % 256)
sys_id = chr(system_id)
comp_id = chr(component_id)
# 消息ID是3个字节（小端序）
msg_id_bytes = struct.pack('<I', message_id)[0:3]
# Step 2: 构造完整数据包（不含 checksum）
header = magic + payload_length + incompat_flags + compat_flags + seq_bytes + sys_id + comp_id + msg_id_bytes
data_for_checksum = header + payload + 'xC8'
# Step 3: 计算校验码
#crc = calculate_crc16(data_for_checksum)
crc = crc_accumulate(data_for_checksum,0xffff)
crc_bytes = struct.pack('<H', crc) # 小端序
# Step 4: 返回完整 MAVLink 数据包
return header + payload + crc_bytes
sequence = 0
def send_data_by_mavlink(posx,posy,posz,distance):
# 设置参数
system_id = 2
component_id = 2
global sequence
sequence = sequence + 1 # 每次发送递增
message_id = 149 # 自定义消息 ID
time_stamp_us = time.time() * 1000000
# 构造 payload
payload = struct.pack('<d', time_stamp_us) + struct.pack('<f',0.02) + struct.pack('<f', 0.02)+
struct.pack('<f', distance) + struct.pack('<f', 0.02) + struct.pack('<f', 0.02) +
struct.pack('<b', 23) + struct.pack('<b', 12) + struct.pack('<f', posx) +
struct.pack('<f', posy) + struct.pack('<f', posz) + struct.pack('<f', 0.0) +
struct.pack('<f', 0.0) + struct.pack('<f', 0.0) + struct.pack('<f', 0.0) +
struct.pack('<B', 1) + struct.pack('<B', 1)
# 构造 MAVLink 消息
mavlink_packet = build_mavlink_message(system_id, component_id, sequence, message_id, payload)
# 发送到串口
ser.write(mavlink_packet)
mtx=np.array([[ 307.59851072, 0. , 294.06291477],
[ 0. , 304.56396729, 175.49323563],
[ 0. , 0. , 1. ]])
dist=np.array([[ 0.07123871, -0.10871371, -0.00184005, 0.00067256, 0.03111138]])
newcameramtx=np.array([[ 352.5602417 , 0. , 270.10114665],
[ 0. , 357.32263184, 202.50286479],
[ 0. , 0. , 1. ]])
# 打开串口
ser = serial.Serial('/dev/ttyTHS1', 115200, timeout=1)
cap = cv2.VideoCapture(0)
font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below)
if not cap.isOpened():
print("无法打开摄像头")
exit()
print(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
print(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
while True:
ret, frame = cap.read() # 读取摄像头画面
h1, w1 = frame.shape[:2] # shape中包含三个属性，长度/宽度/颜色通道数
#纠正畸变
#返回一个新的相机矩阵 newcameramtx 和一个感兴趣区域（ROI）roi，ROI 用于裁剪校正后的图像，以去除可能因校正而产生的黑色边界
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w1, h1), 0, (w1, h1))
dst1 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
# 区域裁剪
x, y, w1, h1 = roi
dst1 = dst1[y:y + h1, x:x + w1]
frame=dst1 # 得到校正后的图像
# 得到灰度图
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
kernel=np.array([[-1, -1, -1], # 定义一个卷积核
[-1, 9, -1],
[-1, -1, -1]])
#创建一个 ArUco 字典和检测参数
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
parameters = aruco.DetectorParameters_create()
#检测，返回ID和标志的4个角点坐标
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)
# 根据检测到的id号对length_vec[]列表进行赋值
if ids is not None:
length_vec = np.random.rand(len(ids))
for i in range(len(ids)):
if ids[i]==232:
length_vec[i]=0.265 #m
elif ids[i]==65 or ids[i]==61:
length_vec[i]=0.145
elif ids[i]==18 or ids[i]==19 or ids[i]==20 or ids[i]==21:
length_vec[i]=0.057
# 新建空列表，用于存储平移向量和旋转向量
rvec = []
tvec = []
for i, corners_i in enumerate(corners):
rvec_p, tvec_p , _= aruco.estimatePoseSingleMarkers([corners_i], length_vec[i], mtx, dist) # 根据焦点坐标和测量的物理尺寸进行三维姿态估计
# 存储计算结果
if rvec is not None and tvec is not None:
rvec.append(rvec_p[0]) # rvec 是一个列表，即使只有一个标记也会这样
tvec.append(tvec_p[0]) # tvec 也是一个列表
# 估计每个标记的姿态并返回值
aruco.drawDetectedMarkers(frame, corners,ids) # 识别结果可视化
the_bigest_mark=0
the_bigest_mark_area=0.0
for i, (rvec_item,tvec_item) in enumerate(zip(rvec,tvec)): #计算二维码占面积最大的那一个
mark_area=cal_aruco_area(corners[i])
if mark_area>the_bigest_mark_area:
the_bigest_mark_area=mark_area
the_bigest_mark=i
for i, (rvec_item,tvec_item) in enumerate(zip(rvec,tvec)): # 对每个识别的二维码分别进行姿态估计
R, _ = cv2.Rodrigues(rvec_item) # 将旋转向量转换为旋转矩阵
q = rotation_matrix_to_quaternion(R) # 转换为四元数
thetaz = -math.degrees(math.atan2(R[0][1], R[0][0])) # 计算方位角度
tvec_temp=tvec_item.transpose() # 由平移向量计算位置
result = np.dot(R.T, tvec_temp) # marker坐标系，x轴向左，y轴向下
# 根据计算结果和物理位置计算每个二维码与飞机相对位置
if ids[i]==18:
result=result+[[0.06],[0.05],[0]]
elif ids[i]==19:
result=result+[[-0.06],[-0.05],[0]]
elif ids[i]==20:
result=result+[[0.06],[-0.05],[0]]
elif ids[i]==21:
result=result+[[-0.06],[0.05],[0]]
elif ids[i]==65:
result=result+[[0.30],[-0.10],[0]]
elif ids[i]==61:
result=result+[[0.30],[0.10],[0]]
elif ids[i]==232:
result=result+[[0.58],[0.0],[0]]
theta = np.radians(thetaz) #此处是坐标变化，是位置正确输出的前提条件
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
Rz = np.array([
[cos_theta, -sin_theta, 0],
[sin_theta, cos_theta, 0],
[0, 0, 1]
])
result_rotated = np.dot(Rz, result)
result = result_rotated # marker在机体坐标系下的位置，x轴向右，y轴向前
#计算二维码的区域占比
mark_area=cal_aruco_area(corners[i])
height, width = frame.shape[:2]
mark_area_percent=1.0 * mark_area / (height*width) * 100.0
if i==the_bigest_mark and ids[i] in ids_family:
print("当前id %2d %.2f %.2f %.2f %.2f %.2f" % (ids[i],result[0][0],result[1][0],result[2][0],mark_area_percent,thetaz))
distance = math.sqrt((result[0][0]*1.0)**2 + (result[1][0]*1.0)**2 + (result[2][0]*1.0)**2)
send_data_by_mavlink(result[1][0]*1.0,result[0][0]*1.0,-result[2][0]*1.0,distance)
cv2.putText(frame, "Id: " + str(ids), (0,64), font, 1, (0,255,0),2, cv2.LINE_AA) # DRAW ID
del rvec
del tvec
else:
cv2.putText(frame, "No Ids", (0,64), font, 1, (0,255,0),2,cv2.LINE_AA) # DRAW "NO IDS"
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
cv2.resizeWindow('frame', 640, 480)
cv2.imshow("frame",frame) # 显示结果
time_end=time.time()
key = cv2.waitKey(1)
if key == 27: # 按esc键退出
print('esc break...')
ser.close() # 关闭串口
cap.release()
cv2.destroyAllWindows()
break
© 版权声明
文章版权归作者所有，未经允许请勿转载。如内容涉嫌侵权，请在本页底部进入<联系我们>进行举报投诉!
THE END
![【解决Maven依赖报错】ERROR] Failed to execute goal on project pscs-pc-core: Could not resolve dependencies - 宋马](https://pic.songma.com/blogimg/20250613/d3370eb06216401e82b47e5533f7a322)
暂无评论内容