使用电脑摄像头计算aruco marker位姿（Python）_python 如何计算aruco码中心点与相机中心点的距离

一、效果图

刚做了一些尝试，算两个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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28