aruco二维码检测与aruco二维码降落原理讲解 20240422

aruco二维码检测讲解视频

aruco二维码降落讲解视频

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/
 

下下来的二维码SVG文件可以用 https://svgtopng.com/zh/ 这个网站转为png格式图片。

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是旋转向量，注意不是欧拉角也不是旋转矩阵不是四元数。
清楚了这个之后，现在只要知道无人机的位姿和USB摄像头在无人机的安装朝向以及aruco二维码的检测结果（主要是平移向量），就可以自己直接推出二维码在无人机世界系下的位置，可以直接推出，不用自己事先拿个摄像头边看检测结果边推了。

#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里

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

二维码降落中的坐标系

变换矩阵的说明
以下面这个变换矩阵为例，一个变换矩阵可以有两种理解方式，可以理解为机体坐标系在世界坐标系下的位姿，最右边的写法可以体现这个含义，也可以说是从机体系到世界系的变换矩阵，最左边的写法可以体现这个含义，因为变换矩阵右边乘的是变换前的坐标系坐标，左边得到的是变换后的坐标系坐标，所以写为Twb正好顺序一致，念的时候也是先念b再念w。
 

二维码降落中的坐标系变换
 

 

基于无人机对的世界系位姿和aruco二维码检测结果即aruco二维码在相机系中的位姿，可以最终解算出aruco二维码在世界系中的位姿。

Tbc的计算过程

Tbc的含义,可以理解为相机坐标系在机体系下的位姿即旋转和平移。是从相机系到机体系的变换矩阵，注意是相机系到机体系不是机体系到相机系。
确定Rbc和tbc
对于坐标系间的旋转角度是90度的倍数时，可以直接观察坐标轴的对应关系或者取坐标轴的特定坐标值的方式快速确定坐标系间的旋转矩阵。默认基于左乘，位置为列向量。
 

tbc 这里默认(0 , 0 , 0) ,顺序依次对应x y z

代码实现关键Eigen

变换矩阵用Eigen::Isometry3d类型
旋转矩阵用Eigen::Matrix3d类型
平移向量用Eigen::Vector3d类型
四元数用Eigen::Quaterniond类型

https://blog.csdn.net/u011092188/article/details/77430988

以下代码示例取自aruco_land_demo.cpp

Eigen::Isometry3d Twa,Tca,Tbc,Twb,Twc;  //变换矩阵

Eigen::Matrix3d rotation_matrix_bc,rotation_matrix_wb,rotation_matrix_ca;;

Eigen::Vector3d t_bc,t_wb,t_ca;

Eigen::Quaterniond q_wb,q_ca;

Vector3d类型和Matrix3d类型的直接赋值

    t_bc <<  0, 0, 0;
    rotation_matrix_bc << 0, -1, 0,
                         -1 , 0, 0,
                          0, 0, -1;

四元数转旋转矩阵

rotation_matrix_ca = q_ca.toRotationMatrix();

基于旋转矩阵和平移向量给变换矩阵赋值

            Tca.linear() = rotation_matrix_ca;
            Tca.translation() = t_ca;

取出变换矩阵里的平移向量

Vector3d t_wa = Twa.translation();

访问平移向量的某一项

Vector3d t_wa;
t_wa[0];
t_wa.x();

变换矩阵相乘

Twa = Twb * Tbc * Tca;

二维码降落的gazebo仿真

gazebo中几个基本概念：world model plugin
https://zhuanlan.zhihu.com/p/131315474?utm_id=0
在.world文件中可以配置好环境信息以及各种model信息。而model就是需要插入到环境中的模型，具体配置是由.sdf文件和其他相关文件决定的，很方便自己编写和定制。而如果要操控model，就需要编写plugin来进行控制，都是cpp的脚本，配置好sdf中的后在其中的Load函数中写好控制流程即可。
plugin可以让模型绑定ros消息，实现传感器的仿真输出（比如摄像头，激光雷达，IMU），电机的控制输入。

二维码降落的gazebo仿真需要构建好带有二维码的world和带有下视单目摄像头的无人机模型。

无人机gazebo仿真 = 实飞程序 + mavros + posix_sitl.launch

posix_sitl.launch = px4 + empty_world.launch + spawn_model(无人机模型)
原始的posix_sitl.launch可以通过roscd px4/launch的路径下找到并查看。

empty_world.launch中开启了Gazebo的服务器和客户端两个节点，原始的empty_world.launch可以在gazebo_ros功能包中查看（roscd gazebo_ros/launch）。

二维码模型的位置可以在world文件的中更改,比如这里的aruco_landing_pad.world，二维码大小可以在二维码模型中更改，比如这里的aruco_landing_pad.sdf。二维码图片自己也可以灵活替换，比如二维码图片可以存放于这里的offboard_pkg/models_for_worlds/aruco_landing_pad/materials/textures文件夹下，并把offboard_pkg/models_for_worlds/aruco_landing_pad/materials/scripts/aruco_landing_pad.material里的图片文件名称改为对应的图片文件即可。

下视摄像头，其实就是用单目摄像头模型，并使其位姿垂直朝下，offboard_pkg/models/iris_downward_camerairis_downward_camera.sdf中确定摄像头的位姿，下面这个位姿态代表绕Y轴（东北天坐标系的Y轴）正转90度，正好摄像头朝下，便为下视摄像头。

      <uri>model://monocular_camera</uri>
      <pose>0 0 -0.05 0 1.5707963 0</pose>

跑px4 gazebo仿真之前会添加一些路径，就是为了三个，找到px4功能包，找到model，找到plugin。所以路径也就是这三种路径。
world不用添加路径因为launch文件里面会直接给world文件的绝对路径让launch文件可以找到world文件，world里面是model组成的，world里面的model和无人机model里的model以及有的model用到的plugin需要能自动找到。
添加路径举例

export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$HOME/src/sitl_gazebo/build
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$HOME/src/sitl_gazebo/models