PnP 问题求解

PnP问题介绍 #

已知n个三维点及其在图像中的二维投影情况下，估计相机的姿态和位置。

已知： 3D点的坐标 $P_i$，2D点的坐标 $p_i$，相机的内参矩阵 $K$。

求解： 相机的外参矩阵 $[R|t]$，记为 $T$。

方法： 使用OpenCV solvePnP() 函数，可以得到 $T$ 初值，用于后续优化。

误差定义 #

定义误差函数为： $$ e = \frac{1}{2}||p - \frac{1}{z_2}(KTP_1)||^2 $$

遇到的问题 #

问题在于，$T$ 对于加法是不封闭的（例如 $T$ 中的旋转矩阵 $R$ 是正交矩阵加上小量不再是旋转矩阵），无法使用普通的求导公式得到

$$ \frac{\partial e}{\partial T} = \frac{\partial e(T+ \Delta T)}{\partial \Delta T} （这是错误的） $$

解决方法 #

由于变换 $T$ 对乘法封闭，我们可以利用指数对数运算使得变换的表示变得对加法封闭，这里用到了李群和李代数的概念。

在李代数中执行加法操作来更新变换矩阵的估计，可以绕过旋转矩阵的正交性约束，然后使用指数映射将更新后的李代数元素映射回李群（变换矩阵）。这样可以用标准的优化技术。

重新定义损失 #

记变换重投影后的点为 $P_2$， 重新定义损失函数为： $$ e = \frac{1}{2}||p - \frac{1}{z_2}(KP_2)||^2 $$ $$ e = \frac{1}{2}||p - \frac{1}{z_2}(Kexp(\hat{\xi})P_1||^2 $$

求导 #

在上述误差的基础上，我们终于可以求导 $$ \frac{\partial\boldsymbol{e}}{\partial\delta\boldsymbol{\xi}}=\lim_{\delta\boldsymbol{\xi}\to0}\frac{\boldsymbol{e}\left(\delta\boldsymbol{\xi}\oplus\boldsymbol{\xi}\right)-\boldsymbol{e}(\boldsymbol{\xi})}{\delta\boldsymbol{\xi}}=\frac{\partial\boldsymbol{e}}{\partial\boldsymbol{P}^{\prime}}\frac{\partial\boldsymbol{P}^{\prime}}{\partial\delta\boldsymbol{\xi}}. $$

其中 $$ \frac{\partial\boldsymbol{e}}{\partial\boldsymbol{P}^{\prime}}=-\begin{bmatrix}\dfrac{\partial u}{\partial X^{\prime}}&\dfrac{\partial u}{\partial Y^{\prime}}&\dfrac{\partial u}{\partial Z^{\prime}}\\dfrac{\partial v}{\partial X^{\prime}}&\dfrac{\partial v}{\partial Y^{\prime}}&\dfrac{\partial v}{\partial Z^{\prime}}\end{bmatrix}=-\begin{bmatrix}\dfrac{f_x}{Z^{\prime}}&0&-\dfrac{f_xX^{\prime}}{Z^{\prime2}}\\0&\dfrac{f_y}{Z^{\prime}}&-\dfrac{f_yY^{\prime}}{Z^{\prime2}}\end{bmatrix} $$

$$ \frac{\partial\left(\boldsymbol{TP}\right)}{\partial\delta\boldsymbol{\xi}}=(\boldsymbol{TP})^{\odot}=\begin{bmatrix}\boldsymbol{I}&-\boldsymbol{P}^{\prime\wedge}\\mathbf{0}^{\mathrm{T}}&\mathbf{0}^{\mathrm{T}}\end{bmatrix}. $$

其中 $\boldsymbol{P}^{\prime\wedge}$ 是反对称矩阵，$\boldsymbol{P}^{\prime\wedge}=\begin{bmatrix}0&-Z^{\prime}&Y^{\prime}\Z^{\prime}&0&-X^{\prime}\-Y^{\prime}&X^{\prime}&0\end{bmatrix}$

最后得到误差对李代数的导数为： $$ \dfrac{\partial\boldsymbol{e}}{\partial\delta\boldsymbol{\xi}}=-\begin{bmatrix}\dfrac{f_x}{Z’}&0&-\dfrac{f_xX’}{Z’^2}&-\dfrac{f_xX’Y’}{Z’^2}&f_x+\dfrac{f_xX’^2}{Z’^2}&-\dfrac{f_xY’}{Z’}\0&\dfrac{f_y}{Z’}&-\dfrac{f_yY’}{Z’^2}&-f_y-\dfrac{f_yY’^2}{Z’^2}&\dfrac{f_yX’Y’}{Z’^2}&\frac{f_yX’}{Z’}\end{bmatrix}. $$

在这个3d-2d点的问题上，如果不会推导的话，可以使用g2o的数值求导，ceres的自动求导，或者使用opencv的solvePnP函数都可以。