【相机与图像】3. ARUCO的相关介绍、相机姿态估计

1 Aruco的引入

【姿态估计（Pose estimation）】
姿态估计（Pose estimation）在 计算机视觉领域扮演着十分重要的角色： 相机姿态估计、机器人导航、增强现实、三维重建：

• 机器人导航
  通过估计相机的姿态，可以确定机器人相对于环境的位置和朝向，从而帮助机器人进行路径规划和导航。
• 增强现实（AR）
  在增强现实应用中，相机姿态估计用于将虚拟对象与实际场景进行对齐。通过估计相机的姿态，可以根据相机的位置和朝向来确定虚拟对象的位置和姿态，从而实现虚拟对象与实际场景的融合。
• 三维重建
  在三维重建中，相机姿态估计用于确定多个相机之间的位置和朝向，从而实现对三维场景的重建。通过估计相机的姿态，可以将多个视角的图像融合起来，得到更准确的三维模型。
   

【ArUco标记的定义】
这需要找到现实世界和图像投影之间的对应点。这通常是很困难的，因此常常用自己制作的或基本的Marker来让这一切变得更容易。
最为流行的：基于二进制的ArUco标记，是可用于摄像机姿态估计的二进制方形基准标记。其基本组成为：

1. 外部宽黑色边框：有助于快速检测标记。
2. 内部二进制矩阵：Aruco 标记 都有一个固定的内部矩阵，由二进制编码组成，编码了标记的ID和其他可能的信息。使得每个标记都是独一无二的，可以通过解码其内部的二进制模式来识别。
    

总之，黑色边框有利于对图形标记的快速检测 来获取相机位姿，内部id使得算法健壮 允许它的标识和错误检测（鲁棒性强）。
 
【ArUco标记的特性】

1. 快速检测与识别：ArUco标记的检测过程相对简单且快速，能够在图像或视频中迅速定位和识别。
2. 鲁棒性强：内部二进制编码使得ArUco标记具有很强的鲁棒性，能够应对部分遮挡、光照变化等复杂环境。
   由于标记可能受到各种因素的影响（如部分遮挡、光照变化、相机视角变化等），因此它们必须能够容忍一定程度的损坏或错误。Aruco 标记的设计考虑到了这一点，通过在其编码中嵌入冗余信息或使用特定的编码方案，可以实现错误检测和纠正。
   当相机捕获到包含Aruco标记的图像时，检测算法会尝试解码标记中的二进制信息。如果检测到错误（例如，由于遮挡导致某些方块无法被正确识别），算法将利用编码中的冗余信息来尝试纠正这些错误，并恢复出正确的标记ID。
3. 易于创建与打印：用户可以根据需要自定义标记的尺寸和编码，并使用打印机轻松打印出来。
    

2 Aruco的细节

• Aruco 字典
  在利用opencv可生成Aruco Masker，总共有25个预定义的标记词典。每个词典中所有的Aruco标记均包含相同数量的块或位（4x4、5x5、6x6、7x7），且每个词典中Aruco标记的数量固定（50、100、250、1000）。
  例如，DICT_4X4_50 的具体含义：
  • 4x4：去除宽的黑边后，为4x4大小的网格。
    50：每个字典中的Marker数量，有效的id数字范围是0到49。不在有效区间的特定id将会产生异常。
     
    opencv的Aruco中具体字典如下：

  PREDEFINED_DICTIONARY_NAME = [
     DICT_4X4_50,
     DICT_4X4_100,
     DICT_4X4_250,
     DICT_4X4_1000,
     DICT_5X5_50,
     DICT_5X5_100,
     DICT_5X5_250,
     DICT_5X5_1000,
     DICT_6X6_50,
     DICT_6X6_100,
     DICT_6X6_250,
     DICT_6X6_1000,
     DICT_7X7_50,
     DICT_7X7_100,
     DICT_7X7_250,
     DICT_7X7_1000,
     DICT_ARUCO_ORIGINAL,  # DICT_5X5_1024
     DICT_APRILTAG_16h5,     #< 4x4 bits, minimum hamming distance between any two codes = 5, 30 codes
     DICT_APRILTAG_25h9,     #< 5x5 bits, minimum hamming distance between any two codes = 9, 35 codes
     DICT_APRILTAG_36h10,    #< 6x6 bits, minimum hamming distance between any two codes = 10, 2320 codes
     DICT_APRILTAG_36h11,    #< 6x6 bits, minimum hamming distance between any two codes = 11, 587 codes
  ]
  

• Aruco 的编码方式：
  内部 id 采用海明码的编码方式。即将整个标记 分成n*n的表格，黑色表示0，白色表示1。然后使用二进制的黑白格子来映射 id。另外，marker corner的顺序是起点为左上角，顺时针。
  
• 对应id的计算方法（内部编码的识别）：
  现将Masker内部（去除外圈黑边）的黑白方格转换成0/1矩阵，算法会提取候选框内部的二进制矩阵，并将其转换为十进制数，从而得到marker的ID。更具体的方法有机会补充

3 ARUCO 标记的使用

【使用方法】

1. 生成标记：用户可以使用OpenCV等库中的函数生成ArUco标记，并指定标记的尺寸、编码等参数。（如 3.1 所记录）
2. 打印标记：将生成的标记图像打印出来，并粘贴到需要识别的物体或场景中。
3. 检测与识别：使用相机捕获图像或视频，并使用相应的算法检测图像中的ArUco标记（如 3.2），解析其编码以获取ID和姿态信息（如 3.3）。

---

3.1 Masker 图的生成

一个在线的aruco 标记生成器：https://chev.me/arucogen/
opencv 中aruco 也提供了相关的接口。核心API：

• cv2.aruco.getPredefinedDictionary：定义具体使用 rauco字典中的哪一种masker。
• cv2.aruco.drawMarker：根据id等入参信息，生成对应的masker图。入参含义在代码备注中
   

具体的python代码如下：

import cv2 
import numpy as np

# step1：选择一个aruco 字典。这里使用的是预定义的字典之一，如DICT_4X4_250
# dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)

# step2：创建一个空白的图像，作为绘制标记的背景  
markerImage = np.zeros((200, 200), dtype=np.uint8)

# step3：调用drawMarker函数绘制标记  
# 参数：字典、ID、大小（边长）、图像、边框宽度（默认为1） 
markerImage = cv2.aruco.drawMarker(dictionary, 36, 200, markerImage, 1) ## marker_id=36

## step3.1: 
img_detected = np.ones((600, 600), dtype=np.uint8) * 255
img_detected[50:250, 50:250] = cv2.aruco.drawMarker(dictionary, id=23, sidePixels=200, borderBits=1)
img_detected[350:400, 350:400] = cv2.aruco.drawMarker(dictionary, id=18, sidePixels=50, borderBits=1)
img_detected[450:500, 450:500] = cv2.aruco.drawMarker(dictionary, id=33, sidePixels=50, borderBits=1)
# cv2.imshow("img_detected", img_detected)
# cv2.waitKey()

cv2.imwrite("marker36.png", markerImage)
cv2.imwrite("marker.png", img_detected )

生成图如下：

---

3.2 ARUCO标记 的检测

核心API：

• cv2.aruco.detectMarkers，用于检测图像中的aruco masker。入参（检测图片，masker从属的字典类型，相机参数(可选)）；返回（n个masker的四个焦点，n个masker的id，n个masker的）
   

具体示例如下：

import numpy as np
 
## step1: 加载用于生成标记的字典
# dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
 
## step2: 创建图像
img_detected = cv2.imread("detect2.jpg")
# img_color = cv2.cvtColor(img_detected, cv2.COLOR_BGR2GRAY)
 
## step3: 查找图像中所有已知字典中的ARUCO标记，并返回所有Masker的ID和对应的四个角点位置和 rejectedImgPoints（非标记的点）等
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image=img_detected ,
                                                         dictionary=dictionary,
                                                         parameters=None)

for corn, id in zip(corners, ids):
    print(id.shape)
    print(corn.shape)

## step4：可视化焦点和id
cv2.aruco.drawDetectedMarkers(img_detected, corners, ids)
cv2.imshow("DectectedMarker", img_detected)
cv2.waitKey()

注意，上例仅仅用于展示cv2.aruco.detectMarkers的使用，若要对 img_detected 进行姿态估计，则要匹配到对应合理的相机内参。


（从别人那薅过来的测试图纸，有污渍…）

---

3.3 ARUCO标记 的姿态估计

假定已知相机内参，使用摄像头完成一个实时动态监测aruco标记并且估计姿势。其核心API：

• cv2.aruco.getPredefinedDictionary
• cv2.aruco.detectMarkers
• cv2.aruco.estimatePoseSingleMarkers：入参为检出的aruco的角点、相机的内参、畸变参数、单个标记器的边长 (单位为米）。返回信息为 旋转平移矩阵：返回的旋转平移矩阵的坐标原点，是ArUco 标记的几何中心。这个几何中心是标记图案的中心点，通常位于标记的四个角点所构成的矩形的中心。。
   

具体示例如下：

import cv2
import numpy as np

dist_coeffs = np.array([[ 1.06812450e-01, -5.78128915e-01,  1.64633443e-03, 2.18013326e-04,  1.52013460e-01]])
inter_matrix = np.array([[713.82549508,   0.        , 330.5763196 ],
                        [  0.        , 713.96207005, 234.76984886],
                        [  0.        ,   0.        ,   1.        ]])

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
## 这里注意下，实际长度的设置，保证画面中的所有masker长度一致。这里为了验证流程第一张图多了个大尺寸的masker
markerLength = 0.5  # 单个标记器的大小(边长)，单位：米

while True:
    # _, frame = self.cap.read()
    frame = cv2.imread("detect1.jpg")
    corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(frame, aruco_dict)

    if ids is not None:
        cv2.aruco.drawDetectedMarkers(frame, corners, ids)

        for i in range(len(corners)):  ## corners center: [373, 360]
            ## rvecs：外参旋转矩阵，shape=(1,1,3)；
            ## tvecs：平移矩阵，shape=(1,1,3)；
            # 返回的旋转平移矩阵的坐标原点，是ArUco 标记的几何中心。
            rvecs, tvecs, obj_points = cv2.aruco.estimatePoseSingleMarkers(corners[i], markerLength=markerLength, cameraMatrix=inter_matrix, distCoeffs=dist_coeffs)
            cv2.drawFrameAxes(frame, inter_matrix, dist_coeffs, rvecs, tvecs, 0.1)
        cv2.imshow("show", frame)
        cv2.waitKey(1)