使用电脑摄像头计算aruco marker位姿（Python）

一、效果图

刚做了一些尝试，算两个aruco之间的距离
先算x方向，用ID=12减去ID=13，tvec的三个坐标依次是Z、Y、X。
所以，ID=12和ID=13的x距离为1.692-1.539=0.153(m), 15.3cm, 实际距离为11cm, 所以有误差

算y方向，用ID=12减去ID=14，
所以，ID=12和ID=14的距离为-0.063-0.119=-0.182（m）， 18.2cm，实际距离为11cm. 有误差

ID=12，用平移向量计算两个aruco之间的距离

ID=13

ID=14

二、笔记本摄像头的标定

进行Aruco码可视化前，需要知道相机的参数，具体为内参数mtx, 畸变参数dist.
需要准备一个棋盘格，拍摄十几张图片，代码如下：

我的棋盘格是7X10的，需要改的参数有棋盘格的行列数，每格的大小毫米数，图片的路径

import cv2
import numpy as np
import glob
import matplotlib.pyplot as plt
import matplotlib.patches as patches

# 找棋盘格角点
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*24  # 24mm    注意改参数！！！！！棋盘格的每格的长度！

# 储存棋盘格角点的世界坐标和图像坐标对
objpoints = [] # 在世界坐标系中的三维点
imgpoints = [] # 在图像平面的二维点

images = glob.glob('./Biaoding/*.jpg')  #   拍摄的十几张棋盘图片所在目录   需要改！！

i = 1
for fname in images:

    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # 找到棋盘格角点
    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', 810, 405)
        cv2.imshow('findCorners',img)
        cv2.waitKey(500)
cv2.destroyAllWindows()
#%% 标定
ret, mtx, dist, rvecs, tvecs = \
    cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)


print("ret:",ret  )
print("mtx:\n",mtx)      # 内参数矩阵     需要！！
print("dist:\n",dist   )   # 畸变系数    需要！！
print("rvecs:\n",rvecs)   # 旋转向量  # 外参数
print("tvecs:\n",tvecs  )  # 平移向量  # 外参数

三、计算Aruco码的位姿

第一步计算出的相机的参数需要输入到第二步中，参数为内参mtx和畸变参数dist. 运行成功，会打开电脑摄像头，实时显示Aruco码的位姿。代码如下：

代码调用aruco包时经常显示没有这个包，No module named ‘aruco’，参考了很多文章，最后解决了，放在第四部分。

如果程序没有问题，可以实时计算显示Aruco码的位姿。

import numpy as np
import time
import cv2
import cv2.aruco as aruco

#相机内参
#mtx =
# [[1.22069150e+03 0.00000000e+00 6.61198100e+02]
# [0.00000000e+00 1.17618858e+03 2.81004495e+02]
#0    0    1

mtx = np.array([
        [1220.69,       0, 661.20],
        [      0, 1176.19, 281.00],
        [      0,       0,      1],
        ])

#相机畸变参数
#dist= [[-4.76531264e-02  8.98268556e-02  8.77978795e-04  1.08441766e-02  9.03702693e-01]]
dist = np.array( [-4.76531264e-02, 8.98268556e-02, 8.77978795e-04, 1.08441766e-02, 9.03702693e-01] )


#打开笔记本摄像头
cap = cv2.VideoCapture(0)
font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below)

#num = 0
while True:
    ret, frame = cap.read()
    # operations on the frame come here

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
    parameters =  aruco.DetectorParameters_create()

    '''
    detectMarkers(...)
        detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI
        mgPoints]]]]) -> corners, ids, rejectedImgPoints
    '''

    #lists of ids and the corners beloning to each id
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,
                                                          aruco_dict,
                                                          parameters=parameters)

#    if ids != None:
    if ids is not None:

        rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, 0.05, mtx, dist)
        # Estimate pose of each marker and return the values rvet and tvec---different
        # from camera coeficcients
        (rvec-tvec).any() # get rid of that nasty numpy value array error

#        aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.1) #Draw Axis
#        aruco.drawDetectedMarkers(frame, corners) #Draw A square around the markers

        for i in range(rvec.shape[0]):
            aruco.drawAxis(frame, mtx, dist, rvec[i, :, :], tvec[i, :, :], 0.03)
            aruco.drawDetectedMarkers(frame, corners)
        #显示ID，rvec,tvec, 旋转向量和平移向量
        cv2.putText(frame, "Id: " + str(ids), (0,40), font, 0.5, (0, 0, 255),1,cv2.LINE_AA)
     #  cv2.putText(frame, "rvec: " + str(rvec[i, :, :]), (0, 60), font, 0.5, (0, 255, 0), 2, cv2.LINE_AA)
        cv2.putText(frame, "tvec: " + str(tvec[i, :, :]), (0,80), font, 0.5, (0, 0, 255), 1, cv2.LINE_AA)


    else:
        ##### DRAW "NO IDS" #####
        cv2.putText(frame, "No Ids", (0,64), font, 1, (0,255,0),2,cv2.LINE_AA)

    # Display the resulting frame
    cv2.imshow("frame",frame)

    key = cv2.waitKey(1)

    if key == 27:         # 按esc键退出
        print('esc break...')
        cap.release()
        cv2.destroyAllWindows()
        break

    if key == ord(' '):   # 按空格键保存
#        num = num + 1
#        filename = "frames_%s.jpg" % num  # 保存一张图像
        filename = str(time.time())[:10] + ".jpg"
        cv2.imwrite(filename, frame)

四、导入Aruco包有问题

1. 去cmd尝试卸载opencv-python 、opencv-contrib-python、

pip uninstall opencv-python
pip uninstall opencv-contrib-python

2. 下载低版本的opencv-contrib-python==4.3.0.36, 从清华镜像下载，在cmd输入以下命令：

pip install opencv-python==4.3.0.36  -i https://pypi.tuna.tsinghua.edu.cn/simple

参考文献：
主要参考博客园的一篇文章，并进行改动，我使用了笔记本摄像头，而作者使用外接摄像头。
链接：博客园的Aruco代码

引用请声明：
https://blog.csdn.net/luckydong156/article/details/128712678