继续视觉里程计求解问题之PnP算法

1 3D-2D：PNP

PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法 。之前讨论了利用对极几何求解了两帧图像之间的位姿变换关系，在SLAM问题中，我们知道了两帧图像的位姿就可以对匹配的特征点进行三角测量，得到一些路标点的世界位置，也就是3D点。这时候我们就可以用PNP求解接下来的各帧位姿了。
那为什么不一直用对极几何约束吗？

2D-2D的对极几何方法需要八个或八个以上的点对（以八点法为例）；
存在着初始化、纯旋转和尺度的问题。
而，如果两张图像中，其中一张特征点的 3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。

3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。

实际上，在双目或 $RGB-D$ 的视觉里程计中，因为我们可以直接测量得到每张图像特征点的深度，所以可以直接使用 $PnP$ 估计相机运动。

求解$PnP$问题，有很多算法，比如用三对点估计位姿的 P3P，直接线性变换（DLT），EPnP（Efficient PnP），UPnP 等，但是最常用的当属SLAM系统中常用的Bundle Adjustment，下面介绍BA的方法。

1.1 Bundle Adjustment（BA）

我们可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题。在这个最小二乘问题中，我们将位姿和地图点都看成优化变量，不断调整他们的位置，使得误差函数最小。在 PnP 中，这个 Bundle Adjustment 问题，就是一个最小化重投影误差（Reprojection error）的过程。
由前面学习的世界坐标点投影到像素平面的知识，我们可以得到下面这个式子，相机位姿用李代数表示：

由于相机位姿未知以及观测点的噪声，该等式存在一个误差。因此，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化（注意这个式子里是由很多个路标点的重投影构成的）：

上式的含义可以这样理解：假定一个初始位姿$\xi$，然后把已经三角化成功的3D点按照这个初始位姿进行投影，然后与实际测量像素值作差产生一个误差值，然后我们不断调整位姿使得这个误差值越来越小（最小化重投影误差），这样位姿也就越接近真实值。
问题：为什么能这样算误差？
是因为特征匹配，我们认为两个像素对应同一个3D点。

不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。

用图表示单个点重投影误差是这样的：

求解上述BA问题，最著名的就是高斯-牛顿法（G-M）和列文伯格-马夸尔特法（L-M）。这里我们用高斯牛顿法。

1.2 高斯牛顿法求解BA

高斯牛顿法是最优化算法里面最简单的方法之一。它的思想是将误差函数（先假设误差函数为$f(x)$）进行一阶的泰勒展开：

这里的$J$是误差函数对位姿的导数（雅可比矩阵），而这里的$\delta$x 表示我们前面说描述的位姿调整量。
这样，前面的单个点最小化重投影误差式子可以写成下面这样：

上式又可以写成这个样子：

根据极值条件，求$\delta$x的导数并将其等于零，就可以求解出位姿调整量$\delta$x。

即，

这个方程叫做正规方程。
再简化一番，式子可以写成：

求解正规方程是整个优化问题的核心所在。

---

高斯牛顿法求解位姿调整量$\delta$x的步骤为：

---

1.3 BA问题中雅可比形式的推导

上面介绍了单个点的最小化重投影误差高斯牛顿法求解问题，现在再回到很多3D点的BA问题上来。就是求解下面这个式子：

第一步肯定是用泰勒展开式将误差函数（设为$e$）线性化。

前面说到这个$J$是$e$对位姿调整量的雅可比矩阵。那么它的形式是什么样的呢？，下面我们来讨论：
①误差是重投影（像素平面上的差值），所以$e(x)$是$2$维的。
②相机位姿 $\xi$ 是旋转和平移，所以是$6$维的.

那么$J$ 将是一个 $2\times 6$ 的矩阵。

---

推导如下：
首先，记变换到相机坐标系下的空间点坐标为 $P^{\prime }$，并且把它前三维取出来：

根据相机投影模型：

其中$s$是地图点的深度，我们可以消掉，得

接下来要用到之前说的扰动模型求导了。我们对 ξ^ 左乘扰动量 $\delta$ξ，然后考虑 $e$ 的变化关于扰动量的导数。利用链式法则：

这里的 $\oplus$ 指李代数上的左乘扰动。我们看到误差函数对位姿调整量求导可以写成两个偏导相乘得形式。注意看，右边得那一项是不是正好是之前推导的变换后的3D点关于李代数的导数？，它的形式就是：

由于我们前面定义$P^{\prime }$时没有用齐次坐标形式，所以这里应该变为这样：

那么，我们再来推导左边偏导的形式。
误差函数得式子其实是这样的$e=u_{观测}-u_{投影}$，这里面只有$u_{投影}$是包含$P^{\prime }$的，这个函数的导数就是-$\frac{\partial u}{\partial p^{{}^{\prime }}}$，具体形式如下

然后将两者相乘就可以得到$J$的形式。它是一个$2X6$的矩阵

到这里，就可以求解前面提到的正规方程了。其中$H=J^{T}J$，$g=-J^{T}e$：

2 非常感激您的阅读！

3 期待您加入

也非常期待您能关注我的微信公众号–“过千帆”，里面不仅有技术文章还有我的读书分享，希望您在那里也有收获。我们一起进步。