首页分享工作笔记系列01：AprilTag详解

工作笔记系列01：AprilTag详解

来源：花匠小妙招时间：2024-12-27 16:27

1.介绍

2.aprilTag原理

3. PnP（Perspective-n-Point）算法

4. 安装apriltag

5.AprilTag系统的核心组件

6.功能封装和详细使用代码（C++）

①aprilTag_operate.h

②aprilTag_operate.cpp

③main(程序调用)

7.总结

1.介绍

AprilTag 是一种方形的黑白标签，内部有独特的编码模式，类似于二维码。每个标签有一个独特的二进制编码，保证在不同位置、角度下都能准确识别。标签的四边为黑色框，内部的二进制模式用于编码。

AprilTag 是一种二维平面视觉标记系统，专门为计算机视觉应用设计，能够在复杂的光照条件下稳定检测，适合在机器人导航、视觉伺服等场景下使用。AprilTag 提供了非常精确的位姿估计，可以检测出标签的平移和旋转信息，并通过标定算法计算摄像机与标签之间的空间关系。

AprilTag 最常见的类型是 tag36h11，其中：

36 表示标签编码是36位的。h11 表示最小的汉明距离为11，用来增强抗噪能力。

汉明距离：表明一个信号变成另一个信号需要的最小操作（替换位），实际中就是比较两个比特串有多少个位不一样，简洁的操作时就是两个比特串进行异或之后包含1的个数

AprilTag 的主要功能是：

标签检测：识别图像中的AprilTag。标签ID：每个AprilTag都有一个唯一的ID。位姿估计：根据标签在图像中的位置，计算相对于摄像机的平移和旋转。

AprilTag的优势：

能够在不同角度、光照下进行稳定识别。精确的位姿估计能力，适合机器人视觉任务。编码密度高（可以容纳更多标签）。

2.aprilTag原理

AprilTag 的检测可以分为以下几个步骤：

边缘检测：通过边缘检测算法（例如 Canny 边缘检测），从图像中找到可能的候选方形区域。顶点检测：从边缘检测结果中检测到四边形的顶点。透视变换：利用四个顶点进行透视变换，将检测到的四边形恢复为标准的正方形。这是因为标签在三维空间中可能有角度倾斜，通过透视变换可以校正图像。解码：将透视变换后的图像转化为二进制矩阵，根据编码模式解析出标签的 ID。姿态估计：一旦 AprilTag 被检测并识别出 ID，接下来可以通过标签的顶点信息和摄像头内参（相机标定参数）计算标签在三维空间中的位置和姿态。具体来说，可以通过PnP（Perspective-n-Point）算法，结合已知的摄像头参数和标签的真实尺寸，推导出标签相对于摄像机的 6 个自由度（位置和姿态）。

3. PnP（Perspective-n-Point）算法

PnP（Perspective-n-Point）算法是计算机视觉和摄影测量中的一个常见问题，其主要目的是通过已知的3D点的坐标及其在2D图像中的投影，来估计摄像机的位置和姿态。

输入：已知的 3D 点坐标和它们在图像中的 2D 投影坐标，以及摄像机的内参矩阵。

输出：摄像机的旋转矩阵 R和平移向量 t，描述摄像机的姿态（即摄像机在世界坐标系中的位置和姿态）。

OpenCV提供了函数：

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。（0：迭代优化方法，使用非线性最小二乘优化来最小化图像空间中的投影误差。虽然这些方法计算较慢，但可以获得更高的精度）

函数返回：成功返回 true，失败返回 false。

注意：输入的【Xi，Yi，Zi】是特征点在世界坐标系中的坐标，【ui，vi】是该3D点在图像上的投影，则输出的R，t则为相机在世界坐标系下的位姿。若此处若输入的【Xi，Yi，Zi】是特征点在标定板坐标系中的坐标（世界坐标系与标定板坐标系重合），则输出的R，t则为相机在标定板坐标系下的位姿。

4. 安装apriltag

源码下载路径：GitHub - AprilRobotics/apriltag: AprilTag is a visual fiducial system popular for robotics research.

编译教程：apriltag在windows下使用msvc进行安装_apriltag window-CSDN博客

补充：不需要python可以把选项【BUILD_PYTHON_WRAPPER】去掉勾选。

文章头部链接提供编译好的windows-x64 apriltag库

5.AprilTag系统的核心组件

1. apriltag_family_t:

功能：定义了AprilTag标签系列的特定结构，每个标签系列（如tag36h11）都有唯一的标识码结构。作用：用于识别特定类型的AprilTag。常见函数： tag36h11_create()：创建tag36h11标签系列实例。tag36h11_destroy()：销毁tag36h11标签系列实例。

apriltag_family_t *tf = tag36h11_create();

apriltag_detector_add_family(td, tf);

tag36h11_destroy(tf);

2. apriltag_detector_t:

功能：AprilTag检测器，用于处理图像并检测其中的AprilTag。作用：负责接收输入图像，处理并返回检测到的标签。成员变量：

quad_decimate：缩小图像以提高检测速度。默认值是1.0，增加该值会缩小图像。

quad_sigma：控制高斯模糊，减少噪声。默认值是0.0，如果图像噪声大，可以尝试设置较高的值（如0.8）。

refine_edges：是否精确查找标签边缘。启用后会提高检测精度，但会降低速度。

nthreads：控制使用多线程进行并行处理，提高处理速度。

常用函数：apriltag_detector_create()：创建一个AprilTag检测器实例。apriltag_detector_add_family()：向检测器中添加标签系列（如tag36h11）apriltag_detector_detect()：输入图像并检测AprilTag，返回检测结果。apriltag_detector_destroy()：销毁AprilTag检测器，释放资源。

#include "apriltag.h"

#include "tag36h11.h"

#include "common/getopt.h"

apriltag_detector_t *td = apriltag_detector_create();

td->quad_decimate = 1.0;

td->quad_sigma = 0.0;

td->refine_edges = 1;

apriltag_family_t *tf = tag36h11_create();

apriltag_detector_add_family(td, tf);

cv::Mat GrayImage = cv::imread(path,-1);

image_u8_t im{ (int32_t)GrayImage.cols, (int32_t)GrayImage.rows, (int32_t)GrayImage.cols, (uint8_t*)GrayImage.data };

zarray_t *detections = apriltag_detector_detect(td, &im);

for (int i = 0; i < zarray_size(detections); i++) {

apriltag_detection_t *det;

zarray_get(detections, i, &det);

printf("标签ID: %dn", det->id);

}

apriltag_detections_destroy(detections);

apriltag_detector_destroy(td);

tag36h11_destroy(tf);

3. apriltag_detection_t:

功能：包含检测到的AprilTag的详细信息，包括标签的ID、位置等。成员变量：

id：标签的唯一ID。

c[2]：标签中心的图像坐标，c[0] 是 x 坐标，c[1] 是 y 坐标。

p[4][2]：标签四个角点的图像坐标。

apriltag_detection_t *det;

zarray_get(detections, i, &det);

printf("标签ID: %dn", det->id);

printf("标签中心: (%f, %f)n", det->c[0], det->c[1]);

for (int j = 0; j < 4; j++) {

printf("角点 %d: (%f, %f)n", j, det->p[j][0], det->p[j][1]);

}

4. zarray_t:

功能：一种通用的动态数组结构，用于存储检测结果。作用：存储多个检测到的标签。常见函数： zarray_size()：返回数组的大小（即检测到的标签数量）。zarray_get()：获取数组中的某个元素。

zarray_t* detections = apriltag_detector_detect(td, &im);

int num_detections = zarray_size(detections);

6.功能封装和详细使用代码（C++）

①aprilTag_operate.h

#pragma once

#include <opencv2/opencv.hpp>

#include <string>

extern "C" {

#include "apriltag.h"

#include "common/getopt.h"

}

#ifndef PI

#define PI 3.14159265358979323846264338

#endif

class CAprilTagDetection

{

public:

CAprilTagDetection();

~CAprilTagDetection();

int init(std::string _family = "tag36h11", double _decimate = 2.0, int _threads = 1, int _quiet = 0, int _refine_edges = 1, float _blur = 0.0, int _debug = 0);

int detectNew(cv::Mat GrayImage, std::vector<std::vector<cv::Point2f>>* pvvp2fCorners, std::vector<cv::Point2f>* pvvp2fCenters, std::vector<std::string>* pvsCode, cv::Mat* pshowImage);

int detect(cv::Mat GrayImage, std::vector<std::vector<cv::Point2f>>* pvvp2fCorners = NULL, std::vector<std::string>* pvsCode = NULL, cv::Mat* pshowImage = NULL);

int calculatePose(float fSideLength, const std::vector<std::vector<cv::Point2f>>& vvp2fCorners, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs, std::vector<cv::Mat>* pvHomMat3DPaperInCam, cv::Mat* pshowImage = NULL);

inline bool CreatHomMatFromRT(const cv::Mat& R, const cv::Mat& T, cv::Mat& HomoMtr);

inline bool CreatHomMatFromRVT(const cv::Mat& Rvec, const cv::Mat& Tvec, cv::Mat& HomMat3d);

cv::Mat ScaleAbsChange(cv::Mat img, float alpha, int beta);

double normalizeAngle(double angle);

bool isRotatedMatrix(const cv::Mat& R);

bool extractHomMtr2RT(const cv::Mat& HomoMtr, cv::Mat& R, cv::Mat& T);

bool rotationMatrixToEulerAngles(const cv::Mat& R, cv::Vec3d& eulerAngle);

inline cv::Vec3d eulerToAngle(const cv::Vec3d& radians) {

return radians * (180.0 / PI);

}

inline cv::Vec3d eulerToRadian(const cv::Vec3d& degrees) {

return degrees * (PI / 180.0);

}

cv::Vec3d getSymmetricEulerAngles(const cv::Vec3d& euler_angles);

cv::Vec3d eulerAngleTranMini(const cv::Vec3d& euler_angles);

private:

std::string family = "tag36h11";

double decimate = 1.0;

int threads = 1;

int quiet = 0;

int refine_edges = 1;

float blur = 0.0;

int debug = 0;

apriltag_family_t* tf = NULL;

apriltag_detector_t* td = NULL;

};

②aprilTag_operate.cpp

#include"aprilTag_operate.h"

#include <iostream>

#include <iomanip>

#include "opencv2/opencv.hpp"

extern "C" {

#include "apriltag.h"

#include "tag36h11.h"

#include "tag25h9.h"

#include "tag16h5.h"

#include "tagCircle21h7.h"

#include "tagCircle49h12.h"

#include "tagCustom48h12.h"

#include "tagStandard41h12.h"

#include "tagStandard52h13.h"

#include "common/getopt.h"

}

using namespace std;

using namespace cv;

CAprilTagDetection::CAprilTagDetection() {

init("tag36h11");

}

CAprilTagDetection::~CAprilTagDetection() {

if (td != NULL)

{

apriltag_detector_destroy(td);

}

if (tf != NULL)

{

const char* famname = family.c_str();

if (!strcmp(famname, "tag36h11")) {

tag36h11_destroy(tf);

}

else if (!strcmp(famname, "tag25h9")) {

tag25h9_destroy(tf);

}

else if (!strcmp(famname, "tag16h5")) {

tag16h5_destroy(tf);

}

else if (!strcmp(famname, "tagCircle21h7")) {

tagCircle21h7_destroy(tf);

}

else if (!strcmp(famname, "tagCircle49h12")) {

tagCircle49h12_destroy(tf);

}

else if (!strcmp(famname, "tagStandard41h12")) {

tagStandard41h12_destroy(tf);

}

else if (!strcmp(famname, "tagStandard52h13")) {

tagStandard52h13_destroy(tf);

}

else if (!strcmp(famname, "tagCustom48h12")) {

tagCustom48h12_destroy(tf);

}

int CAprilTagDetection::init(std::string _family, double _decimate, int _threads, int _quiet, int _refine_edges, float _blur, int _debug)

{

family = _family;

decimate = _decimate;

threads = _threads;

quiet = _quiet;

refine_edges = _refine_edges;

blur = _blur;

debug = _debug;

TickMeter meter;

meter.start();

const char* famname = family.c_str();

if (!strcmp(famname, "tag36h11")) {

tf = tag36h11_create();

}

else if (!strcmp(famname, "tag25h9")) {

tf = tag25h9_create();

}

else if (!strcmp(famname, "tag16h5")) {

tf = tag16h5_create();

}

else if (!strcmp(famname, "tagCircle21h7")) {

tf = tagCircle21h7_create();

}

else if (!strcmp(famname, "tagCircle49h12")) {

tf = tagCircle49h12_create();

}

else if (!strcmp(famname, "tagStandard41h12")) {

tf = tagStandard41h12_create();

}

else if (!strcmp(famname, "tagStandard52h13")) {

tf = tagStandard52h13_create();

}

else if (!strcmp(famname, "tagCustom48h12")) {

tf = tagCustom48h12_create();

}

else {

printf("Unrecognized tag family name. Use e.g. "tag36h11".n");

return 1;

}

td = apriltag_detector_create();

apriltag_detector_add_family(td, tf);

if (errno == ENOMEM) {

printf("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder with the default maximum hamming value of 2. Try choosing an alternative tag family.n");

return 1;

}

td->quad_decimate = decimate;

td->quad_sigma = blur;

td->nthreads = threads;

td->debug = debug;

td->refine_edges = refine_edges;

meter.stop();

cout << "Detector " << famname << " initialized in "

<< std::fixed << std::setprecision(3) << meter.getTimeSec() << " seconds" << endl;

}

int CAprilTagDetection::detectNew(cv::Mat GrayImage, std::vector<std::vector<cv::Point2f>>* pvvp2fCorners, std::vector<cv::Point2f>* pvvp2fCenters, std::vector<std::string>* pvsCode, cv::Mat* pshowImage)

{

#if 0

image_u8_t im = { .width = gray.cols,

.height = gray.rows,

.stride = gray.cols,

.buf = gray.data

};

#else

image_u8_t im{ (int32_t)GrayImage.cols, (int32_t)GrayImage.rows, (int32_t)GrayImage.cols, (uint8_t*)GrayImage.data };

#endif

zarray_t* detections = apriltag_detector_detect(td, &im);

if (errno == EAGAIN) {

printf("Unable to create the %d threads requested.n", td->nthreads);

}

std::vector<std::vector<Point2f>>vvp2fCorners(zarray_size(detections));

std::vector<std::string>vsCode(zarray_size(detections));

for (int i = 0; i < zarray_size(detections); i++) {

apriltag_detection_t* det;

zarray_get(detections, i, &det);

std::vector<Point2f>& vp2fCorners = vvp2fCorners[i];

vp2fCorners.resize(4);

vp2fCorners[0].x = det->p[0][0];

vp2fCorners[0].y = det->p[0][1];

vp2fCorners[1].x = det->p[1][0];

vp2fCorners[1].y = det->p[1][1];

vp2fCorners[2].x = det->p[2][0];

vp2fCorners[2].y = det->p[2][1];

vp2fCorners[3].x = det->p[3][0];

vp2fCorners[3].y = det->p[3][1];

cv::Point2f center;

center.x = det->c[0];

center.y = det->c[1];

pvvp2fCenters->push_back(center);

stringstream ss;

ss << det->id;

String text = ss.str();

vsCode[i] = text;

if (pshowImage != NULL)

{

cv::Mat& frame = *pshowImage;

line(frame, Point(det->p[0][0], det->p[0][1]),

Point(det->p[1][0], det->p[1][1]),

Scalar(0, 0xff, 0), 2);

line(frame, Point(det->p[0][0], det->p[0][1]),

Point(det->p[3][0], det->p[3][1]),

Scalar(0, 0, 0xff), 2);

line(frame, Point(det->p[1][0], det->p[1][1]),

Point(det->p[2][0], det->p[2][1]),

Scalar(0xff, 0, 0), 2);

line(frame, Point(det->p[2][0], det->p[2][1]),

Point(det->p[3][0], det->p[3][1]),

Scalar(0xff, 0, 0), 2);

int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX;

double fontscale = 1.0;

int baseline;

Size textsize = getTextSize(text, fontface, fontscale, 2,

&baseline);

putText(frame, text, Point(det->c[0] - textsize.width / 2,

det->c[1] + textsize.height / 2),

fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

if (1)

{

putText(frame, "0", Point(det->p[0][0], det->p[0][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

putText(frame, "1", Point(det->p[1][0], det->p[1][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

putText(frame, "2", Point(det->p[2][0], det->p[2][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

putText(frame, "3", Point(det->p[3][0], det->p[3][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

}

if (pvvp2fCorners != NULL) *pvvp2fCorners = std::move(vvp2fCorners);

if (pvsCode != NULL) *pvsCode = std::move(vsCode);

apriltag_detections_destroy(detections);

return 0;

}

int CAprilTagDetection::detect(cv::Mat GrayImage, std::vector<std::vector<cv::Point2f>>* pvvp2fCorners, std::vector<std::string>* pvsCode, cv::Mat* pshowImage)

{

#if 0

image_u8_t im = { .width = gray.cols,

.height = gray.rows,

.stride = gray.cols,

.buf = gray.data

};

#else

image_u8_t im{ (int32_t)GrayImage.cols, (int32_t)GrayImage.rows, (int32_t)GrayImage.cols, (uint8_t*)GrayImage.data };

#endif

zarray_t* detections = apriltag_detector_detect(td, &im);

if (errno == EAGAIN) {

printf("Unable to create the %d threads requested.n", td->nthreads);

}

std::vector<std::vector<Point2f>>vvp2fCorners(zarray_size(detections));

std::vector<std::string>vsCode(zarray_size(detections));

for (int i = 0; i < zarray_size(detections); i++) {

apriltag_detection_t* det;

zarray_get(detections, i, &det);

std::vector<Point2f>& vp2fCorners = vvp2fCorners[i];

vp2fCorners.resize(4);

vp2fCorners[0].x = det->p[0][0];

vp2fCorners[0].y = det->p[0][1];

vp2fCorners[1].x = det->p[1][0];

vp2fCorners[1].y = det->p[1][1];

vp2fCorners[2].x = det->p[2][0];

vp2fCorners[2].y = det->p[2][1];

vp2fCorners[3].x = det->p[3][0];

vp2fCorners[3].y = det->p[3][1];

stringstream ss;

ss << det->id;

String text = ss.str();

vsCode[i] = text;

if (pshowImage != NULL)

{

cv::Mat& frame = *pshowImage;

line(frame, Point(det->p[0][0], det->p[0][1]),

Point(det->p[1][0], det->p[1][1]),

Scalar(0, 0xff, 0), 2);

line(frame, Point(det->p[0][0], det->p[0][1]),

Point(det->p[3][0], det->p[3][1]),

Scalar(0, 0, 0xff), 2);

line(frame, Point(det->p[1][0], det->p[1][1]),

Point(det->p[2][0], det->p[2][1]),

Scalar(0xff, 0, 0), 2);

line(frame, Point(det->p[2][0], det->p[2][1]),

Point(det->p[3][0], det->p[3][1]),

Scalar(0xff, 0, 0), 2);

int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX;

double fontscale = 1.0;

int baseline;

Size textsize = getTextSize(text, fontface, fontscale, 2,

&baseline);

putText(frame, text, Point(det->c[0] - textsize.width / 2,

det->c[1] + textsize.height / 2),

fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

if (1)

{

putText(frame, "0", Point(det->p[0][0], det->p[0][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

putText(frame, "1", Point(det->p[1][0], det->p[1][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

putText(frame, "2", Point(det->p[2][0], det->p[2][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

putText(frame, "3", Point(det->p[3][0], det->p[3][1]), fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

}

if (pvvp2fCorners != NULL) *pvvp2fCorners = std::move(vvp2fCorners);

if (pvsCode != NULL) *pvsCode = std::move(vsCode);

apriltag_detections_destroy(detections);

return 0;

}

int CAprilTagDetection::calculatePose(float fSideLength, const std::vector<std::vector<cv::Point2f>>& vvp2fCorners, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs, std::vector<cv::Mat>* pvHomMat3DPaperInCam, cv::Mat* pshowImage)

{

float fW = fSideLength;

std::vector<Point3f> vp3fRealPointSet(4);

vp3fRealPointSet[0].x = 0;

vp3fRealPointSet[0].y = 0;

vp3fRealPointSet[0].z = 0;

vp3fRealPointSet[1].x = fW;

vp3fRealPointSet[1].y = 0;

vp3fRealPointSet[1].z = 0;

vp3fRealPointSet[2].x = fW;

vp3fRealPointSet[2].y = fW;

vp3fRealPointSet[2].z = 0;

vp3fRealPointSet[3].x = 0;

vp3fRealPointSet[3].y = fW;

vp3fRealPointSet[3].z = 0;

std::vector<cv::Mat> vHomMat3DPaperInCam(vvp2fCorners.size());

for (int i = 0; i < vvp2fCorners.size(); i++)

{

Mat matPaperInCamRvec, matPaperInCamTvec;

const std::vector<Point2f>& vp2fCorners = vvp2fCorners[i];

cv::solvePnP(vp3fRealPointSet, vp2fCorners, matCameraMatrix, matDistCoeffs, matPaperInCamRvec, matPaperInCamTvec);

cv::Mat HomMat3DPaperInCam;

CreatHomMatFromRVT(matPaperInCamRvec, matPaperInCamTvec, HomMat3DPaperInCam);

vHomMat3DPaperInCam[i] = HomMat3DPaperInCam;

}

if (pvHomMat3DPaperInCam != NULL) *pvHomMat3DPaperInCam = std::move(vHomMat3DPaperInCam);

return 0;

}

bool CAprilTagDetection::CreatHomMatFromRT(const cv::Mat& R, const cv::Mat& T, cv::Mat& HomoMtr)

{

HomoMtr = cv::Mat::eye(4, 4, CV_64F);

if (R.rows != 3 || R.cols != 3)

{

std::cerr << "[ERROR] CreatHomMatFromRT: Rotation matrix R must be 3x3, but got " << R.rows << "x" << R.cols << "." << std::endl;

return false;

}

if (T.rows != 3 || T.cols != 1)

{

std::cerr << "[ERROR] CreatHomMatFromRT: Translation vector T must be 3x1, but got " << T.rows << "x" << T.cols << "." << std::endl;

return false;

}

R.copyTo(HomoMtr(cv::Range(0, 3), cv::Range(0, 3)));

T.copyTo(HomoMtr(cv::Range(0, 3), cv::Range(3, 4)));

return true;

};

bool CAprilTagDetection::CreatHomMatFromRVT(const cv::Mat& Rvec, const cv::Mat& Tvec, cv::Mat& HomMat3d)

{

if (Rvec.rows != 3 || Rvec.cols != 1)

{

std::cerr << "[ERROR] HomMat3dFromRVT: Rotation vector Rvec must be 3x1, but got "

<< Rvec.rows << "x" << Rvec.cols << "." << std::endl;

return false;

}

if (Tvec.rows != 3 || Tvec.cols != 1)

{

std::cerr << "[ERROR] HomMat3dFromRVT: Translation vector Tvec must be 3x1, but got "

<< Tvec.rows << "x" << Tvec.cols << "." << std::endl;

return false;

}

cv::Mat RMatrix;

cv::Rodrigues(Rvec, RMatrix);

CreatHomMatFromRT(RMatrix, Tvec, HomMat3d);

return true;

}

cv::Mat CAprilTagDetection::ScaleAbsChange(cv::Mat img, float alpha, int beta)

{

cv::Mat result;

cv::convertScaleAbs(img, result, alpha, beta);

return result;

}

double CAprilTagDetection::normalizeAngle(double angle)

{

while (angle > 180.0) angle -= 360.0;

while (angle <= -180.0) angle += 360.0;

return angle;

}

bool CAprilTagDetection::isRotatedMatrix(const cv::Mat& R)

{

if (R.rows != 3 || R.cols != 3 || R.type() != CV_64F && R.type() != CV_32F) return false;

cv::Mat Rt = R.t();

cv::Mat shouldBeIdentity = Rt * R;

cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());

return cv::norm(shouldBeIdentity, I, cv::NORM_INF) < 1e-6;

}

bool CAprilTagDetection::extractHomMtr2RT(const cv::Mat& HomoMtr, cv::Mat& R, cv::Mat& T)

{

if (HomoMtr.rows != 4 || HomoMtr.cols != 4)

{

std::cerr << "[ERROR] extractHomMtr2RT: Homogeneous matrix must be 4x4, but got " << HomoMtr.rows << "x" << HomoMtr.cols << "." << std::endl;

return false;

}

R = HomoMtr(cv::Range(0, 3), cv::Range(0, 3)).clone();

T = HomoMtr(cv::Range(0, 3), cv::Range(3, 4)).clone();

return true;

}

bool CAprilTagDetection::rotationMatrixToEulerAngles(const cv::Mat& R, cv::Vec3d& eulerAngle)

{

if (!isRotatedMatrix(R))

{

std::cerr << "Input is not a valid rotation matrix!" << std::endl;

return false;

}

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;

}

eulerAngle = cv::Vec3d(z, y, x);

return true;

}

cv::Vec3d CAprilTagDetection::getSymmetricEulerAngles(const cv::Vec3d& euler_angles)

{

cv::Vec3d euler_angles_transverted;

euler_angles_transverted[0] = normalizeAngle(euler_angles[0] + 180.0);

euler_angles_transverted[1] = normalizeAngle(180.0 - euler_angles[1]);

euler_angles_transverted[2] = normalizeAngle(euler_angles[2] + 180.0);

return euler_angles_transverted;

}

cv::Vec3d CAprilTagDetection::eulerAngleTranMini(const cv::Vec3d& euler_angles)

{

cv::Vec3d symmetric_angles = getSymmetricEulerAngles(euler_angles);

double original_sum = std::abs(euler_angles[0]) + std::abs(euler_angles[1]) + std::abs(euler_angles[2]);

double symmetric_sum = std::abs(symmetric_angles[0]) + std::abs(symmetric_angles[1]) + std::abs(symmetric_angles[2]);

return (original_sum <= symmetric_sum) ? euler_angles : symmetric_angles;

}

③main(程序调用)

int main() {

CAprilTagDetection cAprilTagDetection;

Mat currentImage = cv::imread("D:/VS2022Project/commom/test/calibration/1_RGB.bmp");

std::vector<std::vector<cv::Point2f>> vvp2fCorners;

std::vector<cv::Point2f > vvp2fCenters;

std::vector<std::string> vsCode;

cv::Mat gray;

cvtColor(currentImage, gray, cv::COLOR_BGR2GRAY);

cv::Mat pshowImage = currentImage.clone();

bool tag1 = false;

bool tag2 = false;

bool tag3 = false;

vector<vector<cv::Point2f>> contours;

std::vector<cv::Point2f> vp2fCorners1, vp2fCorners2, vp2fCorners3;

cv::Point2f vp2fCenter1, vp2fCenter2, vp2fCenter3;

gray = cAprilTagDetection.ScaleAbsChange(gray, 0.8, 0);

for (int cnt = 0; cnt < 19; cnt++)

{

vvp2fCorners.clear();

vsCode.clear();

cAprilTagDetection.detectNew(gray, &vvp2fCorners, &vvp2fCenters, &vsCode, &pshowImage);

for (int i = 0; i < vsCode.size(); i++)

{

if (vsCode[i] == "1")

{

tag1 = true;

vp2fCorners1 = vvp2fCorners[i];

vp2fCenter1 = vvp2fCenters[i];

}

else if (vsCode[i] == "3")

{

tag2 = true;

vp2fCorners2 = vvp2fCorners[i];

vp2fCenter2 = vvp2fCenters[i];

}

else if (vsCode[i] == "2")

{

tag3 = true;

vp2fCorners3 = vvp2fCorners[i];

vp2fCenter3 = vvp2fCenters[i];

}

if (tag1 && tag2 && tag3)

break;

gray = cAprilTagDetection.ScaleAbsChange(gray, 1.1, 5);

}

if (!tag1 || !tag2 || !tag3)

{

cout<< "error:二维码识别异常！";

}

contours.push_back(vp2fCorners1);

contours.push_back(vp2fCorners2);

contours.push_back(vp2fCorners3);

cv::Mat CameraMatrixRGB = (cv::Mat_<float>(3, 3) <<

533.799866,0.000000, 479.854218,

0.000000,533.449341,267.287384,

0.000000, 0.000000, 1.000000);

cv::Mat CameraDistCoeffs=(cv::Mat_<float>(1, 5)<<

0.000000, 0.000000, 0.000000, 0.000000, 0.000000);

std::vector<cv::Mat> pvHomMat3DPaperInCam;

cAprilTagDetection.calculatePose(200, contours, CameraMatrixRGB, CameraDistCoeffs, &pvHomMat3DPaperInCam, &pshowImage);

for (int i = 0; i < pvHomMat3DPaperInCam.size(); i++)

{

cv::Mat R;

cv::Mat T;

cAprilTagDetection.extractHomMtr2RT(pvHomMat3DPaperInCam[i], R, T);

cv::Vec3d eulerAngle;

cAprilTagDetection.rotationMatrixToEulerAngles(R, eulerAngle);

eulerAngle = cAprilTagDetection.eulerAngleTranMini(cAprilTagDetection.eulerToAngle(eulerAngle));

cout << "R" << i << ":" << eulerAngle[0] << " , " << eulerAngle[1] << " , " << eulerAngle[2] << endl;

cout << "T" << i << ":" << T.at<double>(0,0) << " , " << T.at<double>(1, 0) << " , " << T.at<double>(2, 0) << endl;

}

return 0;

}

特别注意：检测到的二维码角点0，1，2，3分别是左下角、右下角、右上角、左上角
输出的码坐标系的原点为码的左下角，x轴正方向从0指向1，y轴正方向从0指向3，Z轴正方向垂直于这个平面指向摄像头。

7.总结

AprilTag 检测工作流

创建检测器：使用 apriltag_detector_create() 创建检测器实例。设置参数：设置检测器的 quad_decimate, quad_sigma, refine_edges 等参数来优化性能。添加标签系列：使用 apriltag_detector_add_family() 将需要检测的标签系列（如tag36h11）添加到检测器中。读取图像：使用OpenCV等库读取图像并转换为image_u8_t格式，传递给检测器。检测AprilTag：使用 apriltag_detector_detect() 进行标签检测，获取检测结果。处理检测结果：遍历 zarray_t 中的每个检测，提取 apriltag_detection_t 结果。释放资源：检测完成后，使用 apriltag_detections_destroy() 和 apriltag_detector_destroy() 释放资源。