ArUco识别定位原理

1. ArUco是什么

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

2. 定位原理

整个过程是建立了一个以二维码为原点的局部坐标系。摄像头通过 ArUco 的特征位置，反向估算自身在这个局部坐标系中的位置和姿态。

ArUco坐标系

ArUco 码可以被看作一个标定好的平面，它的中心是坐标系的原点，且具有固定的朝向：

x: 沿着 ArUco 码的宽度方向（从左到右）。
y: 沿着 ArUco 码的高度方向（从下到上，通常垂直于地面向上）。
z: 垂直于 ArUco 码平面（从二维码面指向摄像头方向）。

当摄像头检测到 ArUco 码时：

检测到的 ArUco 四个角点的位置（图像像素坐标）经过相机标定矩阵和畸变参数的校正，可以用来计算 ArUco 的平移向量 和旋转向量 $r_{vec}$ 。
- $t_{vec}$ 表示摄像头的位置，即摄像头相对于 ArUco 坐标系的
- $r_{vec}$ 表示摄像头的姿态，即摄像头相对于 ArUco 坐标系的旋转。

计算结果 $t_{vec}$ 和 $r_{vec}$ 都是 std::vector<cv::Vec3d>，表示可能同时检测到多个码

$t_{vec}[0]=[0,0,z]$ 表示摄像头位于第一个ArUco的正上方，且高度为 $z$

$t_{vec}[0]=[x,y,z]$ 表示摄像头偏离第一个ArUco，偏移量为 $(x,y)$ , 高度为 $z$

3. opencv函数接口

3.1. 二维码检测

检测二维码接口实现见源码 opencv-4.7.0/modules/objdetect/src/aruco/aruco_detetector.cpp


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：输出参数，被拒绝的标记角点。这些角点未能形成有效的标记。
*/

源码解析

void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corners, OutputArray _ids,
OutputArrayOfArrays _rejectedImgPoints) const {// 确保输入的图像不为空CV_Assert(!_image.empty());// 获取检测器参数和字典，这些在内部实现中定义DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams;const Dictionary& dictionary = arucoDetectorImpl->dictionary;// 确保标记的边界尺寸有效（大于0）CV_Assert(detectorParams.markerBorderBits > 0);// 如果启用了Aruco3检测，确保其参数设置正确CV_Assert(!(detectorParams.useAruco3Detection == true &&detectorParams.minSideLengthCanonicalImg == 0 &&detectorParams.minMarkerLengthRatioOriginalImg == 0.0));// 将输入图像转换为灰度图像进行处理Mat grey;_convertToGrey(_image.getMat(), grey);// 如果不使用Aruco3功能，则将相关参数设置为默认值if (!detectorParams.useAruco3Detection) {detectorParams.minMarkerLengthRatioOriginalImg = 0.0;detectorParams.minSideLengthCanonicalImg = 0;}else {// 启用Aruco3功能时，总是使用亚像素级的角点细化方法detectorParams.cornerRefinementMethod = CORNER_REFINE_SUBPIX;// Todo：考虑以后支持其他角点细化方法}/// 步骤 0: 计算一个缩放因子 (fxfy)，用于后续检测const float fxfy = (!detectorParams.useAruco3Detection ? 1.f : detectorParams.minSideLengthCanonicalImg /(detectorParams.minSideLengthCanonicalImg + std::max(grey.cols, grey.rows) *detectorParams.minMarkerLengthRatioOriginalImg));/// 步骤 1: 创建图像金字塔，用于多尺度检测vector<Mat> grey_pyramid;int closest_pyr_image_idx = 0, num_levels = 0;步骤 1.1: 根据论文中的公式(1)调整图像尺寸if (detectorParams.useAruco3Detection) {const float scale_pyr = 2.f;const float img_area = static_cast<float>(grey.rows*grey.cols);const float min_area_marker = static_cast<float>(detectorParams.minSideLengthCanonicalImg*detectorParams.minSideLengthCanonicalImg);// 根据最小标记面积和图像面积计算金字塔的层数num_levels = static_cast<int>(log2(img_area / min_area_marker)/scale_pyr);// 找到最接近的金字塔图像索引const float scale_img_area = img_area * fxfy * fxfy;closest_pyr_image_idx = cvRound(log2(img_area / scale_img_area)/scale_pyr);}// 构建图像金字塔buildPyramid(grey, grey_pyramid, num_levels);// 如果需要缩放图像，则调整图像大小if (fxfy != 1.f)resize(grey, grey, Size(cvRound(fxfy * grey.cols), cvRound(fxfy * grey.rows)));/// 步骤 2: 检测标记候选区域vector<vector<Point2f> > candidates;vector<vector<Point> > contours;vector<int> ids;vector<vector<vector<Point2f> > > candidatesSet;vector<vector<vector<Point> > > contoursSet;/// 步骤 2.a 使用AprilTag方法检测候选标记 AprilTag是一个视觉基准库if(detectorParams.cornerRefinementMethod == CORNER_REFINE_APRILTAG){_apriltag(grey, detectorParams, candidates, contours);// 将检测到的候选标记和轮廓存入集合candidatesSet.push_back(candidates);contoursSet.push_back(contours);}/// 步骤 2.b 使用传统方式检测候选标记else_detectCandidates(grey, candidatesSet, contoursSet, detectorParams);/// 步骤 2: 检查候选标记的编码，识别标记_identifyCandidates(grey, grey_pyramid, candidatesSet, contoursSet, dictionary,candidates, contours, ids, detectorParams, _rejectedImgPoints);/// 步骤 3: 对角点进行亚像素级细化if (detectorParams.cornerRefinementMethod == CORNER_REFINE_SUBPIX) {// 确保亚像素细化的窗口大小、最大迭代次数和最小精度设置正确CV_Assert(detectorParams.cornerRefinementWinSize > 0 && detectorParams.cornerRefinementMaxIterations > 0 &&detectorParams.cornerRefinementMinAccuracy > 0);// 使用亚像素级精度进行角点细化parallel_for_(Range(0, (int)candidates.size()), [&](const Range& range) {const int begin = range.start;const int end = range.end;for (int i = begin; i < end; i++) {if (detectorParams.useAruco3Detection) {// 如果启用了Aruco3检测，基于金字塔图像进行角点细化const float scale_init = (float) grey_pyramid[closest_pyr_image_idx].cols / grey.cols;findCornerInPyrImage(scale_init, closest_pyr_image_idx, grey_pyramid, Mat(candidates[i]), detectorParams);}else {// 否则，使用传统的亚像素级方法cornerSubPix(grey, Mat(candidates[i]),Size(detectorParams.cornerRefinementWinSize, detectorParams.cornerRefinementWinSize),Size(-1, -1),TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,detectorParams.cornerRefinementMaxIterations,detectorParams.cornerRefinementMinAccuracy));}}});}/// 步骤 3， 可选: 使用轮廓容器进行角点细化if (detectorParams.cornerRefinementMethod == CORNER_REFINE_CONTOUR){if (!_ids.empty()) {// 使用轮廓细化每个检测到的标记的角点parallel_for_(Range(0, (int)candidates.size()), [&](const Range& range) {for (int i = range.start; i < range.end; i++) {_refineCandidateLines(contours[i], candidates[i]);}});}}// 如果没有启用亚像素细化并且缩放因子不为1，则将候选角点恢复到原始尺寸（注意这会影响精度）if (detectorParams.cornerRefinementMethod != CORNER_REFINE_SUBPIX && fxfy != 1.f) {// 目前只有CORNER_REFINE_SUBPIX方法正确实现了Aruco3的细化// Todo: 更新其他细化方法的实现// 将角点缩放回原始尺寸，这会导致检测精度下降for (auto &vecPoints : candidates)for (auto &point : vecPoints)point *= 1.f/fxfy;}// 将检测到的角点和标记ID复制到输出参数_copyVector2Output(candidates, _corners);Mat(ids).copyTo(_ids);
}

3.2. 位姿估计

具体实现见源码 opencv_contrib-4.7.0/modules/aruco/src/aruco.cpp

函数接口

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 标记的三维坐标。这是一个向量数组，每个元素包含标记的四个角点的三维坐标。*/

源码解析

void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength,InputArray _cameraMatrix, InputArray _distCoeffs,OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints,const Ptr<EstimateParameters>& estimateParameters) {// 确保标记的长度大于0CV_Assert(markerLength > 0);// 获取每个标记的物体点坐标（即标记在世界坐标系中的位置）Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength, *estimateParameters);// 获取标记的数量int nMarkers = (int)_corners.total();// 创建输出旋转向量和位移向量矩阵，分别为每个标记的旋转和平移信息_rvecs.create(nMarkers, 1, CV_64FC3);  // 创建旋转向量矩阵（每个标记一个旋转向量）_tvecs.create(nMarkers, 1, CV_64FC3);  // 创建位移向量矩阵（每个标记一个位移向量）// 将输出矩阵转换为Mat类型，以便进行后续操作Mat rvecs = _rvecs.getMat(), tvecs = _tvecs.getMat();对每个标记，计算其姿态（旋转和平移）parallel_for_(Range(0, nMarkers), [&](const Range& range) {const int begin = range.start;const int end = range.end;// 对每个标记执行PNP算法，计算标记的旋转和位移for (int i = begin; i < end; i++) {// solvePnP用于从标记的2D角点坐标和相机的内外参数估计3D姿态solvePnP(markerObjPoints, _corners.getMat(i), _cameraMatrix, _distCoeffs, rvecs.at<Vec3d>(i),tvecs.at<Vec3d>(i), estimateParameters->useExtrinsicGuess, estimateParameters->solvePnPMethod);}});// 如果需要输出物体点（objPoints），则将标记的物体点转换为输出格式if(_objPoints.needed()){markerObjPoints.convertTo(_objPoints, -1);  // 将物体点转换为需要的格式}
}

内部调用的是PnP方法求解位姿

PnP求解相机位姿

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags);
/**
*参数说明
*objectPoints：一个 vector<cv::Point3f>，包含了在世界坐标系中的三维点的坐标，至少需要4个点。
*imagePoints：一个 vector<cv::Point2f>，包含了对应的图像上的二维点的坐标，与 objectPoints 中的点一一对应。
*cameraMatrix：相机的内参数矩阵，类型为 cv::Mat，一般为 3x3 的浮点数矩阵。
*distCoeffs：相机的畸变系数，类型为 cv::Mat，一般为 4x1 或 5x1 的浮点数矩阵。
*rvec：输出的旋转向量，类型为 cv::Mat，是大小为 3x1 的浮点数矩阵。
*tvec：输出的平移向量，类型为 cv::Mat，是大小为 3x1 的浮点数矩阵。
*useExtrinsicGuess：一个布尔值，表示是否使用可选的旋转和平移向量的初始猜测。默认为 false。
*flags：一个用于控制函数行为的选项标志，默认为 0。
*函数返回：
*成功返回 true，失败返回 false。
*/

其中输入参数objectPoints就是世界坐标下（二维码坐标系下）四个角点的世界坐标，已知二维码的物理尺寸，所以生成一个得到四个角落的点直接坐标，即一个矩形框的四个点坐标，在源码中为markerObjPoints，对应的计算方法为：

static Mat _getSingleMarkerObjectPoints(float markerLength, const EstimateParameters& estimateParameters) {CV_Assert(markerLength > 0);Mat objPoints(4, 1, CV_32FC3);// set coordinate system in the top-left corner of the marker, with Z pointing outif (estimateParameters.pattern == ARUCO_CW_TOP_LEFT_CORNER) {objPoints.ptr<Vec3f>(0)[0] = Vec3f(0.f, 0.f, 0);objPoints.ptr<Vec3f>(0)[1] = Vec3f(markerLength, 0.f, 0);objPoints.ptr<Vec3f>(0)[2] = Vec3f(markerLength, markerLength, 0);objPoints.ptr<Vec3f>(0)[3] = Vec3f(0.f, markerLength, 0);}else if (estimateParameters.pattern == ARUCO_CCW_CENTER) {objPoints.ptr<Vec3f>(0)[0] = Vec3f(-markerLength/2.f, markerLength/2.f, 0);objPoints.ptr<Vec3f>(0)[1] = Vec3f(markerLength/2.f, markerLength/2.f, 0);objPoints.ptr<Vec3f>(0)[2] = Vec3f(markerLength/2.f, -markerLength/2.f, 0);objPoints.ptr<Vec3f>(0)[3] = Vec3f(-markerLength/2.f, -markerLength/2.f, 0);}elseCV_Error(Error::StsBadArg, "Unknown estimateParameters pattern");return objPoints;
}

与之对应的像素坐标四个点为_corners，是detectMarkers函数的计算结果值，有了世界坐标与像素坐标的四对点，可以计算出相机在世界坐标下的平移和旋转向量。

estimatePoseSingleMarkers函数最终得到的平移和旋转是以二维码中心点为原点的世界系在相机系下的位姿。
得到的tvecs是平移向量，rvecs是旋转向量，注意不是欧拉角也不是旋转矩阵不是四元数。
清楚了这个之后，现在只要知道无人机的位姿和摄像头在无人机的安装外参以及aruco二维码的检测结果（主要是平移向量），就可以自己直接推出二维码在无人机世界系下的位置。

注：PnP问题参考一文读懂PnP问题及opencv solvePnP、solvePnPRansac函数-CSDN博客

4. opencv生成二维码

首先我们创建aruco标记时，需要先指定一个字典，这个字典表示的是创建出来的aruco标记具有怎样的尺寸、怎样的编码等等内容。

通过在 aruco 模块中选择预定义的字典之一来创建对象。具体来说，此字典由250个标记和6x6位的标记大小(Dictionarycv::aruco::DICT_6X6_250)组成。

generateImageMarker的参数含义如下：

（1）创建的Dictionary对象;

（2）参数id：标记id，表示绘制字典中的哪一个aruco标记。每个字典由不同数量的标记组成，id有效范围是 [ 0，字典包含的标记数）（如cv::aruco::DICT_6X6_250，有效 ID 从 0 变为 249），任何超出有效范围的特定 id 都会产生异常。

（3）参数 200 是输出标记图像的大小。在这种情况下，输出图像的大小为 200x200 像素。请注意，此参数应足够大，以存储特定字典的位数。因此，例如，您不能为 5x5 位的标记大小生成 6x6 像素的图像（并且不考虑标记边界）。此外，为避免变形，此参数应与位数 + 边框大小成正比，或者至少远高于标记大小（如示例中的 200），以便变形微不足道。

（4）参数是输出图像。

（5）参数是可选参数，用于指定标记黑色边框的宽度。大小与位数成比例指定。例如，值 2 表示边框的宽度将相当于两个内部位的大小。默认值为 1。

#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/objdetect.hpp>
#include <iostream>void generateArucoMarker(int dictionaryId, int markerId, int sidePixels, const std::string &outputPath) {cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(dictionaryId);cv::Mat markerImage;cv::aruco::drawMarker(dictionary, markerId, sidePixels, markerImage);cv::imwrite(outputPath, markerImage);std::cout << "ArUco marker saved to: " << outputPath << std::endl;
}void generateCustomArucoMarkers(int markersX, int markersY, int markerSize, int sidePixels, const std::string &outputDir) {int totalMarkers = markersX * markersY;cv::Ptr<cv::aruco::Dictionary> customDictionary = cv::aruco::generateCustomDictionary(totalMarkers, markerSize);for (int markerId = 0; markerId < totalMarkers; ++markerId) {cv::Mat markerImage;cv::aruco::drawMarker(customDictionary, markerId, sidePixels, markerImage);std::string filename = outputDir + "/custom_marker_" + std::to_string(markerId) + ".png";cv::imwrite(filename, markerImage);std::cout << "Custom ArUco marker saved to: " << filename << std::endl;}
}void generateQRCode(const std::string &data, const std::string &outputPath) {try {cv::QRCodeEncoder::Params params; // 默认参数cv::QRCodeEncoder encoder(params);cv::Mat qrCode = encoder.encode(data);cv::imwrite(outputPath, qrCode);std::cout << "QR Code saved to: " << outputPath << std::endl;} catch (const std::exception &e) {std::cerr << "Error generating QR Code: " << e.what() << std::endl;}
}int main() {// 1. 使用预定义字典生成 ArUco 标记码int dictionaryId = cv::aruco::DICT_6X6_250;int markerId = 33;int sidePixels = 200;generateArucoMarker(dictionaryId, markerId, sidePixels, "aruco_marker.png");// 2. 生成自定义 ArUco 字典的标记码int markersX = 3, markersY = 3, markerSize = 6; // 自定义字典参数std::string customOutputDir = "./custom_markers";generateCustomArucoMarkers(markersX, markersY, markerSize, sidePixels, customOutputDir);// 3. 使用 OpenCV 生成标准二维码std::string qrData = "Hello, ArUco and QR Code!";generateQRCode(qrData, "qrcode.png");return 0;
}

opencv检测二维码并定位

// 从旋转矩阵提取欧拉角（单位：度数）
cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat &R) {double sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));bool singular = sy < 1e-6; // 检查奇异情况double x, y, z;if (!singular) {x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));y = atan2(-R.at<double>(2, 0), sy);z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));} else {x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));y = atan2(-R.at<double>(2, 0), sy);z = 0;}//输出结果为[roll, pitch, yaw]return cv::Vec3d(x, y, z) * 180.0 / CV_PI; // 转换为度数
}// 遍历可用的摄像头，返回第一个可用摄像头的 ID
int findAvailableCamera() {int maxTestedCameras = 1000;for (int cameraId = 0; cameraId < maxTestedCameras; ++cameraId) {cv::VideoCapture tempCap(cameraId);if(cameraId % 10 == 0){std::cout << "Testing camera ID: " << cameraId << std::endl;}if (tempCap.isOpened()) {tempCap.release();return cameraId;}}return -1; // 未找到可用摄像头
}void drawAxis(cv::Mat &image, const cv::Vec3d &rvec, const cv::Vec3d &tvec, const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs) {float axisLength = 0.1f; // 坐标轴长度（单位：米）cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, axisLength);
}int main() {// 找到可用的摄像头int cameraId = findAvailableCamera();if (cameraId == -1) {std::cerr << "Error: No available camera found!" << std::endl;return -1;}std::cout << "Using camera ID: " << cameraId << std::endl;// 打开摄像头cv::VideoCapture cap(cameraId);if (!cap.isOpened()) {std::cerr << "Error: Cannot open the camera!" << std::endl;return -1;}// ArUco 配置int dictionaryId = cv::aruco::DICT_6X6_250;float markerLength = 0.1f; // ArUco 码的物理尺寸（单位：米）cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(dictionaryId);cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();// 相机校准参数（替换为实际的内参）cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1000, 0, 640, 0, 1000, 360, 0, 0, 1);cv::Mat distCoeffs = (cv::Mat_<double>(1, 5) << 0, 0, 0, 0, 0); // 无畸变假设cv::Mat frame;while (true) {cap >> frame; // 获取当前帧if (frame.empty()) {std::cerr << "Error: Captured empty frame!" << std::endl;break;}// ArUco 检测std::vector<int> markerIds;std::vector<std::vector<cv::Point2f>> markerCorners;std::vector<cv::Vec3d> rvecs, tvecs; // 旋转和平移向量cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds, parameters);if (!markerIds.empty()) {// 估计姿态, rvecs, tvecs是相机在二维码坐标下的旋转向量和平移向量cv::aruco::estimatePoseSingleMarkers(markerCorners, markerLength, cameraMatrix, distCoeffs, rvecs, tvecs);// 绘制检测到的 ArUco 码边框cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);// 可视化每个 ArUco 码的坐标轴for (size_t i = 0; i < markerIds.size(); ++i) {drawAxis(frame, rvecs[i], tvecs[i], cameraMatrix, distCoeffs);// 将旋转向量转换为旋转矩阵cv::Mat R;cv::Rodrigues(rvecs[i], R);// 从旋转矩阵提取欧拉角cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R);// 在图像上显示位置信息std::string positionText = "Pos: [" + std::to_string(tvecs[i][0]) + ", " +std::to_string(tvecs[i][1]) + ", " +std::to_string(tvecs[i][2]) + "]";cv::putText(frame, positionText, markerCorners[i][0], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 2);// 在图像上显示旋转信息std::string rotationText = "Rot: [" + std::to_string(eulerAngles[0]) + ", " +std::to_string(eulerAngles[1]) + ", " +std::to_string(eulerAngles[2]) + "]";cv::putText(frame, rotationText, markerCorners[i][0] + cv::Point2f(0, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 0, 0), 2);}}// 显示实时检测结果cv::imshow("Real-Time ArUco Detection", frame);// 按下 ESC 键退出if (cv::waitKey(30) == 27) break;}cap.release();cv::destroyAllWindows();return 0;
}