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 Projection
Matrix(call it P Matrix later)?

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

• For P Matrix it 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 corner
so 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 & cy \\ 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} & tx \\ r{21} & r{22} & r{23} & ty \\ r{31} & r{32} & r{33} & tz \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X{w} \\ Y{w} \\ Z{w} \\ 1 \end{bmatrix} \end{align} \]

看一下 solvePnP 的 DLT 办法的原理，参考这个文章

直接假设一个带有12个未知数的 3x4 的矩阵作为未知数，并忽略其中的Rt含义，
然后将上式化简，建立一个关于未知3x4矩阵的方程，下面方程中用 a1~a12 表示。

\[\lambda {\text{ = }}\left[ {\begin{array}{{20}{c}} u \\ v \\ 1 \end{array}} \right] = \left[ {\begin{array}{{20}{c}} {{f_x}}&{}&{{c_x}} \
{}&{{f_y}}&{{c_y}} \\ {}&{}&1 \end{array}} \right]\left[ {\begin{array}{{20}{c}} {{a_1}}&{{a_2}}&{{a_3}}&{{a_4}} \
{{a_5}}&{{a_6}}&{{a_7}}&{{a_8}} \\ {{a9}}&{{a{10}}}&{{a{11}}}&{{a{12}}} \end{array}} \right]\left[ {\begin{array}{{20}{c}} x \\ y \\ z \\ 1 \end{array}} \right] \ \\ \]

\[\left\{ {\begin{array}{*{20}{l}} {\lambda u = x{f_x}{a_1} + x{c_x}{a_9} + y{f_x}{a_2} + y{cx}{a{10}} + z{f_x}{a_3} + z{cx}{a{11}} + {f_x}{a_4} + {cx}{a{12}}} \\ {\lambda v = x{f_y}{a_5} + x{c_y}{a_9} + y{f_y}{a_6} + y{cy}{a{10}} + z{f_y}{a_7} + z{cy}{a{11}} + {f_y}{a_8} + {cy}{a{12}}} \\ {\lambda = x{a9} + y{a{10}} + z{a{11}} + {a{12}}} \end{array}} \right. \ \\ \]

λ 可以表示乘上投影矩阵以后的 z. 那么除以 λ 后得到的 uv 就是 2D 图像空间的数值了。
要是能取到6个点的数据，组成12行，就可以直接进行求解。具体求解过程中的 SVD 分解求最小二乘解的过程不赘述。

注意，这里有个 cv 常用的坐标系与 GL 常用的坐标系的区别，Z轴方向相反。上文提到在求解时会限制 z > 0.
所以要用 pnp 方法来求解的话，需要先把 3d 点做一个 z 轴镜像的变换，然后再构建 K 矩阵

\[K=\begin{bmatrix} \frac{1}{t*a} & &0 \\ & \frac{1}{t} &0 \\ & & 1 \end{bmatrix} \]

同时，2d点不是图像空间的点，而是 NDC 空间的点，如此进行计算得到的RT数据可以大致对齐。

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 \\ \Deltaz \\ 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}{ta(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}{ta(Z_w + \Delta_z)} + Y_2d + \frac{Y_w + \Delta_y}{t*(Z_w + \Delta_z)})^2 \]

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