上一篇簡單介紹了ArUco的安裝與使用，這一篇用ArUco來實作相機的位姿估計。
一、用單個marker實作相機的位姿估計
我們從建立一個工程做起：
- 第1步，建立工程目錄
找個你比較常用的檔案夾，在此檔案夾下打開終端，運作以下指令：
$ mkdir m_aruco
$ cd m_aruco/
$ mkdir bin include lib src test yml
$ touch CMakeLists.txt README
完了以後你的工程目錄裡應該是這樣的：
- 第2步，開始配置這個工程
在終端運作
$ gedit CMakelists.txt
，粘貼以下代碼：
# ----------------------------------------------------------------------------
# Basic Configuration
# ----------------------------------------------------------------------------
cmake_minimum_required(VERSION 3.0)
project(m_aruco LANGUAGES CXX)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_CXX_STANDARD 11) # C++11...
set(CMAKE_CXX_STANDARD_REQUIRED ON) #...is required...
set(CMAKE_CXX_EXTENSIONS ON) #...with compiler extensions like gnu++11
set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )
set( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib )
find_package( OpenCV REQUIRED )
# set(aruco_DIR "/home/exbot/OpenCV/aruco306-installed/share/aruco/cmake")
# find_package( aruco REQUIRED )
# If could not find could not find a package configuration file provided by "aruco",
# set the "aruco_INCLUDE_DIRS" and the "aruco_LIBS" by mabual setting directly.
set(aruco_INCLUDE_DIRS /home/exbot/OpenCV/aruco306-installed/include/aruco)
set(aruco_LIBS /home/exbot/OpenCV/aruco306-installed/lib/libaruco.so)
include_directories( include ${OpenCV_INCLUDE_DIRS} ${aruco_INCLUDE_DIRS})
add_executable( cam_pose_estimator test/cam_pose_estimator.cpp )
target_link_libraries( cam_pose_estimator ${OpenCV_LIBS} ${aruco_LIBS} )
- 第3步，添加源檔案
cd 到test檔案夾下，運作指令：
$ touch cam_pose_estimator.cpp
$ gedit cam_pose_estimator.cpp
然後粘貼以下代碼：
/*****************************************************/
/* By Li Xujie */
/*****************************************************/
#include "aruco.h"
#include "cvdrawingutils.h"
#include <opencv2/opencv.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <stdexcept>
using namespace std;
using namespace cv;
using namespace aruco;
MarkerDetector MDetector;
VideoCapture TheVideoCapturer;
vector<Marker> TheMarkers;
Mat TheInputImage,TheInputImageGrey, TheInputImageCopy;
CameraParameters TheCameraParameters;
int waitTime = 0;
int ref_id = 0;
bool isVideo=false;
class CmdLineParser{int argc;char** argv;public:CmdLineParser(int _argc, char** _argv): argc(_argc), argv(_argv){} bool operator[](string param) {int idx = -1; for (int i = 0; i < argc && idx == -1; i++)if (string(argv[i]) == param)idx = i;return (idx != -1);} string operator()(string param, string defvalue = "-1") {int idx = -1;for (int i = 0; i < argc && idx == -1; i++)if (string(argv[i]) == param)idx = i;if (idx == -1)return defvalue;else return (argv[idx + 1]);}};
struct TimerAvrg{std::vector<double> times;size_t curr=0,n; std::chrono::high_resolution_clock::time_point begin,end; TimerAvrg(int _n=30){n=_n;times.reserve(n); }inline void start(){begin= std::chrono::high_resolution_clock::now(); }inline void stop(){end= std::chrono::high_resolution_clock::now();double duration=double(std::chrono::duration_cast<std::chrono::microseconds>(end-begin).count())*1e-6;if ( times.size()<n) times.push_back(duration);else{ times[curr]=duration; curr++;if (curr>=times.size()) curr=0;}}double getAvrg(){double sum=0;for(auto t:times) sum+=t;return sum/double(times.size());}};
TimerAvrg Fps;
char cam_pose[100];
char cam_vect[100];
void putText(cv::Mat &im,string text,cv::Point p,float size){
float fact=float(im.cols)/float(640);
if (fact<1) fact=1;
cv::putText(im,text,p,FONT_HERSHEY_SIMPLEX, size,cv::Scalar(0,0,0),3*fact);
cv::putText(im,text,p,FONT_HERSHEY_SIMPLEX, size,cv::Scalar(125,255,255),1*fact);
}
void printInfo(cv::Mat &im){
float fs=float(im.cols)/float(1200);
putText(im, "fps="+to_string(1./Fps.getAvrg()),cv::Point(10,fs*30),fs*1.0f);
putText(im, cam_pose, cv::Point(10,fs*60),fs*1.0f);
putText(im, cam_vect, cv::Point(10,fs*90),fs*1.0f);
}
int main(int argc, char** argv)
{
try
{
CmdLineParser cml(argc, argv);
if (argc < 2 || cml["-h"])
{
cerr << "Invalid number of arguments" << endl;
cerr << "Usage: (in.avi|live[:camera_index(e.g 0 or 1)]) [-c camera_params.yml] [-s marker_size_in_meters] [-d dictionaty:"
"ALL_DICTS by default] [-ref_id reference_marker's_id for estimating pose of camera] [-e use EnclosedMarker or not] [-h]" << endl;
return false;
}
/// PARSE ARGUMENTS
string TheInputVideo = argv[1];
ref_id = std::stoi(cml("-ref_id"));
if(-1 == ref_id){cout<<"You need indicate a reference_marker by use the parameter [-ref_id].\n"<<endl;return false;}
// read camera parameters if passed
float TheMarkerSize = std::stof(cml("-s", "-1"));
if (cml["-c"]) TheCameraParameters.readFromXMLFile(cml("-c"));
MDetector.setDictionary(cml("-d", "ALL_DICTS") ); // sets the dictionary to be employed (ARUCO,APRILTAGS,ARTOOLKIT,etc)
MDetector.getParameters().detectEnclosedMarkers(std::stoi(cml("-e", "0"))); //if use enclosed markers, set -e 1(true), or set -e 0(false). Default value is 0.
std::map<uint32_t, MarkerPoseTracker> MTracker; // use a map so that for each id, we use a different pose tracker
/// OPEN VIDEO
// read from camera or from file
if (TheInputVideo.find("live") != string::npos)
{
int vIdx = 0;
// check if the :idx is here
char cad[100];
if (TheInputVideo.find(":") != string::npos)
{
std::replace(TheInputVideo.begin(), TheInputVideo.end(), ':', ' ');
sscanf(TheInputVideo.c_str(), "%s %d", cad, &vIdx);
}
cout << "Opening camera index " << vIdx << endl;
TheVideoCapturer.open(vIdx);
waitTime = 10;
isVideo=true;
}
else{
TheVideoCapturer.open(TheInputVideo);
if ( TheVideoCapturer.get(CV_CAP_PROP_FRAME_COUNT)>=2) isVideo=true;
}
// check video is open
if (!TheVideoCapturer.isOpened()) throw std::runtime_error("Could not open video");
cv::namedWindow("in",cv::WINDOW_AUTOSIZE);
/ CONFIGURE DATA
// read first image to get the dimensions
TheVideoCapturer >> TheInputImage;
if (TheCameraParameters.isValid()) TheCameraParameters.resize(TheInputImage.size());
char key = 0;
int index = 0,indexSave=0;
// capture until press ESC or until the end of the video
do
{
TheVideoCapturer.retrieve(TheInputImage);
TheInputImage.copyTo(TheInputImageCopy);
Fps.start();
// TheMarkers = MDetector.detect(TheInputImage, TheCameraParameters, TheMarkerSize);
TheMarkers = MDetector.detect(TheInputImage);
for (auto& marker : TheMarkers) // for each marker
MTracker[marker.id].estimatePose(marker, TheCameraParameters, TheMarkerSize); // call its tracker and estimate the pose
Fps.stop();
for (unsigned int i = 0; i < TheMarkers.size(); i++)
{
if(ref_id == TheMarkers[i].id)
{
Mat camPosMatrix, camVecMatrix;
Mat vect = (Mat_<float>(3,1)<<0,0,1);
// R、t矩陣法
Mat rMatrix, tMatrix;
Rodrigues(TheMarkers[i].Rvec, rMatrix);
transpose(TheMarkers[i].Tvec, tMatrix);
camPosMatrix = rMatrix.inv()*(-tMatrix);
camVecMatrix = rMatrix.inv()*vect;
cout << "Camara Position: " << camPosMatrix.t() << "\nCamera Direction: " << camVecMatrix.t() << endl;
// 齊次矩陣法
Mat RTinv = MTracker[ref_id].getRTMatrix().inv();
camPosMatrix = RTinv(Rect(3,0,1,3)).clone();
camVecMatrix = RTinv(Range(0,3),Range(0,3))*vect;
sprintf(cam_pose,"Camera Position: px=%f, py=%f, pz=%f", camPosMatrix.at<float>(0,0),
camPosMatrix.at<float>(1,0), camPosMatrix.at<float>(2,0));
sprintf(cam_vect,"Camera Direction: dx=%f, dy=%f, dz=%f", camVecMatrix.at<float>(0,0),
camVecMatrix.at<float>(1,0), camVecMatrix.at<float>(2,0));
cout << TheMarkers[i].dict_info << " " << TheMarkers[i].id << endl;
cout << "Rvec = " << TheMarkers[i].Rvec << endl << "Tvec = " << TheMarkers[i].Tvec << endl;
cout << TheMarkers[i][0] << TheMarkers[i][1] << TheMarkers[i][2] << TheMarkers[i][3] << endl << endl;
CvDrawingUtils::draw3dAxis(TheInputImageCopy, TheMarkers[i], TheCameraParameters);
}
TheMarkers[i].draw(TheInputImageCopy, Scalar(0, 0, 255), 2, true);
// CvDrawingUtils::draw3dCube(TheInputImageCopy, TheMarkers[i], TheCameraParameters);
}
// show input with augmented information and the thresholded image
printInfo(TheInputImageCopy);
cv::imshow("in", TheInputImageCopy);
key = cv::waitKey(waitTime); // wait for key to be pressed
if (key == 's') waitTime = waitTime == 0 ? 10 : 0;
index++; // number of images captured
if (isVideo) if ( TheVideoCapturer.grab()==false) key=27;
} while (key != 27 );
}
catch (std::exception& ex) {cout << "Exception :" << ex.what() << endl; }
}
這個程式是我在aruco_test.cpp源碼的基礎上做的删改，有興趣的同學可以對比一下，我删掉了很多不必要的東西，盡管如此，這一段代碼還是有點長。另外，我對庫函數Marker::draw()的源碼做了一點修改，隻是做了一些marker角點的可視化操作，對本程式的運作不構成影響，故這裡說明一下不再貼出代碼。
- 第4步，編譯運作
cd 到m_aruco目錄下，運作指令：
$ mkdir build
$ cd build
$ cmake ..
$ make
如果你的OpenCV和ArUco都安裝無誤，編譯過程應該是不會報錯的。編譯成功後會在bin目錄下生成一個名為“cam_pose_estimator”的可執行檔案。在運作這個程式之前，請将你之前生成的相機标定檔案copy到yml檔案夾下，并準備好你的二維碼，測出它的邊長。準備就緒以後，cd 到m_aruco目錄下，運作指令：
$ ./bin/cam_pose_estimator live:1 -c yml/out_camera_calibration.yml -s 0.135 -d ARUCO -ref_id 4 -e 1
稍微解釋一下：-c代表相機标定檔案，-s代表marker的邊長，-d代表marker的字典，-ref_id代表參考marker的id（估計的相機位姿以該ID的marker為參考坐标系），-e代表使用的是enclosed_marker。
運作結果如上圖所示，“Camera Position”表示在marker4坐标系下的三維坐标，“Camera Direction”表示在marker4坐标系下相機的朝向。理想情況下，如果相機正對marker4，則CameraDirection=(0, 0, -1)。終端列印的是參考marker的字典資訊以及它的ID，還有marker坐标系到相機坐标系的旋轉向量和平移向量，以及marker每個角點的像素坐标等。終端與圖像中顯示的相機位姿是用不同方法算出來的，大家可參照代碼進行對比，二者結果完全一樣。
如果你發現程式估計的相機位姿與相機真實的位姿差距較大，那麼多半是你的相機标定得不準。這裡不得不說，用上一篇提到的标定方法标定出來的結果非常不準，是以還是建議大家用OpenCV的官方例程重新标定一遍。（OpenCV的官方标定例程在OpenCV源碼包的samples/cpp檔案夾下，名為calibration.cpp，具體怎麼用就不說了）
二、原了解釋
1、代碼流程
個人覺得，ArUco的源碼寫得挺亂的，格式很不規整。可能因為鄙人的C++水準太差，反正我費了好大勁才搞明白程式的整個流程。下面将整個過程描述一下：
（1） 進入main()函數，根據你給的入口參數進行一系列配置，初始化一個非常重要的對象
MarkerDetector MDetector;
。
（2） 進入
do{ }while(key != 27 );
循環，相機裝置傳入圖像資料，開始執行一個非常重要的函數
TheMarkers = MDetector.detect(TheInputImage, TheCameraParameters, TheMarkerSize);
。下面跟随這個函數，來看看marker的檢測、識别和位姿估計過程。這時你可能需要一個好用的IDE打開你的程式，因為接下來我們要在ArUco的源碼中跳來跳去。
- <1> 跳轉到函數的定義
std::vector<aruco::Marker> MarkerDetector::detect(const cv::Mat& input, const CameraParameters& camParams, float markerSizeMeters, bool setYPerperdicular)
void MarkerDetector::detect(const cv::Mat& input, std::vector<Marker>& detectedMarkers, CameraParameters camParams, float markerSizeMeters, bool setYPerpendicular)
detect(input, detectedMarkers, camParams.CameraMatrix, camParams.Distorsion, markerSizeMeters,setYPerpendicular);
void MarkerDetector::detect(const cv::Mat& input, vector<Marker>& detectedMarkers, Mat camMatrix, Mat distCoeff, float markerSizeMeters, bool setYPerpendicular)
- <2> ConvertGrey：彩色圖轉灰階圖
cv::cvtColor(input,grey,CV_BGR2GRAY);
- <3> CreateImageToThreshold：建立一個用于進行圖像門檻值分割的Mat對象imgToBeThresHolded。這裡開始出現一個叫“_params”的結構體對象，由于我們沒有傳入對Detector的參數配置檔案，是以_params中所有的變量都采用預設值。這樣的話，因為
if ( _params.lowResMarkerSize<minpixsize )
imgToBeThresHolded=grey;
- <4> BuildPyramid：建構高斯金字塔。因為_params.maxThreads的預設值是1，是以直接執行
buildPyramid(imagePyramid,grey,2*getMarkerWarpSize());
- <5> Threshold and Detect rectangles：門檻值化圖像并進行初始矩形檢測。進入函數
MarkerCanditates=thresholdAndDetectRectangles(imgToBeThresHolded );
thresholdAndDetectRectangles_thread ( );
vector< MarkerDetector::MarkerCandidate> MarkerDetector::thresholdAndDetectRectangles(const cv::Mat & input, int thres_param1,int thres_param2,bool enclosed,cv::Mat &auxThresImage)
cv::adaptiveThreshold(input, auxImage, 255., ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY_INV, static_cast<int>(thres_param1), static_cast<int>(thres_param2));
cv::findContours(auxThresImage, contours, cv::noArray(), CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
cv::approxPolyDP(contours[i], approxCurve, double(contours[i].size()) * 0.05, true);
for (int j = 0; j < 4; j++) MarkerCanditates.back().push_back( Point2f(static_cast<float>(approxCurve[j].x),static_cast<float>(approxCurve[j].y)));
MarkerCanditates.back().contour=contours[i];
joinVectors( _vcandidates,joined);
- <6> prefilterCandidates：篩選MarkerCanditates，進入函數
MarkerCanditates=prefilterCandidates(MarkerCanditates,imgToBeThresHolded.size());
for(size_t i=0;i<MarkerCanditates.size();i++) if (!toRemove[i])finalCandidates.push_back(MarkerCanditates[i]);
- <7> Marker candidate classification：檢測識别MarkerCanditates，通過解析其内部編碼确定哪些是真正的marker。為了達到這個目的，先進行一步透視變換，來得到每個矩形規範的形态（正視圖），
warp( inToWarp, canonicalMarker, Size(markerWarpSize,markerWarpSize),points2d_pyr);
bool DictionaryBased::detect(const cv::Mat& in, int& marker_id, int& nRotations, std::string &additionalInfo)
cv::threshold(grey, grey, 125, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
getInnerCode(grey,nbits,ids);
if ( dic->is( ids[rot]))
marker_id = dic->at (ids[rot]);
nRotations = rot;
additionalInfo=dic->getName();
detectedMarkers.push_back(MarkerCanditates[i]);
std::rotate(detectedMarkers.back().begin(), detectedMarkers.back().begin() + 4 - nRotations, detectedMarkers.back().end());
- <8> Remvoe Duplicated：剔除一些可能被重複檢測的marker。
removeElements(detectedMarkers,toRemove);
- <9> Corner Refinement：細化marker角點的像素坐标。由于_params.cornerRefinementM的預設值為CORNER_SUBPIX，是以執行函數
cornerSubPix(grey, Corners, cvSize(halfwsize,halfwsize), cvSize(-1, -1),cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 12, 0.005));
- <10> Pose Estimation：估計marker的位姿，求取Rvec（旋轉向量）和Tvec（平移向量）。跳轉到函數
void Marker::calculateExtrinsics(float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff, bool setYPerpendicular)
vector<cv::Point3f> objpoints = get3DPoints(markerSizeMeters);
solvePnP(objpoints, *this,camMatrix, distCoeff,raux,taux);
void solvePnP(const vector<cv::Point3f>& objPoints, const std::vector<cv::Point2f>& imgPoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,cv::Mat &rvec,cv::Mat &tvec)
solvePoseOfCentredSquare(markerLength, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, reprojErr1, Rvec2, Tvec2, reprojErr2);
（3） 我的天呐！這個函數終于跑完了！！回到主函數！！！現在我們已經檢測到了圖像中所有的marker，并且得到了每個marker的id、角點像素坐标以及非常重要的Rvec和Tvec等資訊，下面就來計算相機在參考marker下的位姿。因為Rvec和Tvec是從marker坐标系到相機坐标系的變換，而我們要求的是相機在marker坐标系下的位姿（由相機坐标系變換到marker坐标系），是以
camPosMatrix = rMatrix.inv()*(-TheMarkers[i].Tvec);
，
camVecMatrix=rMatrix.inv()*vect;
。
（4） 列印顯示輸出一些資訊，這裡沒什麼要說的，代碼一看就明白。
至此，整個程式的工作流程大體介紹完了。接下來說一說這個PnP，看看它是怎麼用二維碼的四個角點求出Rvec和Tvec的。
2、PnP原理
這個地方我可能講得不好，先放上Wikipedia裡關于PnP的解釋：Perspective-n-Point 。在下才疏學淺，有許多地方自己還不是很了解，如不嫌棄，各位将就着往下看吧！
PnP（Perspective-n-Point）是求解3D到2D點對運動的方法。它描述了當知道n個三維空間點坐标及其二維投影位置時，如何估計相機的位姿。試想一下，在一幅圖像中，我們隻要知道其中至少三個點的3D空間坐标就可以用于估計相機的運動以及相機的姿态（需要至少一個額外點驗證結果）。PnP問題，就是指通過世界中的n個特征點與圖像中對應的n個像點，計算出其投影關系，進而獲得相機或物體位姿的問題。 好吧，我們看圖說話：
上圖中，我們以二維碼的坐标系為世界坐标系，則二維碼四個角點的三維空間坐标分别為 A = ( − s / 2 , s / 2 , 0 ) T A=(-s/2, s/2, 0)^T A=(−s/2,s/2,0)T、 B = ( s / 2 , s / 2 , 0 ) T B=(s/2, s/2, 0)^T B=(s/2,s/2,0)T、 C = ( s / 2 , − s / 2 , 0 ) T C=(s/2, -s/2, 0)^T C=(s/2,−s/2,0)T、 D = ( − s / 2 , − s / 2 , 0 ) T D=(-s/2, -s/2, 0)^T D=(−s/2,−s/2,0)T。假設它們在圖像中對應投影點的像素坐标分别為 a = ( u a , v a ) T a=(u_a, v_a)^T a=(ua,va)T、 b = ( u b , v b ) T b=(u_b, v_b)^T b=(ub,vb)T、 c = ( u c , v c ) T c=(u_c, v_c)^T c=(uc,vc)T、 d = ( u d , v d ) T d=(u_d, v_d)^T d=(ud,vd)T，相機的内參矩陣 K K K已知，則三維空間坐标與像素坐标之間的轉換關系為（以A-a為例）：
z [ a 1 ] = z [ u a v a 1 ] = K [ R ∣ t ] [ A 1 ] = [ f x 0 u 0 0 f y v 0 0 0 1 ] [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] [ − s / 2 s / 2 0 1 ] (1) z\begin{bmatrix}a\\ 1 \\ \end{bmatrix} = z\begin{bmatrix} u_a \\ v_a \\ 1 \\ \end{bmatrix} = K \begin{bmatrix} R|t \end{bmatrix} \begin{bmatrix} A\\1 \end{bmatrix} = \begin{bmatrix} f_x&0&u_0 \\0 &f_y&v_0\\0&0&1\end{bmatrix} \left[ \begin{array}{ccc|c} r_{11}&r_{12}&r_{13}&t_1 \\ r_{21}&r_{22}&r_{23}&t_2 \\ r_{31}&r_{32}&r_{33}&t_3 \end{array} \right] \begin{bmatrix}-s/2\\s/2\\0\\1\end{bmatrix}\tag 1 z[a1]=z⎣⎡uava1⎦⎤=K[R∣t][A1]=⎣⎡fx000fy0u0v01⎦⎤⎣⎡r11r21r31r12r22r32r13r23r33t1t2t3⎦⎤⎣⎢⎢⎡−s/2s/201⎦⎥⎥⎤(1)
其中的 R = [ r 1 r 2 r 3 ] = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\begin{bmatrix}\bf r_1&\bf r_2&\bf r_3\end{bmatrix}=\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix} R=[r1r2r3]=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤、 t = [ t 1 t 2 t 3 ] t=\begin{bmatrix}t_1\\t_2\\t_3\end{bmatrix} t=⎣⎡t1t2t3⎦⎤即為我們要求的旋轉矩陣與平移向量，其實它倆就是所謂相機的外參。通過相機的外參可以将世界坐标系下的點轉換到相機坐标系下表示，通過相機的内參可以将相機坐标系下的點轉換到像素坐标系下表示。
為了簡化表示，我們暫時不考慮相機内參（因為内參 K K K是已知且不變的），并令 P A = [ A 1 ] P_A=\begin {bmatrix} A \\ 1 \end {bmatrix} PA=[A1]、 T = [ R ∣ t ] = [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] = [ t 11 t 12 t 13 t 14 t 21 t 22 t 23 t 24 t 31 t 32 t 33 t 34 ] = [ τ 1 τ 2 τ 3 ] T=\begin{bmatrix}R|t\end{bmatrix}=\left[\begin{array}{ccc|c}r_{11}&r_{12}&r_{13}&t_1\\r_{21}&r_{22}&r_{23}&t_2\\r_{31}&r_{32}&r_{33}&t_3\end{array}\right]=\begin{bmatrix}t_{11}&t_{12}&t_{13}&t_{14}\\t_{21}&t_{22}&t_{23}&t_{24}\\t_{31}&t_{32}&t_{33}&t_{34}\end{bmatrix} = \begin{bmatrix}\bf{\tau_1}\\ \bf{\tau_2}\\ \bf{\tau_3}\end{bmatrix} T=[R∣t]=⎣⎡r11r21r31r12r22r32r13r23r33t1t2t3⎦⎤=⎣⎡t11t21t31t12t22t32t13t23t33t14t24t34⎦⎤=⎣⎡τ1τ2τ3⎦⎤，則（1）式可表示為：
z [ u a v a 1 ] = [ t 11 t 12 t 13 t 14 t 21 t 22 t 23 t 24 t 31 t 32 t 33 t 34 ] [ A 1 ] = [ τ 1 τ 2 τ 3 ] P A z\begin{bmatrix} u_a \\ v_a \\ 1 \\ \end{bmatrix} =\begin{bmatrix}t_{11}&t_{12}&t_{13}&t_{14}\\t_{21}&t_{22}&t_{23}&t_{24}\\t_{31}&t_{32}&t_{33}&t_{34}\end{bmatrix}\begin{bmatrix}A\\1\end{bmatrix} = \begin{bmatrix}\bf{\tau_1}\\ \bf{\tau_2}\\ \bf{\tau_3}\end{bmatrix}P_A z⎣⎡uava1⎦⎤=⎣⎡t11t21t31t12t22t32t13t23t33t14t24t34⎦⎤[A1]=⎣⎡τ1τ2τ3⎦⎤PA
用最後一行把 z z z 消去（ z = τ 3 P A z={\bf\tau_3}P_A z=τ3PA），得到兩個限制：
u a = τ 1 P A τ 3 P A ， v a = τ 2 P A τ 3 P A u_a=\frac{{\bf\tau_1}P_A}{{\bf\tau_3}P_A} \quad，v_a=\frac{{\bf\tau_2}P_A}{{\bf\tau_3}P_A} \quad ua=τ3PAτ1PA，va=τ3PAτ2PA
于是有：
{ τ 1 P A − τ 3 P A u a = 0 , τ 2 P A − τ 3 P A v a = 0. (2) \begin{cases} {\bf\tau_1}P_A-{\bf\tau_3}P_Au_a=0, \\ {\bf\tau_2}P_A-{\bf\tau_3}P_Av_a=0. \end{cases}\tag 2 {τ1PA−τ3PAua=0,τ2PA−τ3PAva=0.(2)
可以看到，每個特征點提供了兩個關于 T T T 的線型限制，而 T T T 中有12個未知數，理論上至少需要6對比對點才能解出矩陣 T T T 。但我們知道，旋轉矩陣 R ∈ SO ( 3 ) R \in \text{SO}(3) R∈SO(3)是一個機關正交陣，自身具有六個限制：
{ r 1 T ⋅ r 2 = r 1 T ⋅ r 3 = r 2 T ⋅ r 3 = 0 ∥ r 1 ∥ = ∥ r 2 ∥ = ∥ r 3 ∥ = ∣ R ∣ = 1 \begin{cases}{\bf r_1}^\text{T}\cdot{\bf r_2}={\bf r_1}^\text{T}\cdot{\bf r_3}={\bf r_2}^\text{T}\cdot{\bf r_3}=0\\ {\bf\| r_1\|}={\bf\| r_2\|}={\bf\| r_3\|}=|{\bf R}|=1\end{cases} {r1T⋅r2=r1T⋅r3=r2T⋅r3=0∥r1∥=∥r2∥=∥r3∥=∣R∣=1
這樣的話，我們正好用二維碼的三個角點加上旋轉矩陣自身的六個限制就可以求得 R R R、 t t t。下面我用另一種方式再強行解釋一波：
認真的同學應該會發現，由于我們的世界坐标系建在二維碼之上，導緻每個角點的 z z z 坐标都變成了 0，這就在無形中給我們增加了 4 個條件。不妨把（1）式重新寫一遍：
z [ u a v a 1 ] = K [ R ∣ t ] [ A 1 ] = K [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] [ − s / 2 s / 2 0 1 ] = [ r 11 r 12 t 1 r 21 r 22 t 2 r 31 r 32 t 3 ] [ − s / 2 s / 2 1 ] = [ τ ~ 1 τ ~ 2 τ ~ 3 ] P A ~ = T ~ P A ~ (3) z\begin{bmatrix} u_a \\ v_a \\ 1 \\ \end{bmatrix} = K \begin{bmatrix} R|t \end{bmatrix} \begin{bmatrix} A\\1 \end{bmatrix} =K \left[ \begin{array}{ccc|c} r_{11}&r_{12}&r_{13}&t_1 \\ r_{21}&r_{22}&r_{23}&t_2 \\ r_{31}&r_{32}&r_{33}&t_3 \end{array} \right] \begin{bmatrix}-s/2\\s/2\\0\\1\end{bmatrix}=\left[ \begin{array}{cc|c} r_{11}&r_{12}&t_1 \\ r_{21}&r_{22}&t_2 \\ r_{31}&r_{32}&t_3 \end{array} \right] \begin{bmatrix}-s/2\\s/2\\1\end{bmatrix}=\begin{bmatrix}\bf{\tilde\tau_1}\\ \bf\tilde\tau_2\\ \bf\tilde\tau_3\end{bmatrix}\tilde{P_A}=\tilde T\tilde{P_A} \tag 3 z⎣⎡uava1⎦⎤=K[R∣t][A1]=K⎣⎡r11r21r31r12r22r32r13r23r33t1t2t3⎦⎤⎣⎢⎢⎡−s/2s/201⎦⎥⎥⎤=⎣⎡r11r21r31r12r22r32t1t2t3⎦⎤⎣⎡−s/2s/21⎦⎤=⎣⎡τ~1τ~2τ~3⎦⎤PA~=T~PA~(3)
由式（3）同樣可得兩個關于 T ~ \tilde T T~ 的線型限制：
{ τ ~ 1 P A ~ − τ ~ 3 P A ~ u a = 0 , τ ~ 2 P A ~ − τ ~ 3 P A ~ v a = 0. (4) \begin{cases} {\bf{\tilde\tau_1}}\tilde{P_A}-{\bf\tilde\tau_3}\tilde{P_A}u_a=0, \\ {\bf\tilde\tau_2}\tilde{P_A}-{\bf\tilde\tau_3}\tilde{P_A}v_a=0. \end{cases}\tag 4 {τ~1PA~−τ~3PA~ua=0,τ~2PA~−τ~3PA~va=0.(4)
而此時的 T ~ \tilde T T~ 隻有 9 個未知數，再加上旋轉矩陣中的三個限制：
{ r 1 T ⋅ r 2 = 0 r 1 T ⋅ r 1 = r 2 T ⋅ r 2 = 1 \begin{cases}{\bf r_1}^\text{T}\cdot{\bf r_2}=0\\ {\bf r_1}^\text{T}\cdot{\bf r_1}={\bf r_2}^\text{T}\cdot{\bf r_2}=1\end{cases} {r1T⋅r2=0r1T⋅r1=r2T⋅r2=1
正好，我們用三個點便可求出 R R R、 t t t（ r 3 \bf r_3 r3可由 r 1 \bf r_1 r1、 r 2 \bf r_2 r2求得 r 3 = r 1 × r 2 \bf r_3=\bf r_1\times\bf r_2 r3=r1×r2），另一個點留作驗證。
以上隻是我在理論上強行解釋了通過二維碼的四個角點可以求得相機的外參 R R R、 t t t，然而，ArUco源碼中實際用的是一種叫 IPPE 的方法來求解PnP問題。源碼的具體實作真的是看不懂，有興趣的同學請參考這篇論文：Infinitesimal Plane-Based Pose Estimation。
3、坐标變換
下面簡單描述一下相機與marker之間的坐标變換：
相機外參描述了如何把點從世界坐标系轉換到相機坐标系，但并不表示相機在世界坐标系中的位置和姿态，相反，它表示了世界坐标系在相機坐标系下的姿态 R \,R\, R以及世界坐标系原點在相機坐标系下的位置 t \,t\, t（即相機坐标系可經過先平移 t \,t\, t，再旋轉 R \,R\, R得到世界坐标系），是以相機外參才能把世界坐标系中的點變換到相機坐标系下表示。要得到相機在世界坐标系中的位姿，隻需将外參矩陣求逆即可。
在目前這個問題中，我們用PnP求出了相機的外參 R \,R R、 t t t，對應程式中的Rvec、Tvec。設marker在marker坐标系下的坐标為 m P m = ( 0 , 0 , 0 ) T {}^{m}\!P_{m}=(0, 0, 0)^\text{T} mPm=(0,0,0)T ，則marker在相機坐标系下的坐标為 c P m = [ R ∣ t ] [ m P m 1 ] = R m P m + t = t {}^{c}\!P_{m}=\begin{bmatrix} R|t \end{bmatrix} \begin{bmatrix}{}^{m}\!P_{m}\\1 \end{bmatrix} = R\,{}^{m}\!P_{m}+ t=t cPm=[R∣t][mPm1]=RmPm+t=t。設相機在相機坐标系下的坐标為 c P c = ( 0 , 0 , 0 ) T {}^{c}\!P_{c}=(0, 0, 0)^\text{T} cPc=(0,0,0)T，相機在相機坐标系下的方向向量為 c P c V = ( 0 , 0 , 1 ) T {}^{c}\!P_{c}V=(0, 0, 1)^\text{T} cPcV=(0,0,1)T，則相機在marker坐标系下的坐标為 m P c = R − 1 ( c P c − t ) = R − 1 ( − t ) {}^{m}\!P_{c}=R^{-1}({}^{c}\!P_{c}-t)=R^{-1}(-t) mPc=R−1(cPc−t)=R−1(−t)，相機在marker坐标系下的方向向量為 m V c = R − 1 c V c {}^{m}V_{c}=R^{-1}{^c}V_{c} mVc=R−1cVc。
用齊次變換矩陣表示：設marker坐标系到相機坐标系的齊次變換為 T = [ R t 0 1 ] T=\begin{bmatrix} R&t\\0&1 \end{bmatrix} T=[R0t1]，則相機坐标系到marker坐标系的齊次變換為 T − 1 = [ R − 1 − R − 1 t 0 1 ] = [ R T − R T t 0 1 ] T^{-1}=\begin{bmatrix} R^{-1}&-R^{-1}t\\0&1 \end{bmatrix}=\begin{bmatrix} R^T&-R^Tt\\0&1 \end{bmatrix} T−1=[R−10−R−1t1]=[RT0−RTt1]，進而marker在相機坐标系下的位姿矩陣為 c P ~ m = T m P ~ m = [ R t 0 1 ] [ I m P m 0 1 ] = [ R t 0 1 ] [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] = [ R t 0 1 ] {}^{c}\!\tilde P_{m}=T\,{}^{m}\!\tilde P_{m}=\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}\begin{bmatrix} I&{^m}\!P_{m}\\0&1 \end{bmatrix}=\begin{bmatrix} R&t\\0&1 \end{bmatrix}\begin{bmatrix} 1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&1 \end{bmatrix}=\begin{bmatrix} R&t\\0&1 \end{bmatrix} cP~m=TmP~m=[R0t1][I0mPm1]=[R0t1]⎣⎢⎢⎡1000010000100001⎦⎥⎥⎤=[R0t1]，相機在marker坐标系下的位姿矩陣為 m P ~ c = T − 1 c P ~ c = [ R − 1 − R − 1 t 0 1 ] [ c V c c P c 0 1 ] = [ R − 1 c V c − R − 1 t 0 1 ] {^m}\!\tilde P{_c}{}=T^{-1}{^c}\!\tilde P{_c}{}=\begin{bmatrix} R^{-1}&-R^{-1}t\\0&1 \end{bmatrix}\begin{bmatrix}{^c}V{_c}& {^c}\!P{_c}\\0&1 \end{bmatrix}=\begin{bmatrix} R^{-1}{^c}V{_c}&-R^{-1}t\\0&1 \end{bmatrix} mP~c=T−1cP~c=[R−10−R−1t1][cVc0cPc1]=[R−1cVc0−R−1t1]。（此處公式推導不是很嚴謹，不必在意細節）
畢！
#####參考文獻
[1] https://www.zybuluo.com/codeep/note/163962
[2] https://en.wikipedia.org/wiki/Perspective-n-Point
[3] https://blog.csdn.net/ZJU_fish1996/article/details/72312514?locationNum=7&fps=1
[4] https://docs.google.com/document/d/1QU9KoBtjSM2kF6ITOjQ76xqL7H0TEtXriJX5kwi9Kgc/edit （此連結需要翻牆）