在上一篇【Originbot探索发现③】AprilTag探索 中，从AprilTag码入手，分析了AprilTag码工具包的安装使用，主要原理以及标定原理，本节将继续探索，首先针对Originbot小车的相机进行标定，然后去通过AprilTag码获取平面的法向量。

1. 获取Originbot相机内参

具体操作步骤：

• 首先下载一个棋盘格的图打印下来，我这里提供一个 棋盘格.pdf，点开就能下载

• 平铺在桌面上，用Originbitd的相机对其拍n张不同角度的照片，下面给一个快速查看Originbot图像的方式，参考相机驱动与图像可视化：
  SSH连接OriginBot成功后，在终端中输入如下指令：

ros2 launch websocket hobot_websocket.launch.py

• 然后同一网络的PC打开浏览器，网址栏输入Originbit的IP，点击web展示端，加可以看到图像了，然后直接截图

• 将截图的照片放在同一个文件夹内

• 运行标定程序读到相机参数

下面直接放代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time     : 2023/6/10 13:37
# @Author   : Chenan_Wang
# @File     : calib.py
# @Project  : charging 
# @Software : PyCharm

import numpy as np
import cv2 as cv
import glob

criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((8 * 5, 3), np.float32)
objp[:, :2] = np.mgrid[0:8, 0:5].T.reshape(-1, 2)

# 用于存储所有图像中的对象点和图像点的数组。
obj_points = []     # 世界坐标系中的3d点
img_points = []     # 图像坐标系中的2d点

images = glob.glob("C:/Users/***/Desktop/image/*jpg")    # 修改存放路径
win_name = "Verify"
cv.namedWindow(win_name, cv.WND_PROP_FULLSCREEN)
cv.setWindowProperty(win_name, cv.WND_PROP_FULLSCREEN, cv.WINDOW_FULLSCREEN)

print("getting images")
for f_name in images:
    img = cv.imread(f_name)
    print(f_name)
    gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
    # 找到棋盘角
    ret, corners = cv.findChessboardCorners(gray, (8, 5), None)
    if ret:
        obj_points.append(objp)
        # 第三个为搜索窗口的一半尺寸。若为(5,5)，那就表示用 (5*2+1)×(5*2+1)=11×11 大小的搜索窗口。
        corners2 = cv.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)
        img_points.append(corners)
        # 绘制并显示角
        cv.drawChessboardCorners(img, (8, 5), corners2, ret)
        cv.imshow(win_name, img)
        cv.waitKey(500)
cv.destroyAllWindows()

size = gray.shape[::-1]
ret, cam_mtx, dist, rvecs, tvecs = cv.calibrateCamera(obj_points, img_points, size, None, None)
print(">==> Starting calibration")
print("ret:", ret)          # 所有校正点偏差距离平方和
print("mtx:\n", cam_mtx)    # 内参数矩阵K
print("dist:\n", dist)      # 畸变系数  distortion coefficients = (k_1, k_2, p_1, p_2, k_3)
print("rvecs:\n", rvecs)    # 旋转向量 (外参数)
print("tvecs:\n", tvecs)    # 平移向量 (外参数)

cv.waitKey()
cv.destroyAllWindows()

期间会看到如图所示的标定过程

最后得到如下的输出：

拿到内矩阵参数矩阵K：

mtx = np.array([[1421.94379, 0.0, 1904.90777],
                [0.0, 1431.00638, 1013.22526],
                [0.0, 0.0, 1.0]])

2. 估算平面法向量

接下来查看法向量，这样就可以通过二维的AprilTag码推算出相机（Originbot小车与贴有码的充电桩的相对位置了）

先通过刚刚截图的手段，用Originbot获取一张AprilTag码的图片，如下

同样放出完整代码，将刚刚标定得到的相机内参填入进去

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time     : 2023/6/11 15:06
# @Author   : Chenan_Wang
# @File     : AprilTag_Test.py
# @Project  : charging
# @Software : PyCharm

import pupil_apriltags as apriltag  # Windows
# import apriltag     # Ubuntu
import cv2 as cv
import numpy as np
from matplotlib import pyplot as plt
from scipy.spatial.transform import Rotation

img = cv.imdecode(
    np.fromfile('C:/Users/INTEL/Desktop/AprilTag_img/008.png', dtype=np.uint8),
    cv.IMREAD_COLOR)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

at_detector = apriltag.Detector(families='tag25h9')  # Windows
# at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11 tag25h9'))    # Ubuntu
tags = at_detector.detect(gray)

for tag in tags:
    # print('tag', tag)
    for i in range(4):
        cv.circle(img, tuple(tag.corners[i].astype(int)), 4, (255, 0, 0), 3)
    cv.circle(img, tuple(tag.center.astype(int)), 4, (255, 0, 0), 5)

# 获取单应矩阵
homo = tags[0].homography
# 相机内参矩阵
mtx = np.array([[1421.94379, 0.0, 1904.90777],
                [0.0, 1431.00638, 1013.22526],
                [0.0, 0.0, 1.0]])
# 单应矩阵分解
num, Rs, Ts, Ns = cv.decomposeHomographyMat(homo, mtx)
print("num: {}".format(num), "\nRs: {}".format(Rs), "\nTs: {}".format(Ts), "\nNs: {}".format(Ns))

angle_dim = []
r = Rotation.from_matrix(Rs[1].T)
euler_angle = r.as_euler('xyz').T * 180 / np.pi
angle_dim.append(euler_angle[2])

dir_angle = (euler_angle[2] - 5) * np.pi / 180 * 1.8
ARROW_LENGTH = 120
delta_x = np.sin(dir_angle) * ARROW_LENGTH
delta_y = ARROW_LENGTH / 2 * np.cos(dir_angle)
new_center = tags[0].center + np.array([delta_x, delta_y])

cv.line(img, tuple(new_center.astype(int)), tuple(tags[0].center.astype(int)), (255, 0, 0), 2)  # 画出法向量
cv.circle(img, tuple(new_center.astype(int)), 4, (0, 0, 255), 5)    # 法向量方向标红圈

cv.imshow('img', img)
cv.waitKey()
cv.destroyAllWindows()

输出如下：

num: 4 
Rs: (array([[-0.08762995,  0.35394522,  0.93115185],
       [-0.77831311, -0.60773327,  0.15776239],
       [ 0.62173121, -0.71090298,  0.32873584]]), array([[-0.08762995,  0.35394522,  0.93115185],
       [-0.77831311, -0.60773327,  0.15776239],
       [ 0.62173121, -0.71090298,  0.32873584]]), array([[ 0.51211893,  0.71833206,  0.47088561],
       [-0.72576777,  0.65509626, -0.21002389],
       [-0.45934229, -0.23419639,  0.85682945]]), array([[ 0.51211893,  0.71833206,  0.47088561],
       [-0.72576777,  0.65509626, -0.21002389],
       [-0.45934229, -0.23419639,  0.85682945]])) 
Ts: (array([[ 0.63170677],
       [ 1.28788424],
       [-0.00364431]]), array([[-0.63170677],
       [-1.28788424],
       [ 0.00364431]]), array([[ 0.57120038],
       [-0.23733832],
       [-1.29426164]]), array([[-0.57120038],
       [ 0.23733832],
       [ 1.29426164]])) 
Ns: (array([[ 0.19462906],
       [ 0.91219589],
       [-0.36058034]]), array([[-0.19462906],
       [-0.91219589],
       [ 0.36058034]]), array([[-0.83473401],
       [ 0.37089171],
       [ 0.40701163]]), array([[ 0.83473401],
       [-0.37089171],
       [-0.40701163]]))

可以看到有四种可能的结果。

查看输出图片，看到最终效果如下：

实际法向量差不多就是向上的，基本正确，具体精度和偏移量需要后续进一步的计算

如果将代码中的：

img = cv.imdecode(
    np.fromfile('C:/Users/INTEL/Desktop/AprilTag_img/008.png', dtype=np.uint8),
    cv.IMREAD_COLOR)

修改为摄像头的视频流信息，设定好帧率，就可以通过视频信息，将AprilTag码在Originbot前面不同角度测试，获取实时的效果了。