在之前的筆記《OpenCV4學習筆記（72）》中，記錄了在OpenCV中關於aruco標記的創建和檢測這方面的內容，今天就基於aruco標記檢測來進一步實現對aruco標記的實時姿態估計。
首先我們需要知道，所謂姿態估計問題就是要確定某個三維物體的方位指向問題，也就是確定以該物體爲中心原點的一個座標系。姿態估計在機器人視覺、動作跟蹤和單照相機定標等很多領域都有應用，當然了在不同領域中實現姿態估計的方法也有很多種。在本次筆記中，就來整理一下OpenCV中的ArUco模塊對於aruco標記是如何實現姿態估計的。
那麼對於aruco標記進行姿態估計，第一步是要先檢測到我們的aruco標記，這就涉及到《OpenCV4學習筆記（72）》中的內容了，可以參閱一下。這裏直接給出演示代碼：
auto dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_6X6_250);
vector<vector<Point2f>>corners, rejectedImgPoints;
vector<int>ids;
auto parameters = aruco::DetectorParameters::create();
aruco::detectMarkers(test_image, dictionary, corners, ids, parameters, rejectedImgPoints);
那麼當我們每檢測到一個aruco標記，vector<vector<Point2f>>corners中就會獲得一個檢測到標記的角點列表，我們接下來就通過這個角點列表來對該標記進行姿態估計。
OpenCV中提供了estimatePoseSingleMarkers()這個API來實現對這種簡單標記的姿態估計，其參數含義如下：
（1）參數corners： detectMarkers()返回的檢測到標記的角點列表，是一個vector<vector<Point2f>>類型的元素；
（2）參數markerLength：aruco標記的實際物理尺寸，也就是我們打印出來的aruco標記的實際尺寸，一般以米爲單位；
（3）參數cameraMatrix：用來拍攝aruco標記的相機的內參矩陣，突然發現還沒有整理過相機標定這一塊內容，有空再補一下嘿嘿(o゜▽゜)o☆;
（4)參數distCoeffs：用來拍攝aruco標記的相機的畸變參數；
（5）參數rvecs：vector<cv::Vec3d>類型的向量，其中每個元素爲每個標記相對於相機的旋轉向量；
（6）參數tvecs：vector<cv::Vec3d>類型的向量，其中每個元素爲每個標記相對於相機的平移向量。
（7）參數_objPoints：每個標記角點的對應點數組。
estimatePoseSingleMarkers()接收 detectMarkers()檢測到的aruco標記（以輸出角點列表的方式表示），並分別對每個標記進行姿態估計。因此，每個aruco標記都將返回一個相對於相機的旋轉向量和平移矢量，返回的點數組是將標記角點從每個標記座標系轉換到相機座標系下的表示。
標記座標系原點位於標記的中心，Z軸垂直於標記平面，每個標記的四個角點在其座標系中的座標爲：（-markerLength / 2，markerLength / 2，0），（markerLength / 2，markerLength / 2，0），（markerLength / 2，-markerLength / 2 ，0），（-markerLength / 2，-markerLength / 2，0），其中，markerLength是aruco標記的邊長。
通過estimatePoseSingleMarkers()這個API，我們就能實現對簡單aruco標記的姿態估計了，但是我們還需要把它進行可視化來便於我們查看，所以我們可以繪製出每個aruco標記的座標軸，從而查看姿態估計的結果。
我們使用drawAxis（）這個API來繪製座標軸，其參數含義爲：
（1）參數image：繪製座標軸的輸入、輸出圖像（通常是進行檢測標記的圖像）；
（2）參數cameraMatrix：相機的內參矩陣；
（3）參數distCoeffs：相機的畸變參數；
（4）~（5）參數rvec和參數tvec：當前要繪製座標軸的物體的姿態參數，分別是旋轉向量和平移向量；
（6）參數length：繪製座標軸的長度，單位通常爲米。
那麼上面就對aruco標記的姿態估計所需API進行了介紹，下面就進入代碼演示和效果演示部分。
首先，我們需要加載相機的內參矩陣和畸變係數，這裏是默認標定好了的，直接獲取的參數
cv::Mat cameraMatrix, distCoeffs;
vector<double> camera = { 657.1548323619423, 0, 291.8582472145741,0, 647.384819351103, 391.254810476919,0, 0, 1 };
cameraMatrix = Mat(camera);
cameraMatrix = cameraMatrix.reshape(1,3);
vector<double> dist = { 0.1961793476399528, -1.38146317350581, -0.002301820186177369, -0.001054637905895881, 2.458286937422959 };
distCoeffs = Mat(dist);
distCoeffs = distCoeffs.reshape(1, 1);
然後，調用攝像頭
VideoCapture capture;
capture.open(0);
if (!capture.isOpened())
{
cout << "can't open camera" << endl;
exit(-1);
}
接着在每一幀圖像中都進行aruco標記的檢測以及姿態估計，並繪製出每一幀中檢測到的aruco標記的座標軸。
Mat frame;
while (capture.read(frame))
{
Mat test_image;
resize(frame, test_image, Size(800, 800));
imshow("test_image", test_image);
auto dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_6X6_250);
vector<vector<Point2f>>corners, rejectedImgPoints;
vector<int>ids;
auto parameters = aruco::DetectorParameters::create();
aruco::detectMarkers(test_image, dictionary, corners, ids, parameters, rejectedImgPoints);
aruco::drawDetectedMarkers(test_image, corners, ids, Scalar(0, 255, 0));
//namedWindow("dectect", WINDOW_FREERATIO);
//imshow("dectect", test_image);
std::vector<cv::Vec3d> rvecs;
std::vector<cv::Vec3d> tvecs;
cv::aruco::estimatePoseSingleMarkers(corners, 0.053, cameraMatrix, distCoeffs, rvecs, tvecs);
for (int i = 0;i < rvecs.size();i++)
{
//繪製座標軸，檢查姿態估計結果
cv::aruco::drawAxis(test_image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.02);
}
//namedWindow("pose", WINDOW_FREERATIO);
imshow("pose", test_image);
char ch = cv::waitKey(1);
if (27 == ch)
{
break;
}
}
下面是效果演示：
到此我們就實現了對於簡單的aruco標記進行實時姿態估計的功能啦，本次筆記也就到此結束。
PS：本人的註釋比較雜，既有自己的心得體會也有網上查閱資料時摘抄下的知識內容，所以如有雷同，純屬我向前輩學習的致敬，如果有前輩覺得我的筆記內容侵犯了您的知識產權，請和我聯繫，我會將涉及到的博文內容刪除，謝謝！