solvePnP

相机位姿估计

本文不带有 PnP 的推导，是以应用为主，记录如何使用 OpenCV 中的相关方法解决相机位姿估计问题。使用的 OpenCV 版本为 4.5.3。

本文记录的是个人对于一些概念的理解和看法，用一大堆废话来助你理解。

cv::solvePnP()

相机位姿估计就是利用特征点的3D坐标（世界坐标系下），和各自对应在图像上的坐标（像素坐标系），解算出相机坐标系相对于世界坐标系的位姿关系（旋转向量和平移向量），其核心方法就是使用 PnP 进行求解。

方法官方文档

bool cv::solvePnP(
    InputArray objectPoints, 
    InputArray imagePoints, 
    InputArray cameraMatrix, 
    InputArray distCoeffs, 
    OutputArray rvec, 
    OutputArray tvec, 
    bool useExtrinsicGuess = false, 
    int flags = SOLVEPNP_ITERATIVE
)

• objectPoints：[输入] 特征点的世界坐标，Nx3 1 通道或 1xN/Nx1 3 通道，其中 N 是坐标点的个数
  • 坐标值需为float型，不能为double型
  • 可传入mat类型，也可以传递 std::vector<cv::Point3d>
• imagePoints：[输入] 特征点在图像中的坐标，需要与前面输入的世界坐标一一对应，Nx2 1 通道或 1xN/Nx1 2 通道，其中 N 是坐标点的个数
  • 可传入mat类型，也可以传递 std::vector<cv::Point2d>
• cameraMatrix：[输入] 相机3x3内参矩阵
• distCoeffs：[输入] 相机畸变向量
• rvec：[输出] 解算得出的旋转向量，它与 tvec 一起能够将将3D点从世界坐标系转换到相机坐标系的2D坐标
• tvec：[输出] 解算得出的平移向量
• useExtrinsicGuess：若 flags = SOLVEPNP_INTERACTIVE，如果 useExtrinsicGuess = true，则函数将提供的 rvec 和 tvec 值分别用作旋转和平移向量的初始近似值，也就是你可以输入一个带有值的 rvec 和 tvec ，这样做函数会进一步优化 rvec 和 tvec
• flags：解决PnP问题的方法
  • SOLVEPNP_ITERATIVE：默认值，它通过迭代求出重投影误差最小的解作为问题的最优解
  • SOLVEPNP_EPNP：使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解
  • SOLVEPNP_P3P：使用非常经典的Gao的P3P问题求解算法
  • SOLVEPNP_DLS：OpenCV 4.5.3似乎已移除，使用此参数将会自动变为SOLVEPNP_EPNP
  • SOLVEPNP_UPNP：OpenCV 4.5.3似乎已移除，使用此参数将会自动变为SOLVEPNP_EPNP
  • SOLVEPNP_AP3P：
  • SOLVEPNP_IPPE：
  • SOLVEPNP_IPPE_SQUARE：
  • SOLVEPNP_SQPNP：

个人使用的 OpenCV 版本是 4.5.3，不清楚 SOLVEPNP_DLS 和 SOLVEPNP_UPNP 是具体在哪个版本开始移除的。

cv::Rodrigues()

cv::solvePnP()解算得出的是旋转向量，而为了求得以欧拉角形式表达的位姿关系，需要转换为旋转矩阵。旋转向量与旋转矩阵之间的变换又称为罗德里格斯变换（Rodrigues）。

方法官方文档

void cv::Rodrigues(	
    InputArray src,
    OutputArray dst,
    OutputArray jacobian = noArray() 
)

• src：[输入] 1x3 或 3x1 的旋转向量，或 3x3 的旋转矩阵
• dst：[输出] 对于输入，分别输出 3x3 的旋转矩阵，或1x3 或 3x1 的旋转向量
• jacobian：[可选输出] 输出 3x9 或 9x3 的雅可比矩阵，它是 dst 相对于 src 的偏导数的矩阵。

平移向量

cv::solvePnP() 的输出参数 tvec（平移向量），能够与 rvec（旋转向量）一起，将世界坐标系上的点转换为相机坐标系上的坐标。

tvec（平移向量）的实际意义就是，世界坐标系相对于相机坐标系的平移向量。

这个平移向量，各个分量（\(x、y、z\)）在数值上等于：在相机坐标系下，以相机坐标系原点为起点，终点为世界坐标系原点所表示的向量的各个分量的数值。

若上述无法理解，可以通过真实数据来理解：

在双目相机中，双目标定结果其中的一个数据就是左相机相对于右相机的平移向量
（MATLAB 工具结果的真实性，想不必多说了）。

上图红色框中，从左至右，便是左相机相对于右相机的平移向量的 \(x，y，z\) 分量数值；
camera1 为左相机，camera2 为右相机，其位姿如上图图中所示；
左右相机坐标系都为右手系，各个轴的朝向与 camera 1 所标注的形式一致。

左相机相对于右相机的平移向量，各个分量在数值上等于：在右相机坐标系下，以右相机坐标系原点为起点，终点为左相坐标系原点所表示的向量的各个分量的数值。

与上图对比，很明显，在右相机坐标系（camera 2）下，左相坐标系原点在右相机坐标系原点的左边，\(x\) 轴方向的数值是为负的（个人的双目相机装配时，基本只在 \(x\) 轴方向有差异，\(y、z\) 轴相差不大）。

左相机相对于右相机的平移向量，很容易误解为：在左相机坐标系下，以左相机坐标系原点为起点，终点为右相坐标系原点所表示的向量。这就与准确表示的平移向量的相反了。

旋转矩阵

就 cv::solvePnP() 的输出参数 rvec 而言，它与 tvec 一起能够将点从世界坐标系转换到相机坐标系。

rvec（旋转向量）的实际意义就是，世界坐标系相对于相机坐标系的旋转向量。

那么通过 rvec 转换为旋转矩阵（世界坐标系相对于相机坐标系的旋转矩阵），再转换为欧拉角，就能够直观的将相机坐标系通过欧拉角旋转坐标轴，使得相机坐标系的各坐标轴与世界坐标系的各坐标轴对应平行。

因为是相对的关系，世界坐标系相对于相机坐标系，那么就该以相机坐标系为基准，去看世界坐标系的各个坐标轴，是怎么用相机坐标系，通过旋转相机坐标系各个坐标轴，转到跟世界坐标系的各个坐标轴的方向一致的。因此是说“将相机坐标系通过欧拉角旋转坐标轴，使得相机坐标系的各坐标轴与世界坐标系的各坐标轴对应平行”，而不是反过来说“将世界坐标系通过欧拉角旋转坐标轴，使得世界坐标系的各坐标轴与相机坐标系的各坐标轴对应平行”，这里说的有些绕口了，但是自己比划比划想想也是能够解决的。

一样的，若上述无法理解，我们来通过真实数据来验证验证我说的：

左侧为：经过转换 cv::solvePnP() 解算得出的旋转向量为旋转矩阵，再以 zxy 顺规转化为的欧拉角，为了便于理解就不以 pitch、yaw、roll 来描述欧拉角了。

世界坐标系为：以装甲板中心为原点，\(z\) 轴垂直于装甲板向里的右手系；相机坐标系也是右手系，右手系，轴以逆时针旋转为正值。

假设这一帧的装甲板与相机成像平面完全平行，也就是忽略控制台上输出的数值。

这里先演示 \(z\) 轴，假设 \(x，y\) 轴没转动；
\(z\) 轴的逆时针旋转，在屏幕上观察到的是顺时钟转动装甲板的形式，\(z\) 轴顺时针旋转反之。

这一帧装甲板可以观察到它是顺时钟旋转了，观察左侧控制台输出变化，\(z\) 轴欧拉角数值变大。

这一帧装甲板可以观察到它是逆时钟旋转了，观察左侧控制台输出变化，\(z\) 轴欧拉角数值变小。

根据数值可以加以想象：相机坐标系的 \(z\) 轴，转动屏幕上的 \(z\) 轴欧拉角数值（正值表示逆时针旋转，负值顺时钟），就可以使得相机坐标系的各个轴变换到跟世界坐标系（装甲板）各个轴对应平行。

这个就可以简单的想象一下，如果是把世界坐标系（装甲板）的 \(z\) 轴，转动屏幕上的 \(z\) 轴欧拉角数值
并不能转回到相机坐标系，应该反向转动（也就是屏幕上若是负值，应该逆时针转，而不是顺时钟转）才能够转回到相机坐标系。

所以直观理解的：使得相机坐标系的各个轴变换到跟世界坐标系（装甲板）各个轴对应平行，是相驳的，不对的。

因此解出来的欧拉角，意义是相机坐标系通过转动这些欧拉角，使得相机坐标系的各轴对应与世界坐标系的各轴平行。

再来几幅图稍加想象一下就可以加深理解了：

相机坐标系的 \(x\) 轴如绿色箭头所示。
\(x\) 轴的逆时针转动在屏幕里，是向前倒的形式；
越往前倒，也就是逆时针转的越多，\(x\) 轴欧拉角数值上就大了；
越往后倒，也就是顺时钟转动，\(x\) 轴欧拉角数值上就小了。

这里是 \(y\) 轴，一样的道理。

\(y\) 轴的逆时针，在屏幕里观察的是什么样的形式，自己可以比划想象一下。

\(y\) 轴的逆时针，在屏幕里观察为：屏幕里的左侧灯条会向里去。

总的来说，平移矩阵和旋转矩阵都是表示相对的关系，注意是谁相对于谁。

比如讲旋转矩阵 \(R\) 和平移向量 \(t\)，能够使得坐标系 \(A\) 上的点转换为坐标系 \(B\) 上的坐标：

• \(R\) 表示坐标系 \(A\) 相对于坐标系 \(B\) 的旋转矩阵，意义是：能够使得，将 \(B\) 坐标系通过欧拉角旋转其坐标轴，使得 \(B\) 坐标系的各坐标轴与 \(A\) 坐标系的各坐标轴对应平行
• \(t\) 表示坐标系 \(A\) 相对于坐标系 \(B\) 的平移向量，这个平移向量，各个分量（x、y、z）在数值上等于：在 \(B\) 坐标系下，以 \(B\) 坐标系原点为起点，终点为 \(A\) 坐标系原点所表示的向量的各个分量的数值

而 \(R\) 和 \(t\) 会表示为 \(R_A^B\) 和 \(t_A^B\)，或者其它差不多的表达形式。

在世界坐标系下，相机坐标系原点的坐标

方法一

坐标系规则一

假设有两个坐标系\(A,B\)
其中坐标系\(B\)中的点\(b\)是由坐标系\(A\)中的点\(a\)转换来的，则点\(a\)和点\(b\)之间有如下等式

\[ a = {R_A^B} * b + {T_A^B} \\ b = {R_B^A} * a + {T_B^A} \]

由第二个式子可以得到：

\[\begin{aligned} a & = {R_B^A}^{-1}(b - {T_B^A}) \\ & = {R_B^A}^{-1} * b - {R_B^A}^{-1}{T_B^A} \end{aligned} \]

因为\({R_A^B} = {R_B^A}^{-1}\)，再与第一个式子相比较，可以得出：

\[ \begin{cases} a = {R_A^B} * b - {R_B^A}^{-1}{T_B^A} \\ a = {R_A^B} * b + {T_A^B} \end{cases} \]

\[ {T_A^B} = - {R_B^A}^{-1}{T_B^A} \]

cv::solvePnP中的rVec和tVec代表相机坐标系(\(C\))到世界坐标系(\(W\))的旋转向量和平移向量

将旋转向量转换为旋转矩阵可使用cv::Rodrigues进行转换

设rVec转换为旋转矩阵\(R_C^W\)，tVec转换为平移矩阵\(T_C^W\)

运用坐标系规则一，设\(A\)为世界坐标系\(W\)，\(B\)为相机坐标系\(C\)，那么有：

\[ {T_W^C} = - {R_C^W}^{-1}{T_C^W} \]

平移矩阵的性质
若平移矩阵\(T_A^B\)为，从\(A\)坐标系到\(B\)坐标系的平移矩阵
那么在\(A\)坐标系下，\(B\)坐标系原点的坐标就可以表示为\({(T_A^B)}^T\)
注：平移矩阵为\(3*1\)，用矩阵表示坐标为\(1*3\)，因此这里为矩阵转置

根据平移矩阵的性质，在世界坐标系下，相机坐标系原点的坐标可以表示为\({(^C_WT)}^T\)

方法二

设\(P_C\)为相机坐标系下的矩阵形式表示的坐标，\(P_W\)为世界坐标系下的矩阵形式表示的坐标

设rVec转换为旋转矩阵\(R_C^W\)，tVec转换为平移矩阵\(T_C^W\)

有如下关系：

\[ P_C = R_C^WP_W + T_C^W \]

令\(P_W = (0, 0, 0)^T\)，即表示世界坐标系原点
那么解得的\(P_C\)表示相机坐标系下，世界坐标系原点的坐标，此时\(P_C = T_C^W\)

令\(P_C = (0, 0, 0)^T\)，即表示相机坐标系原点
那么解得的\(P_W\)表示世界坐标系下，相机坐标系原点的坐标，此时\(P_W = {-R_C^W}^{-1}T_C^W\)，与方法一中的结果是一致的

设\(P_C\)为相机坐标系下的矩阵形式表示的坐标，\(P_W\)为世界坐标系下的矩阵形式表示的坐标

设rVec转换为旋转矩阵\(R_W^C\)，tVec转换为平移矩阵\(T_W^C\)

有如下关系：

\[ P_C = R_W^CP_W + T_W^C \]

令\(P_W = (0, 0, 0)^T\)，即表示世界坐标系原点
那么解得的\(P_C\)表示相机坐标系下，世界坐标系原点的坐标，此时\(P_C = T_W^C\)

令\(P_C = (0, 0, 0)^T\)，即表示相机坐标系原点
那么解得的\(P_W\)表示世界坐标系下，相机坐标系原点的坐标，此时\(P_W = {-R_W^C}^{-1}T_W^C\)，与方法一中的结果是一致的