上一篇简单介绍了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 （此链接需要翻墙）