aruco二维码检测原理详解与基于opencv的代码实现（自己详细整理）

aruco二维码介绍

aruco又称为aruco标记、aruco标签、aruco二维码等，其中 CharucoBoard GridBoard AprilTag 原理相通，只是生成字典不同，而AprilTag用于机器人领域或可编程摄像头比较多，而aruco CharucoBoard GridBoard则用于AR应用或智能眼镜比较多，都是用于实现精准定位，如无人机降落时就可以使用它实现精准降落。

ArUco marker是一种汉明码方格图。它由一个宽的黑边和一个内部的二进制矩阵组成，黑色的边界有利于快速检测到图像，Marker ID是他的二进制矩阵编码，Marker size是图片的大小。黑色方块对应0，白色方块对应1。一个二维码就是一个矩阵。

原始aruco二维码汉明码解码举例
 

总共是7*7的方格，除去最外层的黑色边框，是5*5，其中奇数列是校验位，偶数列是数据位，所以第1 3 5列为校验位，2 4列为数据位，提取出数据位为

0 0
0 0
0 1
1 1
0 1

每行首尾相接整理得：0000011101 转为十进制是16+8+4+1=29

aruco二维码生成网站
https://chev.me/arucogen/

opencv包含这些预先定义好的二维码字典，最后检测得到的ID好对应字典里的索引。上面的二维码选用的字典是
https://docs.opencv.org/3.4/d9/d6a/group__aruco.html

大小二维码叠加
白色方格内黑色占比小于二分之一，则还是当做白色方格，所以可以叠加小二维码，而不影响大二维码检测。
如下图所示的大小重叠二维码，小二维码的存在也是不影响大二维码识别的，因为中间方格内的小二维码，黑色占比大于二分之一，所以检测大二维码时，小二维码依旧被当作黑色方格。

二维码检测

相机模型

世界系 -> 相机系 -> 成像平面坐标系 -> 像素坐标系

成像平面坐标系和像素坐标系的区别：成像平面坐标系是相机感光元件上的实际物理坐标系，而像素坐标系是数字图像中的抽象坐标系，用于表示像素的位置，成像平面坐标系的单位是长度单位，而像素坐标系的单位是像素。成像平面坐标系到像素坐标系就是一个离散化的过程。

下式中 Puv

为像素坐标， Pw 为世界系坐标， T 为相机系和世界系间的外参，即相机外参， K

为归一化相机系到像素坐标系的外参即相机内参。

Puv=KTPw

下式中 P

为相机系下的坐标， 1ZP 为归一化相机系下的坐标，归一化相机系通过相机内参矩阵转到像素坐标系。
⎛⎝⎜uv1⎞⎠⎟=1Z⎛⎝⎜fx000fy0cxcy1⎞⎠⎟⎛⎝⎜XYZ⎞⎠⎟≜1ZKP

在计算机图形学和计算机视觉中，通常采用的坐标系是以左上角为原点，水平向右为 x 轴正方向，垂直向下为 y 轴正方向的坐标系。这种方向的选择符合我们在屏幕上观察图像时的习惯：像素从左上角开始计数，水平方向增加表示向右移动，垂直方向增加表示向下移动。像素坐标系，成像平面坐标系和相机坐标系都是如此。
看下面这张图时，谈左上角时，注意这个视角朝向，不同视角的左是不同的，需要人前方视角和相机前方是一致的情况下，再看左上角。

关于aruco二维码检测时的相机系和世界系

这里的坐标系都是右手坐标系，包括相机系和aruco世界系。

对于旋转，estimatePoseSingleMarkers函数得到的是旋转向量。
旋转向量其实想表示的就是绕着某个旋转轴转了某个角度，该向量的方向就是旋转轴，它的模就是绕轴逆时针旋转的角度(这里逆时针依照右手定则，也可以这么说旋转轴正方向面对观察者时，逆时针方向的旋转是正、顺时针方向的旋转是负)。
旋转向量没有欧拉角直观，可以借助这个网站把旋转向量转为欧拉角 https://www.andre-gaschler.com/rotationconverter/

https://zhuanlan.zhihu.com/p/144032401?from=singlemessage
欧拉角正负：
如果是右手系，旋转轴正方向面对观察者时，逆时针方向的旋转是正、顺时针方向的旋转是负。
亦可这样描述：使用右手的大拇指指向旋转轴正方向，其他4个手指在握拳过程中的指向便是正方向。

https://blog.csdn.net/yuewei19/article/details/53023992/
需要注意的是，欧拉角的表示方式里，yaw、pitch、roll的顺序对旋转的结果是有影响的。给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的。
对于x,y,z三个轴的不同旋转顺序一共有(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)六种组合，在旋转角度相同的情况下不同的旋转顺序得到的姿态是不一样的。

给出一组欧拉角,绕x,y,z三个轴的转角分别为(α,β,γ),我们不能能确定一个明确的姿态.需要再追加两个属性:(1)旋转顺序(2)内旋/外旋.才能确定的给出这组欧拉角对应的姿态。（ https://blog.csdn.net/whl0071/article/details/126738369 ）

这里使用内旋，X-Y-Z的顺序分析欧拉角。

内旋示意：
https://img-blog.csdn.net/20160423193023033

在像素坐标系中，图像的原点通常位于左上角，x 轴的正方向是向右，y 轴的正方向是向下。因此，相机坐标系的定义与像素坐标系的定义一致，这样可以简化计算和理解。

二维码中心点为世界系原点，红色为世界系x，绿色为世界系y，蓝色为世界系z。

平移维度分析
输出的平移向量的单位是米

旋转分析

旋转向量和欧拉角的转换，可以用下面这个网站。
https://www.andre-gaschler.com/rotationconverter/
estimatePoseSingleMarkers函数得到的旋转向量里面值的单位是弧度。

 

基于opencv写aruco二维码检测

cv::aruco::detectMarkers(image_, dictionary_, corners, ids, detectorParams_, rejected);
参数：
(1)image :输入的需要检测标记的图像。 
(2)dictionary :进行检测的字典对象指针，这里的字典就是我们创建aruco 标记时所使用的字典，检测什么类型的aruco 标记就使用什么类型的字典。 
(3)corners :输出参数，检测到的aruco 标记的角点列表，是一个向量数组，每个元素对应一个检测到的标记，每个标记有四个角点，其四个角点均按其原始顺序返回 (从左上角开始顺时针旋转)。 
(4)ids:输出参数，检测到的每个标记的id,需要注意的是第三个参数和第四个参数具有相同的大小。 
(5)parameters:ArUco 检测器的参数。是一个 cv::aruco::DetectorParameters 类型的对象，用于设置检测器的各种参数，例如边缘阈值、最小标记区域等。
(6)rejectedImgPoints：输出参数，被拒绝的标记角点。这些角点未能形成有效的标记。  
 

https://docs.opencv.org/3.4/d9/d6a/group__aruco.html#ga061ee5b694d30fa2258dd4f13dc98129

cv::aruco::estimatePoseSingleMarkers(corners, markerLength, intrinsic_matrix_, distortion_matrix_, rvecs, tvecs, _objPoints);
参数：
(1)corners :detectMarkers ()返回的检测到标记的角点列表，里面每个元素都是一个浮点型向量，表示检测到的标记的四个角点的坐标，向量的顺序通常是左上、右上、右下和左下。； 
(2)markerLength :aruco 标记的实际物理尺寸，也就是打印出来的aruco标记的实际尺寸，以m为单位； 
(3)intrinsic_matrix_ :相机的内参矩阵;
(4)distortion_matrix_ :相机的畸变参数;
(5)rvecs : 标记相对于相机的旋转向量。 
(6)tvecs : 标记相对于相机的平移向量。 
(7)_objPoints :可选参数，用于返回每个 ArUco 标记的三维坐标。这是一个向量数组，每个元素包含标记的四个角点的三维坐标。  
 

https://docs.opencv.org/3.4/d9/d6a/group__aruco.html#ga896ca24f0c1b4b277b6e59d5fe001dd5
 

estimatePoseSingleMarkers函数里面就是调用的solvePnP函数
modules/aruco/src/aruco.cpp · wanghanlin/opencv_contrib - Gitee.com

PNP求解相机位姿

已知几个点在世界系下的坐标和归一化相机系下的坐标，求这两个坐标系的变换矩阵，也就是旋转和平移。

输入：二维码边长，可以得到世界系的坐标，基于像素坐标和畸变参数，可以得到去畸变后的像素坐标，去畸变后的像素坐标和相机内参可以得到归一化相机系下的坐标系。
输出：世界系和归一化相机系间的平移向量和旋转矩阵，即，相机外参也即是相机在世界系下的位姿。

estimatePoseSingleMarkers函数最终得到的平移和旋转是以二维码中心点为原点的世界系在相机系下的位姿。
得到的tvecs是平移向量，rvecs是旋转向量，注意不是欧拉角也不是旋转矩阵不是四元数。

#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
​
int main()
{
    // 定义ArUco字典和参数
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
    cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
​
    // 相机标定参数
    double fx = 406.932130;
    double fy = 402.678201;
    double cx = 316.629381;
    double cy = 242.533947;
​
    double k1 = 0.039106;
    double k2 = -0.056494;
    double p1 = -0.000824;
    double p2 = 0.092161;
    double k3 = 0.0;
​
    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 
    fx, 0, cx,
    0, fy, cy,
    0, 0, 1);
​
    cv::Mat distCoeffs = (cv::Mat_<double>(5, 1) << k1, k2, p1, p2, k3);
​
​
    // 打开相机
    cv::VideoCapture cap(0);
    if (!cap.isOpened())
    {
        std::cout << "无法打开相机" << std::endl;
        return -1;
    }
​
    while (true)
    {
        // 读取相机帧
        cv::Mat frame;
        cap >> frame;
​
        // 转换为灰度图像
        cv::Mat gray;
        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
​
        // 检测ArUco二维码
        std::vector<int> markerIds;
        std::vector<std::vector<cv::Point2f>> markerCorners, rejectedCandidates;
        cv::aruco::detectMarkers(gray, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
​
        // 绘制检测结果
        if (markerIds.size() > 0)
        {
            cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
​
            // 估计相机姿态
            std::vector<cv::Vec3d> rvecs, tvecs;
            cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);
​
            // 绘制相对位姿
            for (int i = 0; i < markerIds.size(); ++i)
            {
                cv::aruco::drawAxis(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
            }
        }
​
        // 显示图像
        cv::imshow("ArUco Detection", frame);
​
        // 按下 'q' 键退出循环
        if (cv::waitKey(1) == 'q')
        {
            break;
        }
    }
​
    // 释放资源
    cap.release();
    cv::destroyAllWindows();
​
    return 0;
}
​

相机标定

用ros自带的camera_calibration功能包(sudo apt-get install ros-melodic-camera-calibration)进行标定

一个可以在线生成棋盘格的网站，大小和行列数可配：https://calib.io/pages/camera-calibration-pattern-generator

操作可以参考此篇博客单目相机标定

rosrun camera_calibration cameracalibrator.py --size 11x8 --square 0.02 image:=/usb_cam/image_raw

--size 11x8 用于指明标定板的内角点数量，如下图每个红圈的位置就是一个内角点，我所使用的GP290标点板有横向有11个内角点，纵向有8个内角点。需要注意的时11x8中的‘x’时小写的英文字母‘x’，不是数学符号‘*’。
 

--square 0.02指明标定板中每个方块的边长0.02m，即2cm，根据你所使用标定板实际尺寸修改
image:=/usb_cam/image_raw，指明图像话名。根据你实际使用的相机驱动节点产生的话题名做修改

标定板的移动可以参考此视频Calibrating a Monocular Camera with ROS
X:标定板在摄像头视野中的左右移动
Y:标定板在摄像头视野中的上下移动
Size:标定板在摄像头视野中的前后移动
Skew:标定板在摄像头视野中的倾斜转动
 

标定完成终端会打印标定结果，包括相机内参和畸变参数
 

将相机内参和畸变参数填到camera.yaml里

标定结果检验
把二维码的检测结果中的平移向量和实际距离（可以用尺子衡量）进行对比