标记姿态估计

在三维空间中，可通过标记角点的精确位置来估计摄像机与标记之间的变换。此操作称为二维到三维的姿态估计。该估计过程会在物体与摄像机之间找到一个欧氏空间的变换（该变换仅由旋转和坐标平移构成）。

先来观察右图：


 

图中的C表示摄像机中心，点P1-P4是现实坐标系中的三维点，而p1-p4是将点P1-P4投影到摄像机的图像平面而得到的。标记位置估计的目的就是在已知三维世界的标记位置（p1-p4）、有内部参数矩阵的摄像机C以及已知图像平面的投影点（P1-P4）的情况下，找到标记与摄像机之间相对变换关系。但在哪儿能得到三维空间中标记位置的坐标呢？可以想象一下，标记总是方形且所有顶点都在一个平面上，因此可按如右方式定义标记的角点：

可将标记放在XY平面上（Z分量为零），其标记的中心为点（0.0, 0.0, 0.0）。这是一个很好的提示，因为在这种情形下，坐标系统的起始处就是标记的中心（Z轴是与标记平面垂直）。

为了用已知2D-3D的对应关系来找到摄像机位置，可用cv::solvePnP函数：

其中，参数objectPoints是输入数组，它包含着对象所在空间的点，这里可将std::vector类型所定义的变量传递给该参数。只要是3×N或N×3的OpenCV矩阵也都作为该参数的输入，其中N是点的数量。本项目将三维空间中标记坐标的列表传递给该参数。

数组imagePoints是相应图像点（或投影）数组。该参数的类型也是std::vector 或cv::Mat，其大小为2×N或N×2的矩阵， N表示点的数量。本项目将找到的标记顶点列表传递给该参数。

cameraMatrix是3×3的摄像机内部参数矩阵；

distCoeffs是4×1、1×4、5×1或1×5的畸变向量；

rvec是输出的旋转向量，该向量将点由模型坐标系统转换为摄像机坐标系统；

tvec是输出的平移向量；

变量useExtrinsicGuess的值为true，函数将使用所提供的rvec和tvec向量作为旋转和平移的初始值，然后会不断优化它们。

函数按最小化重投影误差来计算摄像机的变换，即让所得的投影向量imagePoints与被投影向量之间的距离平方和最小。

被估计的变换由旋转向量（rvec）和平移向量（tvec）来定义，这也称为欧氏变换或刚性变换。

刚性变换的正式定义为：对任意向量v，会得到一个变换向量T（v），其具体形式为

T（v） = Rv + t

其中，RT=R-1（即R为正交变换），t是一个平移向量，它给出了整个变换的起始点。另外，刚性变换还有一个性质：

det（R）=1

这意味着R只是旋转（一个保持方向的正交变换）并不会产生反射。

可使用函数cv::Rodrigues将旋转向量转换为3×3的旋转矩阵。该函数将旋转向量转变为与之等价的旋转矩阵。

注意：cv::solvePnP将在三维空间找到与标记姿态的摄像机位置，因此需要反转所得到的变换。所得变换将用摄像机的坐标系统来描绘标记变换，该坐标系统对渲染过程更方便。

下面给出了函数estimatePosition的实现过程，它会找到给定标记的位置。