关于Aruco标记的理解并对其进行姿态估计的可视化显示

ArUco二维码标定板自动生成网站：
https://chev.me/arucogen/
可以选择Aruco码、棋盘格标定板、圆形标定板、非对称圆形标定板进行打印
https://calib.io/pages/camera-calibration-pattern-generator

1.什么是ArUco标记

  ArUco标记最初由S.Garrido-Jurado等人在2014年发表的论文Automatic generation and detection of highly reliable fiducial markers under occlusion中提出。ArUco的全称是Augmented Reality University of Cordoba，下面给出ArUco标记的一些示例。

  ArUco标记是可用于摄像机姿态估计的二进制方形基准标记。它的主要优点是检测简单、快速，并且具有很强的鲁棒性。ArUco 标记是由宽黑色边框和确定其标识符（id）的内部二进制矩阵组成的正方形标记。ArUco标记的黑色边框有助于其在图像中的快速检测，内部二进制编码用于识别标记和提供错误检测和纠正。ArUco标记尺寸的大小决定内部矩阵的大小，例如尺寸为 4x4 的标记由 16 位二进制数组成。
  ArUco标记的尺寸可以任意的更改，为了成功检测可根据对象大小和场景选择合适的尺寸。在实际使用中，如果标记的尺寸太小，可能无法检测到它，这时可以选择更换较大尺寸的标记，或者将相机离标记更近一些。

  在机器人应用中，可以将这些标记沿着仓库机器人的路径放置。当安装在机器人上的摄像头检测到这些标记时，由于每个标记都有唯一的ID，并且且标记在仓库中的放置位置已知，因此就可以知道机器人在仓库中的精确位置。

  通俗地说，Aruco标记其实就是一种编码，就和我们日常生活中的二维码是相似的，只不过由于编码方式的不同，导致它们存储信息的方式、容量等等有所差异，所以在应用层次上也会有所不同。由于单个ArUco标记就可以提供足够的对应关系，例如有四个明显的角点及内部的二进制编码，所以ArUco标记被广泛用来增加从二维世界映射到三维世界时的信息量，便于发现二维世界与三维世界之间的投影关系，从而实现姿态估计、相机矫正等等应用。

1.1 Marker和字典

  一个ArUco marker是一个二进制平方标记，它由一个宽的黑边和一个内部的二进制矩阵组成，内部的矩阵决定了它们的id。黑色的边界有利于快速检测到图像，二进制编码可以验证id，并且允许错误检测和矫正技术的应用。marker的大小决定了内部矩阵的大小。例如，一个4x4的marker由16bits组成。

2.通过使用OpenCV生成ArUco标记图

  本小节通过使用OpenCV库生成上小节中的ArUco标记图，方便打印后为后面做姿态估计。

  使用OpenCV可轻松生成这些标记。OpenCV中的Aruco模块总共有25个预定义的标记词典。每个词典中所有的Aruco标记均包含相同数量的块或位(例如4×4、5×5、6×6或7×7)，且每个词典中Aruco标记的数量固定(例如50、100、250或1000)。接下来将展示如何在C++和Python中生成和检测各种aruco标记。

  调用getPredefinedDictionary()函数加载包含250个标记的字典，其中每个标记都是6×6位二进制模式。具体代码在下面给出。

C++代码

#include 
 
Mat markerImage; 
// 加载用于生成标记的字典
Ptr<cv::aruco::Dictionary>dictionary=aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); 
// 生成标记图
aruco::drawMarker(dictionary, 33, 200, markerImage, 1);

Python代码

import cv2 as cv
import numpy as np
 
# 加载用于生成标记的字典
dictionary = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
 
# Generate the marker
markerImage = np.zeros((200, 200), dtype=np.uint8)
markerImage = cv.aruco.drawMarker(dictionary, 33, 200, markerImage, 1);
 
cv.imwrite("marker33.png", markerImage);

  代码中drawMarker函数可以从由250个aruco标记组成的集合中选择给定id(第二个参数– 33)的标记，这250个标记的id由0~249表示。drawMarker函数的第三个参数决定生成的标记的大小，在上面的示例中，它将生成200×200像素的图像。第四个参数表示将要存储aruco标记的对象(上面的markerImage）。最后，第五个参数是边界宽度参数，它决定应将多少位（块）作为边界添加到生成的二进制图案中。

  在上面的代码中，将在6×6生成的图形周围添加1位的边界，以在200×200像素的图像中生成7×7位的图像。上述代码生成的aruco标记如下图所示。

  在实际应用时，我们可能需要生成多个标记。之后我们只需要将这些标记打印出来就可以直接使用了。

3.检测Aruco标记

  将aruco标记放置在环境中后，我们需要检测它们并将其用于进一步处理。接下来介绍如何通过代码检测标记。
C++代码

// 加载用于生成标记的字典
Ptr<Dictionary> dictionary = getPredefinedDictionary(DICT_6X6_250);
 
// 使用默认值初始化检测器参数  
Ptr<DetectorParameters> parameters = DetectorParameters::create();
 
// 声明包含检测到的标记角和被拒绝的标记候选的向量  
vector<vector<Point2f>> markerCorners, rejectedCandidates;
 
// 检测到的标记的id存储在一个向量中  
vector<int> markerIds;
 
// 检测图像中的标记
detectMarkers(frame, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);

Python代码

# 加载用于生成标记的字典
dictionary = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
 
# 使用默认值初始化检测器参数 
parameters =  cv.aruco.DetectorParameters_create()
 
# 检测图像中的标记
markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(frame, dictionary, parameters=parameters)

  对于每次成功检测到标记，将按从左上，右上，右下和左下的顺序检测标记的四个角点。在C ++中，将这4个检测到的角点存储为点矢量，并将图像中的多个标记一起存储在点矢量容器中。在Python中，它们存储为Numpy 数组。

4.一些API的介绍

  姿态估计问题就是要确定某个三维物体的方位指向问题，也就是确定以该物体为中心原点的一个坐标系。

getPredefinedDictionary()

auto dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_6X6_250);

• 关于DICT_4X4_50含义
    以我的运行环境opencv3.4.7和opencv_contrib为例，其中DICT是dictionary的缩写，6×6表示去掉一个宽的黑边后的网格大小，如下图所示是6×6的，蓝色的矩形框所示。
  50表示每个字典中的Marker数目，有效的id数字范围是0到49。不在有效区间的特定id将会产生异常。
  
    上图为一个典型的ArUco marker，去除黑色边框后为6X6的格子（黑色表示0，白色表示1），6X6的格子的外边缘为黑色。

id的计算方法：
1 0 0 1 1 0
1 0 0 0 1 1
0 0 0 0 0 1
0 0 0 1 1 1
1 1 0 0 1 1
0 1 1 1 1 0

每行组成一个二进制数，将这6个二进制数进行相加转换为十进制，就是id。

enum PREDEFINED_DICTIONARY_NAME {
    DICT_4X4_50 = 0,
    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_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
};

注: marker corner的顺序是起点为左上角, 顺时针
注: DICT_ARUCO_ORIGINAL: 为DICT_5X5_1024

drawMarker()

• 函数作用：detectMarkers函数用于检测和确定标记角点的位置。

cv::Mat markerImage; 
cv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

cv::aruco::drawMarker(dictionary, 23, 200, markerImage, 1);

  首先，我们通过选择aruco模块中一个预定义的字典来创建一个字典对象，具体而言，这个字典是由250个marker组成的，每个marker的大小为6x6bits(DICT_6X6_250)

drawMarker的参数如下：

• 第一个参数是之前创建的字典对象（带有标记的场景图像）。
• 第二个参数是marker的id（用于生成标记的字典），成功检测到的标记将存储在markerCorners中，在这个例子中选择的是字典DICT_6X6_250第23个marker。注意到每个字典是由不同数目的Marker组成的，在这个例子中，有效的Id数字范围是0到249。不在有效区间的特定id将会产生异常。
• 第三个参数，200，是输出Marker图像的大小。在这个例子中，输出的图像将是200x200像素大小。注意到这一参数需要满足能够存储特定字典 的所有位。所以，举例而言，你不能为6x6大小的marker生成一个5x5图像（这还没有考虑到Marker的边界）。除此之外，为了避免变形，这一参数最好和位数+边界的大小成正比，至少要比marker的大小大得多（如这个例子中的200)，这样变形就不显著了。
• 第四个参数是输出的图像。
• 最后一个参数是一个可选的参数，它指定了Marker黑色边界的大小。这一大小与位数数目成正比。例如，值为2意味着边界的宽度将会是2的倍数。默认的值为1。
  生成的图像如下：
  

estimatePoseSingleMarkers()

cv::aruco::estimatePoseSingleMarkers(v_marker_corner, marker_size, cameraMatrix, distCoeffs, rvecs, tvecs);

• 函数作用：估计marker在相机坐标系中的3D位姿。
  参数：
• 第一个参数：
• 第二个参数：marker的尺寸
• 第三个参数：相机的内参矩阵
• 第四个参数：相机的畸变系数
• 第五个参数：输出marker坐标系到相机坐标系的旋转变换矩阵R
• 第六个参数：输出marker坐标系到相机坐标系的旋转变换矩阵T
  注: marker坐标系用marker的位置有关, z轴向外。

5.ArUco模块之aruco标记的创建与检测

特别感谢这篇博文：
OpenCV4学习笔记（72）——ArUco模块之aruco标记的创建与检测

6.对aruco标记进行姿态估计的可视化显示

void detectPoseShow()
{
    // step 1: 加载当前搭载相机的内参矩阵和畸变系数
    cv::Mat cameraMatrix, distCoeffs;
    std::vector<double> camera = { 5421.4770, 0, 1268.5471, 0, 5443.9587,
                                   900.4128,  0, 0,         1 };
    cameraMatrix = cv::Mat(camera);
    cameraMatrix = cameraMatrix.reshape(1, 3);
    std::vector<double> dist = { -0.07785, -0.92021, -0.00303, 0.000363,
                                 31.54394 };
    distCoeffs = cv::Mat(dist);
    distCoeffs = distCoeffs.reshape(1, 1);

    cv::Mat src_image = cv::imread("aruco.bmp");

    // step 2: 对标记图像都进行aruco标记的检测以及姿态估计
    cv::Mat test_image;
    cv::resize(src_image, test_image, cv::Size(800, 600));
    cv::imshow("test_image", test_image);
    auto dictionary = cv::aruco::getPredefinedDictionary(
        cv::aruco::PREDEFINED_DICTIONARY_NAME::DICT_4X4_50);
    std::vector<std::vector<cv::Point2f>> corners, rejectedImgPoints;
    std::vector<int> ids;
    auto parameters = cv::aruco::DetectorParameters::create();
    cv::aruco::detectMarkers(test_image, dictionary, corners, ids, parameters,
                             rejectedImgPoints);
    cv::aruco::drawDetectedMarkers(test_image, corners, ids,
                                   cv::Scalar(0, 255, 0));

    std::vector<cv::Vec3d> rvecs;
    std::vector<cv::Vec3d> tvecs;
    cv::aruco::estimatePoseSingleMarkers(corners, 0.053, cameraMatrix,
                                         distCoeffs, rvecs, tvecs);

    // step 3: 绘制坐标轴并进行可视化显示
    for (int i = 0; i < rvecs.size(); i++) {
        cv::aruco::drawAxis(test_image, cameraMatrix, distCoeffs, rvecs[i],
                            tvecs[i], 0.02);
    }
    cv::imshow("pose", test_image);
}

效果：

个人觉得不错的博文：
增强现实应用：https://blog.csdn.net/sinat_17456165/article/details/105649131
OpenCV4学习笔记（74）——ArUco模块之对aruco标记进行实时姿态估计
OpenCV4学习笔记（76）——基于ArUco模块+QT实现增强现实（AR）
https://blog.csdn.net/weixin_45224869/article/month/2020/05
OpenCV官方文档：https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html
使用opencv的aruco库进行位姿估计：https://blog.csdn.net/weixin_43053387/article/details/86301547
视觉标记定位aruco使用：https://blog.csdn.net/sinat_16643223/article/details/114252467
marker中的id计算：https://blog.csdn.net/sinat_16643223/article/details/114261925
ArUco使用：https://blog.csdn.net/sinat_16643223/article/details/115212935
对于ArUco标记的检测，并将坐标轴可视化：https://blog.csdn.net/qq_53457019/article/details/125811861