網(wǎng)站建設(shè)維護(hù)協(xié)議書北海百度seo
目錄
- 1.什么是Aruco標(biāo)記
- 2.Aruco碼解碼說明
- 2.1 Original ArUco
- 2.2 預(yù)設(shè)的二維碼字典
- 2.3 大小Aruco二維碼疊加
- 3.函數(shù)說明
- 3.1 cv::aruco::detectMarkers
- 3.2 cv::solvePnP
- 4.代碼注解
- 4.1 Landmark圖說明
- 4.2 算法源碼注解
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)粘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_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;
}