PnP and Perspective Projection and Pose Computation

Review PnP problem from a computer graphics rendering view

(资料图)

首先从一个 StackExchange问题出发，下面是本人的回答摘录。

Intrinsic Matrix vs. Projection Matrix

What is the difference between Intrinsic Matrix( K ) and Perspective ProjectionMatrix(call it P Matrixlater)?

• For K Matrixit transform 3D points to 2D pixels in image space.And during this procedure only x and y value are concerned.
• For P Matrixit transform 3D points to NDC space.

Take a look at two matrices:

\[K = \begin{bmatrix}f_x& 0& c_x\\ 0& f_y& c_y\\0 & 0 & 1\\\end{bmatrix}\]\[P = \begin{bmatrix}\frac{1}{t*a}& 0& 0& 0\\0& \frac{1}{t}& 0& 0\\0 & 0 & A& B&\\ 0 & 0 & -1& 0\\ \end{bmatrix}\]\[t=tan(\frac{fovy}{2})\]\[a=\frac{width}{height}\]

Let"s add perspective divide and show the result of the above two matrices:

Intrinsic case: $$x_{2d} = \frac{x_0}{z_0 * \frac{1}{f_x}} + c_x$$

Perspective case: $$x_{2d} = \frac{x_0}{-z_0(t*a)}$$

Similar with some difference.

The image space: Origin from left-top cornerso should add Cx Cy as the offset from center to left-top corner.And in NDC space we assume Z-axis direct out of screen so P(3,2) = -1.

从该问题引申，继续思考： PnP 问题中的投影过程如何体现？

Dig into solvePnP

我们看一下 PnP 问题的描述，下面这个公式来自 OpenCV 文档

\[\begin{align*} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} &= \bf{A} \hspace{0.1em} \Pi \hspace{0.2em} ^{c}\bf{T}_w \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} \\ \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} &= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} \end{align*}\]

很遗憾，这里面是没有上一个段落所提及的 perspective divide.为了再次确认，直接看一下 solvePnP 的 DLT 办法的原理，参考这个文章

直接假设一个带有12个未知数的 3x4 的矩阵作为未知数，并忽略其中的Rt含义，然后将上式化简，建立一个关于未知3x4矩阵的方程。取6个点的数据共12行，就可以直接进行求解。具体求解过程中的 SVD 分解求最小二乘解的过程不赘述。

注意：

1. 整个过程没有透视除法 (perspective divide)
2. 投影后的z值全部为 1 或者说为 λ

这正是这套逻辑与图形渲染中透视投影的很大的一个区别。x y 的值没有根据 z 的大小进行缩放。z 全部投到了一个平面上。据此分析，这套方法与其叫做 Perspective-n-Point 不如叫做Orthographic-n-Point.

结论：

PnP 方法适用于正交投影下获得的 2D 点与原始世界坐标系中的 3D 点来计算相机位姿，如果想适用普通的透视投影渲染，得到对齐的结果，还要想想别的办法才行。

How to compute object pose with Perspective Projection

我们考虑一个图形学常见的透视投影，投影矩阵 P 见第一部分的定义，并加入透视除法 （最大的区别！）。

如果问题简化一下，假设 3D 物体和 2D 投影的朝向已经对齐，亦即旋转部分 R已经完成。那么剩下需要计算的就只剩下了平移，这里因为是透视投影，所以在 Z 轴的平移会直接影响最终成像的大小，这是之前 PnP 方法里面所没有涉及的。

如果旋转没有对齐，那么该怎么计算旋转呢？可以利用 SVD 分解得到旋转矩阵，暂先略过……

回到投影计算，投影的方程如下，其中 \(\Delta\) 是未知的平移变换。

\[\begin{align*} \begin{bmatrix} x" \\ y" \\ z" \\ w" \end{bmatrix} &= \bf{P} * （\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} + \begin{bmatrix} \Delta_x \\ \Delta_y \\ \Delta_z \\ 0 \end{bmatrix} ）\\ \begin{bmatrix} X_{2d} \\ Y_{2d} \end{bmatrix} &= \begin{bmatrix} \frac{x"}{w"} \\ \frac{y"}{w"} \end{bmatrix} \end{align*}\]

将 P 用具体数值代入可得：

\[ \begin{align*} X_2d &= -\frac{X_w + \Delta_x}{t*a*(Z_w + \Delta_z)} \\ Y_2d &= -\frac{Y_w + \Delta_y}{t*(Z_w + \Delta_z)}\end{align*} \]

定义优化目标：

\[\Delta = arg \, \min\limits_{\Delta} 0.5(X_2d+\frac{X_w + \Delta_x}{t*a*(Z_w + \Delta_z)} + Y_2d + \frac{Y_w + \Delta_y}{t*(Z_w + \Delta_z)})^2\]

使用高斯牛顿法迭代求解，并结合透视投影渲染，整体 3D 与 2D 的对齐效果好。