2.5D视觉——Aruco码定位检测

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标记被广泛用来增加从二维世界映射到三维世界时的信息量，便于发现二维世界与三维世界之间的投影关系，从而实现姿态估计、相机矫正等等应用。

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

2.Aruco码解码说明

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

2.1 Original ArUco

  早期的Aruco码Marker id解码规则是（这里以Marker id 21举例）：

  上面是7x7的方格，除去最外层的黑色边框，中间是5x5，其中奇数列是校验位，偶数列是数据位，所以第1、3、5列为校验位，2、4列为数据位，根据黑色方块对应0，白色方块对应1，提取出数据位为：

0	0
0	0
0	1
0	1
0	1每行首尾相接整理得：0000010101 转为十进制是1+4+16=21，对应Marker ID

2.2 预设的二维码字典

  OpenCV中预存了一些设置好Marker ID的字典，直接查找相对应的即可。

/*** @brief Predefined markers dictionaries/sets* Each dictionary indicates the number of bits and the number of markers contained* - DICT_ARUCO_ORIGINAL: standard ArUco Library Markers. 1024 markers, 5x5 bits, 0 minimumdistance*/
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 codesDICT_APRILTAG_25h9,     ///< 5x5 bits, minimum hamming distance between any two codes = 9, 35 codesDICT_APRILTAG_36h10,    ///< 6x6 bits, minimum hamming distance between any two codes = 10, 2320 codesDICT_APRILTAG_36h11     ///< 6x6 bits, minimum hamming distance between any two codes = 11, 587 codes
};

举例：

2.3 大小Aruco二维码叠加

应用场景：如无人机降落时可以使用大小Aruco码叠加实现精准降落。在距离地面较远时，通过大的Aruco码进行定位，在距离较近时，由于大的Aruco已经识别不全了，需要小的Aruco码进行定位。

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

3.函数说明

3.1 cv::aruco::detectMarkers

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

3.2 cv::solvePnP

  cv::solvePnP 是 OpenCV 库中用于解决 PnP (Perspective-n-Point) 问题的函数。PnP 问题的目标是从一组已知的三维空间点和它们在二维图像平面上的投影，估计出相机的外参，即相机的位置和朝向。cv::solvePnP 函数的目标是求解相机的 旋转矩阵（R）和 平移向量（T），从而描述从世界坐标系到相机坐标系的变换。

  cv::solvePnP 计算的结果是世界坐标系在相机坐标系中的位姿，即 旋转向量（rvec）和平移向量（tvec）。

CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints,InputArray cameraMatrix, InputArray distCoeffs,OutputArray rvec, OutputArray tvec,bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );// objectPoints    				    // 3D 世界坐标系中的点
// imagePoints     				    // 对应的 2D 图像坐标
// cameraMatrix                     // 相机内参矩阵
// distCoeffs                       // 畸变系数（可选，默认为 0）
// rvec                             // 输出的旋转向量
// tvec                             // 输出的平移向量
// bool useExtrinsicGuess = false   // 是否使用外部猜测值
// int flags = 0                    // 计算方法的标志位，指定不同的求解方法

参数说明：

• objectPoints：一个包含了 N 个三维点的向量，每个点的类型是 cv::Point3f，即 (x, y, z) 坐标。该点是世界坐标系下的坐标。
• imagePoints：一个包含了 N 个二维点的向量，每个点的类型是 cv::Point2f，即 (x, y) 坐标。这些是已知的三维点投影到相机图像平面上的二维坐标。
• cameraMatrix：相机的内参矩阵，通常是一个 3x3 的矩阵，描述相机的焦距和主点（通常是图像中心），其形式如下：

cameraMatrix = [ fx  0   cx ][  0   fy  cy ][  0    0   1 ]

fx、fy：焦距，单位为像素，通常等于图像的焦距与像素尺寸的比值。
cx、cy：主点坐标，通常是图像中心。

• distCoeffs：相机的畸变系数。这个参数是一个 1x5 或 1x8 的矩阵，表示相机的径向畸变和切向畸变的系数。一般情况下，畸变系数会是：
  k1, k2, k3：径向畸变系数
  p1, p2：切向畸变系数
  如果没有畸变或畸变系数未知，可以传入零矩阵（即 cv::Mat::zeros(1, 5, CV_64F)）。
• rvec：输出的旋转向量，表示从世界坐标系到相机坐标系的旋转变换。旋转向量是一个 3x1 的矩阵，它是 Rodrigues 旋转公式的参数。可以使用 cv::Rodrigues 将旋转向量转换为旋转矩阵。
• tvec：输出的平移向量，表示从世界坐标系到相机坐标系的平移。它是一个 3x1 的矩阵，单位通常是米或者毫米，表示相机位置相对于世界坐标系的平移量。
• useExtrinsicGuess（可选）：一个布尔值，指示是否使用外部猜测的旋转和平移向量进行初始化。默认值是 false，如果设置为 true，可以加速求解。
• flags（可选）：计算方法的标志位。可以选择不同的 PnP 求解方法，例如：
  cv::SOLVEPNP_ITERATIVE：标准的迭代求解方法（默认）
  cv::SOLVEPNP_EPNP：基于 EPnP (Efficient PnP) 的求解方法，适用于更少的点
  cv::SOLVEPNP_DLS：基于 DLS (Direct Linear Solver) 的求解方法，适用于多点情况
  cv::SOLVEPNP_P3P：专门用于 4 个点的求解方法，适用于精度要求较高的应用。

返回值：
cv::solvePnP 返回一个布尔值，表示是否成功求解。

4.代码注解

4.1 Landmark图说明

  上图是Landmark图，其中包含了四个Aruco码，id分别是21，23，25，27（从左上角开始，顺时针旋转），每个Aruco码的边长是40mm，每个Aruco码相对于Landmark图的中心偏移是5mm。

4.2 算法源码注解

• 结构体声明

/*** @brief    姿态的四元数*/
struct OrientQuaternion
{double w;double x;double y;double z;
};/*** @brief    位置的xyz方向偏移*/
struct Location
{double x;double y;double z;
};

• getObjectPoints
  获取每张Landmark图上所有Aruco码的角点坐标（4x4）指实际的物理坐标，需要测量Landmark图实际的尺寸，这里以Landmark图的中心点为原点，每个Aruco码有四个角点。

/*** @brief        获取每张Landmark图上所有Aruco码的角点坐标* @note* @param[in]    idx          根据不同的标记类型，指定生成哪种类型的3D点坐标。idx == 1:    对应特定范围的Aruco ID（21到27）idx == 2:    对应特定范围的Aruco ID（31到37）idx == 0:    对应特定范围的Aruco ID（1到7）* @param[in]    ids          Aruco标记的ID列表，每个ID对应一个检测到的Aruco标记* @param[out]   obj_points   Aruco标记的四个角点的3D点坐标（Z方向默认给0）* @return*/
int getObjectPoints(int idx, const std::vector<int> &ids, std::vector<cv::Point3f> &obj_points)
{obj_points.clear();float code_size, offset_size;int code_pos = 0;if (idx == 1) {code_size   = 0.04 / 2; // 每个Aruco码的边长 40mmoffset_size = code_size + 0.005; // 以landmark图中心点为基准,Aruco码的偏移为5mmcode_pos    = 21;} else if (idx == 2) {code_size   = 0.02 / 2;offset_size = code_size + 0.0025;code_pos    = 31;} else {code_size   = 0.03;offset_size = 0.035;code_pos    = 1;}// 确保传入的 ids 数组中的编号必须是以 code_pos为基础的偶数编号//（例如，code_pos=21，则有效编号是 27, 25, 23, 21 等，确保 landmark中的id号是按照规则的）int num, temp;num = ids.size();int i;for (i = 0; i < num; i++) {temp = (ids[i] - code_pos) / 2;temp *= 2;temp += code_pos;if (temp != ids[i])return -1;}cv::Point3f c4[4], c_offset[4];c4[0]       = cv::Point3f(-code_size, code_size, 0);c4[1]       = cv::Point3f(code_size, code_size, 0);c4[2]       = cv::Point3f(code_size, -code_size, 0);c4[3]       = cv::Point3f(-code_size, -code_size, 0);c_offset[0] = cv::Point3f(-offset_size, offset_size, 0);  // 1c_offset[1] = cv::Point3f(offset_size, offset_size, 0);   // 3c_offset[2] = cv::Point3f(-offset_size, -offset_size, 0); // 5c_offset[3] = cv::Point3f(offset_size, -offset_size, 0);  // 7for (i = 0; i < num; i++) {obj_points.push_back(c4[0] + c_offset[(ids[i] - code_pos) / 2]);obj_points.push_back(c4[1] + c_offset[(ids[i] - code_pos) / 2]);obj_points.push_back(c4[2] + c_offset[(ids[i] - code_pos) / 2]);obj_points.push_back(c4[3] + c_offset[(ids[i] - code_pos) / 2]);}return 0;
}

• rotVecToQuaternion

/*** @brief   旋转向量转四元数* @note* @param	rvec[in]  旋转向量* @param	ori[out]  四元数* @return  0 代表转换成功*/
inline int rotVecToQuaternion(float *rvec, OrientQuaternion &ori)
{float r, x, y, z;x = rvec[0];y = rvec[1];z = rvec[2];r = sqrt(x * x + y * y + z * z);x /= r;y /= r;z /= r;r /= 2.0;ori.w = cos(r);ori.x = x * sin(r);ori.y = y * sin(r);ori.z = z * sin(r);return 0;
}

• quaternionToEuler

/*** @brief    四元数转换为欧拉角* @note* @param    ori[int]       四元数* @param    rx,ry,rz[out]      欧拉角(弧度)* @return*/
inline int QuaternionToEuler(OrientQuaternion &ori, double &rx, double &ry, double &rz)
{float rpyInfo[3] = { 0, 0, 0 };memset(rpyInfo, 0, sizeof(rpyInfo));// 算法提供的四元数转欧拉角,得到的欧拉角是角度且顺序是z,y,xfloat RotN[3][3];RotN[0][0] = 2 * (ori.w * ori.w + ori.x * ori.x) - 1;RotN[0][1] = 2 * (ori.x * ori.y - ori.w * ori.z);RotN[0][2] = 2 * (ori.x * ori.z + ori.w * ori.y);RotN[1][0] = 2 * (ori.x * ori.y + ori.w * ori.z);RotN[1][1] = 2 * (ori.w * ori.w + ori.y * ori.y) - 1;RotN[1][2] = 2 * (ori.y * ori.z - ori.w * ori.x);RotN[2][0] = 2 * (ori.x * ori.z - ori.w * ori.y);RotN[2][1] = 2 * (ori.y * ori.z + ori.w * ori.x);RotN[2][2] = 2 * (ori.w * ori.w + ori.z * ori.z) - 1;double eps = 1e-16;if (fabs(RotN[0][0]) < eps && fabs(RotN[1][0]) < eps) {rpyInfo[0] = 0.0;rpyInfo[1] = atan2(-RotN[2][0], RotN[0][0]) * 180 / M_PI;rpyInfo[2] = atan2(-RotN[1][2], RotN[1][1]) * 180 / M_PI;} else {rpyInfo[0] = atan2(RotN[1][0], RotN[0][0]) * 180 / M_PI;rpyInfo[1] = atan2(-RotN[2][0], sqrt(RotN[0][0] * RotN[0][0] + RotN[1][0] * RotN[1][0])) * 180 / M_PI;rpyInfo[2] = atan2(RotN[2][1], RotN[2][2]) * 180 / M_PI;}// 算法返回的顺序是z,y,x,需要反转打印rx = rpyInfo[2] / 180.0 * M_PI;ry = rpyInfo[1] / 180.0 * M_PI;rz = rpyInfo[0] / 180.0 * M_PI;return 0;
}

• calCameraInLandmarkPose
    算法思想：首先通过OpenCV自带的detectMarkers函数，检测出所有的Aruco码的四个角点的像素坐标，然后根据事先测量好的Landmark图实际尺寸，以中心点为原点，计算出实际的所有的Aruco码的四个角点的3D坐标（这里Z方向设置0），通过OpenCV自带的solvePnP函数，计算得到相机相对于世界坐标系的外参（这里是以Landmark图中心点为世界坐标系），即 旋转向量（rvec）和平移向量（tvec），世界坐标系在相机坐标系中的位姿。
    所以最终得到的是T_target2camera

/*** @brief        计算landmark图在相机下的位姿信息* @note* @param[in]    image                图像信息* @param[in]    cam_intrinsics       相机的内参矩阵* @param[in]    cam_distortion       相机的畸变系数* @param[out]   landmark_to_cam_pose landmark图相对于相机的位姿* @return       landmark图计算结果*           --0 计算成功*               失败返回错误码*/
int calLandmarkInCameraPose(cv::Mat &image,const std::vector<double> &cam_intrinsics,const std::vector<double> &cam_distortion,std::vector<double> &landmark_to_cam_pose)
{if (image.cols() == 0 || image.rows() == 0) {return -1;}OrientQuaternion landmark_ori;Location landmark_loc;// 加载用于生成标记的字典cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);// 存储检测到的标记的 IDstd::vector<int> ids;// 声明包含检测到的标记角和被拒绝的候选标记的角点// 在检测过程中，可能会有一些候选标记没有通过验证（例如它们可能是噪声或误识别的对象），这些被拒绝的候选会被存储在rejectCandidates中std::vector<std::vector<cv::Point2f>> corners, reject_candidates;// 使用默认值初始化检测器参数cv::Ptr<cv::aruco::DetectorParameters> parameters;parameters = cv::aruco::DetectorParameters::create();// 使用 ArUco 方法检测角点后，通过轮廓点线拟合的方法来细化角点位置parameters->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR;// 检测图像中的标记,靶标cv::aruco::detectMarkers(preview_image, dictionary, corners, ids, parameters, reject_candidates);int j;// 3D 世界坐标系中的点std::vector<cv::Point3f> obj_pts;int obj_idx;obj_pts.clear();// ids.size()表示一张landmark图中有多少张Aruco标记图// Aruco码中的ID需要去查字典if (ids.size() > 0 && ids.size() <= 4) {if (ids[0] > 20 && ids[0] <= 27)obj_idx = 1;else if (ids[0] > 30 && ids[0] <= 37)obj_idx = 2;else if (ids[0] > 0 && ids[0] <= 7)obj_idx = 0;else {obj_idx = -1;return arva::ErrorREC::E_REC_LANDMARK_WRONG;}j = getObjectPoints(obj_idx, ids, obj_pts);// std::cout << "Landmark id is " << obj_idx << std::endl;} else {// 未检测到Landmarkstd::cout << "There is no target in the image!" << std::endl;return -1;}// 角点对应的 2D 图像像素坐标std::vector<cv::Point2f> image_pts;unsigned int k;// if(ids.size() == 4)if (ids.size() > 0 && ids.size() <= 4) {float dx, dy, dis;dx  = corners[0][0].x - corners[0][1].x;dy  = corners[0][0].y - corners[0][1].y;dis = sqrt(dx * dx + dy * dy);for (k = 0; k < ids.size(); k++) {// 绘制landmark上面的角点cv::circle(preview_image, corners[k][0], dis / 15, cv::Scalar(255, 0, 0), dis / 30, 8);cv::circle(preview_image, corners[k][1], dis / 15, cv::Scalar(0, 0, 255), dis / 30, 8);cv::circle(preview_image, corners[k][2], dis / 15, cv::Scalar(0, 255, 0), dis / 30, 8);for (j = 0; j < 4; j++) {image_pts.push_back(corners[k][j]);}}}// 打印 objPts 中的所有点std::cout << "objPts" << std::endl;for (size_t i = 0; i < objPts.size(); i++) {std::cout << "Point " << i << ": (" << objPts[i].x << ", " << objPts[i].y << ", " << objPts[i].z << ")"<< std::endl;}// 打印 imgPts 中的所有点std::cout << "imgPts" << std::endl;for (size_t i = 0; i < imgPts.size(); i++) {std::cout << "Point " << i << ": (" << imgPts[i].x << ", " << imgPts[i].y << ")" << std::endl;}float img_intrinsics[9];for (int n = 0; n < cam_intrinsics.size(); n++) {img_intrinsics[n] = cam_intrinsics[n];}cv::Mat camera_matrix = cv::Mat(3, 3, CV_32FC1, img_intrinsics);float img_distortion[5];for (int m = 0; m < cam_distortion.size(); m++) {img_distortion[m] = cam_distortion[m];}cv::Mat distortion_coefficients = cv::Mat(5, 1, CV_32FC1, img_distortion);float tv[3], rv[3];cv::Mat tvec = cv::Mat(3, 1, CV_32FC1, tv);cv::Mat rvec = cv::Mat(3, 1, CV_32FC1, rv);if (ids.size() > 0 && ids.size() <= 4) {bool state;// 计算出相机相对于Landmark图中心点的旋转和位移向量state = cv::solvePnP(obj_pts, image_pts, camera_matrix, distortion_coefficients, rvec, tvec);// Rodrigues(rvec,r_m);if (state) {// 将solvePnP输出的旋转向量转换为四元数rotVecToQuaternion(rv, landmark_ori);landmark_loc.x = tv[0];landmark_loc.y = tv[1];landmark_loc.z = tv[2];// 输出Posture结构体的位姿信息,需要将四元数转为欧拉角进行赋值landmark_to_cam_pose.push_back(landmark_loc.x);landmark_to_cam_pose.push_back(landmark_loc.y);landmark_to_cam_pose.push_back(landmark_loc.z);double rx, ry, rz;double rx, ry, rz;quaternionToEuler(landmark_ori, rx, ry, rz);landmark_to_cam_pose.push_back(rx);landmark_to_cam_pose.push_back(ry);landmark_to_cam_pose.push_back(rz);return -1;} else {std::cout << "solvePnP failed" << std::endl;}}return -1;
}