OpenCV 识别 apriltags 标签并且获取对应二维旋转角度_机械臂识别apriltag最佳角度-CSDN博客

本文链接：https://blog.csdn.net/qq_44759447/article/details/128586872

学习过程中为了防止遗忘以此文字记录如有错误多多包涵

怕什么真理无穷,进一寸有一寸的欢喜 --- 胡适

前置内容：

学习目标：

python 通过 pupil_apriltags 库识别 apriltags 标签并且获取对应二维平面旋转角度

学习内容：

import time

import pupil_apriltags as apriltag

import cv2

import numpy as np
b = (1, 0)
degree = 0.0
def cv_show(name, img):
    cv2.imshow(name, img)
    cv2.waitKey(1)
def find_dir():
    global degree
    mid_point0 = [0, 0]
    mid_point1 = [0, 0]

    k_point0 = i.corners[0].astype(float)
    k_point1 = i.corners[1].astype(float)
    k_point2 = i.corners[2].astype(float)
    k_point3 = i.corners[3].astype(float)

    point0 = tuple(i.corners[0].astype(int))
    point1 = tuple(i.corners[1].astype(int))
    point2 = tuple(i.corners[2].astype(int))
    point3 = tuple(i.corners[3].astype(int))

    # cv2.circle(original_map, point0, 0, color=(125, 125, 255), thickness=3)
    # cv2.circle(original_map, point1, 0, color=(125, 125, 255), thickness=3)
    # cv2.circle(original_map, point2, 0, color=(125, 125, 255), thickness=3)
    # cv2.circle(original_map, point3, 0, color=(125, 125, 255), thickness=3)

    mid_point0[0] = ((point0[0] + point1[0]) / 2)
    mid_point0[1] = ((point0[1] + point1[1]) / 2)
    mid_point1[0] = ((point2[0] + point3[0]) / 2)
    mid_point1[1] = ((point2[1] + point3[1]) / 2)

    a = [(mid_point0[0] - mid_point1[0]), (mid_point0[1] - mid_point1[1])]
    cos = (a[0]*b[0] + a[1]*b[1]) / (1*pow(a[0]*a[0]+a[1]*a[1], 0.5))
    degree =  np.arccos(cos) * 360 / 3.14 / 2

    # cv2.circle(original_map, (mid_point0[0].astype(int), mid_point0[1].astype(int)), 0, color=(0, 125, 255), thickness=15)
    # cv2.circle(original_map, (mid_point1[0].astype(int), mid_point1[1].astype(int)), 0, color=(0, 125, 255), thickness=15)
    cv2.line(original_map, (mid_point1[0].astype(int), mid_point1[1].astype(int)), (mid_point0[0].astype(int), mid_point0[1].astype(int)), color=(0, 0, 255),thickness=3)
    cv2.line(original_map, (mid_point1[0].astype(int), mid_point1[1].astype(int)), point0, color=(0, 0, 255), thickness=3)
    cv2.line(original_map, (mid_point1[0].astype(int), mid_point1[1].astype(int)), point1, color=(0, 0, 255), thickness=3)

    if mid_point0[1] < mid_point1[1]:
        degree = -degree
    else:
        degree = degree
    print("cos = %f , degree = %d" % (cos, degree))

def get_DroidCam_url(ip, port=4747, res='1080p'):
    res_dict = {
        '1080p': '720x1280'
    }
    url = f'http://{ip}:{port}/mjpegfeed?{res_dict[res]}'
    return url

camera = cv2.VideoCapture(get_DroidCam_url('192.168.3.28', 4747, '1080p'))
grabbed, original_map = camera.read()

at_detector = apriltag.Detector(
   families="tag36h11",
   nthreads=1,
   quad_decimate=1.0,
   quad_sigma=0.0,
   refine_edges=1,
   decode_sharpening=0.25,
   debug=0
)

while 1:
    grabbed, original_map = camera.read()
    gary_map = cv2.cvtColor(original_map, cv2.COLOR_BGR2GRAY)  ## 转换为灰度图
    # detector = apriltag.Detector()
    result = at_detector.detect(gary_map)#,estimate_tag_pose = True, camera_params = [3595.60492898583, 3596.72948838094, 370.627983778196, 256.018080375677], tag_size = 3.5
    for i in result:
        find_dir()
    cv_show('original',original_map)

效果图