1.什么是Aruco標(biāo)記

??ArUco標(biāo)記最初由S.Garrido-Jurado等人在2014年發(fā)表的論文Automatic generation and detection of highly reliable fiducial markers under occlusion中提出。ArUco的全稱是Augmented Reality University of Cordoba，下面給出ArUco標(biāo)記的一些示例。

??ArUco標(biāo)記是可用于攝像機(jī)姿態(tài)估計(jì)的二進(jìn)制方形基準(zhǔn)標(biāo)記。它的主要優(yōu)點(diǎn)是檢測(cè)簡(jiǎn)單、快速，并且具有很強(qiáng)的魯棒性。ArUco 標(biāo)記是由寬黑色邊框和確定其標(biāo)識(shí)符（id）的內(nèi)部二進(jìn)制矩陣組成的正方形標(biāo)記。ArUco標(biāo)記的黑色邊框有助于其在圖像中的快速檢測(cè)，內(nèi)部二進(jìn)制編碼用于識(shí)別標(biāo)記和提供錯(cuò)誤檢測(cè)和糾正。ArUco標(biāo)記尺寸的大小決定內(nèi)部矩陣的大小，例如尺寸為 4x4 的標(biāo)記由 16 位二進(jìn)制數(shù)組成。

??ArUco標(biāo)記的尺寸可以任意的更改，為了成功檢測(cè)可根據(jù)對(duì)象大小和場(chǎng)景選擇合適的尺寸。在實(shí)際使用中，如果標(biāo)記的尺寸太小，可能無法檢測(cè)到它，這時(shí)可以選擇更換較大尺寸的標(biāo)記，或者將相機(jī)離標(biāo)記更近一些。

??在機(jī)器人應(yīng)用中，可以將這些標(biāo)記沿著倉庫機(jī)器人的路徑放置。當(dāng)安裝在機(jī)器人上的攝像頭檢測(cè)到這些標(biāo)記時(shí)，由于每個(gè)標(biāo)記都有唯一的ID，并且且標(biāo)記在倉庫中的放置位置已知，因此就可以知道機(jī)器人在倉庫中的精確位置。

??通俗地說，Aruco標(biāo)記其實(shí)就是一種編碼，就和我們?nèi)粘Ｉ钪械亩S碼是相似的，只不過由于編碼方式的不同，導(dǎo)致它們存儲(chǔ)信息的方式、容量等等有所差異，所以在應(yīng)用層次上也會(huì)有所不同。由于單個(gè)ArUco標(biāo)記就可以提供足夠的對(duì)應(yīng)關(guān)系，例如有四個(gè)明顯的角點(diǎn)及內(nèi)部的二進(jìn)制編碼，所以ArUco標(biāo)記被廣泛用來增加從二維世界映射到三維世界時(shí)的信息量，便于發(fā)現(xiàn)二維世界與三維世界之間的投影關(guān)系，從而實(shí)現(xiàn)姿態(tài)估計(jì)、相機(jī)矯正等等應(yīng)用。

??ArUco marker是一種漢明碼方格圖。它由一個(gè)寬的黑邊和一個(gè)內(nèi)部的二進(jìn)制矩陣組成，黑色的邊界有利于快速檢測(cè)到圖像，Marker ID是他的二進(jìn)制矩陣編碼。黑色方塊對(duì)應(yīng)0，白色方塊對(duì)應(yīng)1。一個(gè)二維碼就是一個(gè)矩陣。

2.Aruco碼解碼說明

Aruco二維碼生成網(wǎng)站
https://chev.me/arucogen/

2.1 Original ArUco

??早期的Aruco碼Marker id解碼規(guī)則是（這里以Marker id 21舉例）：

??上面是7x7的方格，除去最外層的黑色邊框，中間是5x5，其中奇數(shù)列是校驗(yàn)位，偶數(shù)列是數(shù)據(jù)位，所以第1、3、5列為校驗(yàn)位，2、4列為數(shù)據(jù)位，根據(jù)黑色方塊對(duì)應(yīng)0，白色方塊對(duì)應(yīng)1，提取出數(shù)據(jù)位為：

0	0
0	0
0	1
0	1
0	1每行首尾相接整理得：0000010101 轉(zhuǎn)為十進(jìn)制是1+4+16=21，對(duì)應(yīng)Marker ID

2.2 預(yù)設(shè)的二維碼字典

??OpenCV中預(yù)存了一些設(shè)置好Marker ID的字典，直接查找相對(duì)應(yīng)的即可。

/*** @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二維碼疊加

應(yīng)用場(chǎng)景：如無人機(jī)降落時(shí)可以使用大小Aruco碼疊加實(shí)現(xiàn)精準(zhǔn)降落。在距離地面較遠(yuǎn)時(shí)，通過大的Aruco碼進(jìn)行定位，在距離較近時(shí)，由于大的Aruco已經(jīng)識(shí)別不全了，需要小的Aruco碼進(jìn)行定位。

??白色方格內(nèi)黑色占比小于二分之一，則還是當(dāng)做白色方格，所以可以疊加小二維碼，而不影響大二維碼檢測(cè)。
??如下圖所示的大小重疊二維碼，小二維碼的存在也是不影響大二維碼識(shí)別的，因?yàn)橹虚g方格內(nèi)的小二維碼，黑色占比大于二分之一，所以檢測(cè)大二維碼時(shí)，小二維碼依舊被當(dāng)作黑色方格。

3.函數(shù)說明

3.1 cv::aruco::detectMarkers

cv::aruco::detectMarkers(image_, dictionary_, corners, ids, detectorParams_, rejected);
// 參數(shù)：
// (1)image :輸入的需要檢測(cè)標(biāo)記的圖像。 
// (2)dictionary :進(jìn)行檢測(cè)的字典對(duì)象指針，這里的字典就是我們創(chuàng)建aruco 標(biāo)記時(shí)所使用的字典，檢測(cè)什么類型的aruco 標(biāo)記就使用什么類型的字典。 
// (3)corners :輸出參數(shù)，檢測(cè)到的aruco 標(biāo)記的角點(diǎn)列表，是一個(gè)向量數(shù)組，每個(gè)元素對(duì)應(yīng)一個(gè)檢測(cè)到的標(biāo)記，每個(gè)標(biāo)記有四個(gè)角點(diǎn)，其四個(gè)角點(diǎn)均按其原始順序返回 (從左上角開始順時(shí)針旋轉(zhuǎn))。 
// (4)ids:輸出參數(shù)，檢測(cè)到的每個(gè)標(biāo)記的id,需要注意的是第三個(gè)參數(shù)和第四個(gè)參數(shù)具有相同的大小。 
// (5)parameters:ArUco 檢測(cè)器的參數(shù)。是一個(gè) cv::aruco::DetectorParameters 類型的對(duì)象，用于設(shè)置檢測(cè)器的各種參數(shù)，例如邊緣閾值、最小標(biāo)記區(qū)域等。
// (6)rejectedImgPoints：輸出參數(shù)，被拒絕的標(biāo)記角點(diǎn)。這些角點(diǎn)未能形成有效的標(biāo)記。  

3.2 cv::solvePnP

??cv::solvePnP 是 OpenCV 庫中用于解決 PnP (Perspective-n-Point) 問題的函數(shù)。PnP 問題的目標(biāo)是從一組已知的三維空間點(diǎn)和它們?cè)诙S圖像平面上的投影，估計(jì)出相機(jī)的外參，即相機(jī)的位置和朝向。cv::solvePnP 函數(shù)的目標(biāo)是求解相機(jī)的 旋轉(zhuǎn)矩陣（R）和 平移向量（T），從而描述從世界坐標(biāo)系到相機(jī)坐標(biāo)系的變換。

??cv::solvePnP 計(jì)算的結(jié)果是世界坐標(biāo)系在相機(jī)坐標(biāo)系中的位姿，即 旋轉(zhuǎn)向量（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 世界坐標(biāo)系中的點(diǎn)
// imagePoints     				    // 對(duì)應(yīng)的 2D 圖像坐標(biāo)
// cameraMatrix                     // 相機(jī)內(nèi)參矩陣
// distCoeffs                       // 畸變系數(shù)（可選，默認(rèn)為 0）
// rvec                             // 輸出的旋轉(zhuǎn)向量
// tvec                             // 輸出的平移向量
// bool useExtrinsicGuess = false   // 是否使用外部猜測(cè)值
// int flags = 0                    // 計(jì)算方法的標(biāo)志位，指定不同的求解方法

參數(shù)說明：

• objectPoints：一個(gè)包含了 N 個(gè)三維點(diǎn)的向量，每個(gè)點(diǎn)的類型是 cv::Point3f，即 (x, y, z) 坐標(biāo)。該點(diǎn)是世界坐標(biāo)系下的坐標(biāo)。
• imagePoints：一個(gè)包含了 N 個(gè)二維點(diǎn)的向量，每個(gè)點(diǎn)的類型是 cv::Point2f，即 (x, y) 坐標(biāo)。這些是已知的三維點(diǎn)投影到相機(jī)圖像平面上的二維坐標(biāo)。
• cameraMatrix：相機(jī)的內(nèi)參矩陣，通常是一個(gè) 3x3 的矩陣，描述相機(jī)的焦距和主點(diǎn)（通常是圖像中心），其形式如下：

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

fx、fy：焦距，單位為像素，通常等于圖像的焦距與像素尺寸的比值。
cx、cy：主點(diǎn)坐標(biāo)，通常是圖像中心。

• distCoeffs：相機(jī)的畸變系數(shù)。這個(gè)參數(shù)是一個(gè) 1x5 或 1x8 的矩陣，表示相機(jī)的徑向畸變和切向畸變的系數(shù)。一般情況下，畸變系數(shù)會(huì)是：
  k1, k2, k3：徑向畸變系數(shù)
  p1, p2：切向畸變系數(shù)
  如果沒有畸變或畸變系數(shù)未知，可以傳入零矩陣（即 cv::Mat::zeros(1, 5, CV_64F)）。
• rvec：輸出的旋轉(zhuǎn)向量，表示從世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)變換。旋轉(zhuǎn)向量是一個(gè) 3x1 的矩陣，它是 Rodrigues 旋轉(zhuǎn)公式的參數(shù)。可以使用 cv::Rodrigues 將旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣。
• tvec：輸出的平移向量，表示從世界坐標(biāo)系到相機(jī)坐標(biāo)系的平移。它是一個(gè) 3x1 的矩陣，單位通常是米或者毫米，表示相機(jī)位置相對(duì)于世界坐標(biāo)系的平移量。
• useExtrinsicGuess（可選）：一個(gè)布爾值，指示是否使用外部猜測(cè)的旋轉(zhuǎn)和平移向量進(jìn)行初始化。默認(rèn)值是 false，如果設(shè)置為 true，可以加速求解。
• flags（可選）：計(jì)算方法的標(biāo)志位?？梢赃x擇不同的 PnP 求解方法，例如：
  cv::SOLVEPNP_ITERATIVE：標(biāo)準(zhǔn)的迭代求解方法（默認(rèn)）
  cv::SOLVEPNP_EPNP：基于 EPnP (Efficient PnP) 的求解方法，適用于更少的點(diǎn)
  cv::SOLVEPNP_DLS：基于 DLS (Direct Linear Solver) 的求解方法，適用于多點(diǎn)情況
  cv::SOLVEPNP_P3P：專門用于 4 個(gè)點(diǎn)的求解方法，適用于精度要求較高的應(yīng)用。

返回值：
cv::solvePnP 返回一個(gè)布爾值，表示是否成功求解。

4.代碼注解

4.1 Landmark圖說明

??上圖是Landmark圖，其中包含了四個(gè)Aruco碼，id分別是21，23，25，27（從左上角開始，順時(shí)針旋轉(zhuǎn)），每個(gè)Aruco碼的邊長(zhǎng)是40mm，每個(gè)Aruco碼相對(duì)于Landmark圖的中心偏移是5mm。

4.2 算法源碼注解

• 結(jié)構(gòu)體聲明

/*** @brief    姿態(tài)的四元數(shù)*/
struct OrientQuaternion
{double w;double x;double y;double z;
};/*** @brief    位置的xyz方向偏移*/
struct Location
{double x;double y;double z;
};

• getObjectPoints
  獲取每張Landmark圖上所有Aruco碼的角點(diǎn)坐標(biāo)（4x4）指實(shí)際的物理坐標(biāo)，需要測(cè)量Landmark圖實(shí)際的尺寸，這里以Landmark圖的中心點(diǎn)為原點(diǎn)，每個(gè)Aruco碼有四個(gè)角點(diǎn)。

/*** @brief        獲取每張Landmark圖上所有Aruco碼的角點(diǎn)坐標(biāo)* @note* @param[in]    idx          根據(jù)不同的標(biāo)記類型，指定生成哪種類型的3D點(diǎn)坐標(biāo)。idx == 1:    對(duì)應(yīng)特定范圍的Aruco ID（21到27）idx == 2:    對(duì)應(yīng)特定范圍的Aruco ID（31到37）idx == 0:    對(duì)應(yīng)特定范圍的Aruco ID（1到7）* @param[in]    ids          Aruco標(biāo)記的ID列表，每個(gè)ID對(duì)應(yīng)一個(gè)檢測(cè)到的Aruco標(biāo)記* @param[out]   obj_points   Aruco標(biāo)記的四個(gè)角點(diǎn)的3D點(diǎn)坐標(biāo)（Z方向默認(rèn)給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; // 每個(gè)Aruco碼的邊長(zhǎng) 40mmoffset_size = code_size + 0.005; // 以landmark圖中心點(diǎn)為基準(zhǔn),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 數(shù)組中的編號(hào)必須是以 code_pos為基礎(chǔ)的偶數(shù)編號(hào)//（例如，code_pos=21，則有效編號(hào)是 27, 25, 23, 21 等，確保 landmark中的id號(hào)是按照規(guī)則的）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   旋轉(zhuǎn)向量轉(zhuǎn)四元數(shù)* @note* @param	rvec[in]  旋轉(zhuǎn)向量* @param	ori[out]  四元數(shù)* @return  0 代表轉(zhuǎn)換成功*/
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    四元數(shù)轉(zhuǎn)換為歐拉角* @note* @param    ori[int]       四元數(shù)* @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));// 算法提供的四元數(shù)轉(zhuǎn)歐拉角,得到的歐拉角是角度且順序是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,需要反轉(zhuǎn)打印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函數(shù)，檢測(cè)出所有的Aruco碼的四個(gè)角點(diǎn)的像素坐標(biāo)，然后根據(jù)事先測(cè)量好的Landmark圖實(shí)際尺寸，以中心點(diǎn)為原點(diǎn)，計(jì)算出實(shí)際的所有的Aruco碼的四個(gè)角點(diǎn)的3D坐標(biāo)（這里Z方向設(shè)置0），通過OpenCV自帶的solvePnP函數(shù)，計(jì)算得到相機(jī)相對(duì)于世界坐標(biāo)系的外參（這里是以Landmark圖中心點(diǎn)為世界坐標(biāo)系），即 旋轉(zhuǎn)向量（rvec）和平移向量（tvec），世界坐標(biāo)系在相機(jī)坐標(biāo)系中的位姿。
  ??所以最終得到的是T_target2camera

/*** @brief        計(jì)算landmark圖在相機(jī)下的位姿信息* @note* @param[in]    image                圖像信息* @param[in]    cam_intrinsics       相機(jī)的內(nèi)參矩陣* @param[in]    cam_distortion       相機(jī)的畸變系數(shù)* @param[out]   landmark_to_cam_pose landmark圖相對(duì)于相機(jī)的位姿* @return       landmark圖計(jì)算結(jié)果*           --0 計(jì)算成功*               失敗返回錯(cuò)誤碼*/
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;// 加載用于生成標(biāo)記的字典cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);// 存儲(chǔ)檢測(cè)到的標(biāo)記的 IDstd::vector<int> ids;// 聲明包含檢測(cè)到的標(biāo)記角和被拒絕的候選標(biāo)記的角點(diǎn)// 在檢測(cè)過程中，可能會(huì)有一些候選標(biāo)記沒有通過驗(yàn)證（例如它們可能是噪聲或誤識(shí)別的對(duì)象），這些被拒絕的候選會(huì)被存儲(chǔ)在rejectCandidates中std::vector<std::vector<cv::Point2f>> corners, reject_candidates;// 使用默認(rèn)值初始化檢測(cè)器參數(shù)cv::Ptr<cv::aruco::DetectorParameters> parameters;parameters = cv::aruco::DetectorParameters::create();// 使用 ArUco 方法檢測(cè)角點(diǎn)后，通過輪廓點(diǎn)線擬合的方法來細(xì)化角點(diǎn)位置parameters->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR;// 檢測(cè)圖像中的標(biāo)記,靶標(biāo)cv::aruco::detectMarkers(preview_image, dictionary, corners, ids, parameters, reject_candidates);int j;// 3D 世界坐標(biāo)系中的點(diǎn)std::vector<cv::Point3f> obj_pts;int obj_idx;obj_pts.clear();// ids.size()表示一張landmark圖中有多少張Aruco標(biāo)記圖// 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 {// 未檢測(cè)到Landmarkstd::cout << "There is no target in the image!" << std::endl;return -1;}// 角點(diǎn)對(duì)應(yīng)的 2D 圖像像素坐標(biāo)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上面的角點(diǎn)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 中的所有點(diǎn)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 中的所有點(diǎn)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;// 計(jì)算出相機(jī)相對(duì)于Landmark圖中心點(diǎn)的旋轉(zhuǎn)和位移向量state = cv::solvePnP(obj_pts, image_pts, camera_matrix, distortion_coefficients, rvec, tvec);// Rodrigues(rvec,r_m);if (state) {// 將solvePnP輸出的旋轉(zhuǎn)向量轉(zhuǎn)換為四元數(shù)rotVecToQuaternion(rv, landmark_ori);landmark_loc.x = tv[0];landmark_loc.y = tv[1];landmark_loc.z = tv[2];// 輸出Posture結(jié)構(gòu)體的位姿信息,需要將四元數(shù)轉(zhuǎn)為歐拉角進(jìn)行賦值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;
}