最小化重投影误差(BA法)求解PnP

1.引言

PnP算法是什么、用途以及部分求解方法我在PnP算法详解(超详细公式推导)中介绍过，但在那篇文章中基于基于优化的PnP求解方法我没有讲，因为我觉得这个方法比较重要，涉及一些李群李代数求导和非线性优化的知识，所以打算单独写一篇博客，后面也会出一片全c++代码实现的文章。

2.核心思想

重投影误差法，也叫Bundle Adjustment(BA法)，顾名思义这个问题的误差项是3D点的投影位置与实际位置作差，如图1所示，$p_1$和$p_2$是同一个空间点P在不同相机姿态下的投影，但由于我们不知道相机姿态。在初始值中。P的投影$\widehat{p_2}$与实际的$p_2$有一定的距离。于是我们通过优化的方式调整位姿，使得这个距离变小。不过，由于这个调整需要考虑多个点，所以最后的效果是整体误差的缩小，而每个点的误差通常不会精确为零。

图1

3.模型建立

考虑n个三维空间点$P$及其投影$p$，计算相机的姿态$R，t$,它的李群表示为$T$。假设某空间点的坐标为$P_i=[X_i,Y_i,Z_i{]}^T$，其投影的像素坐标为$u_i=[u_i,v_i{]}^T$。根据相机模型，建立空间点位置与像素位置关系如公式(1):
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=KT\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_1\\ 1\end{array}\right](1)$$

写成矩阵的形式就是公式(2):
$$s_i{u}_i=KT{P}_i(2)$$

该式隐含了一次从齐次坐标到非齐次的转换，否则按矩阵的乘法来说维度是不对的，也就是$T{P}_i$是4x1的坐标，取出前三维变成非齐次坐标后再与内参相乘。
现在由于相机位姿位置以及观测点存在噪声(非slam问题可能没有就无需优化),该等式存在一个误差。因此，我们把误差求和，构建最小二乘法问题，然后寻找最优的相机位姿，使公式(3)最小化:
$$T^{\ast }=argmi{n}_T\frac{1}{2}\sum _{i=1}^n{\Vert u_i-\frac{1}{s_i}KT{P}_i\Vert }_2^2(3)$$

对公式(3)进行优化的方法有很多，比如一阶梯度法、二阶梯度法、高斯牛顿法和列文伯格-马夸尔特方法等，这里使用高斯牛顿法，我们首先简单介绍下高斯牛顿法。

4.高斯牛顿法

高斯牛顿法是优化算法中最简单的方法。他的思想是将$f(x)$进行一阶的泰勒展开，也可以说是一阶线性化，注意这里不是目标函数$F(x)$而是$f(x)$，否则就变成牛顿法了。
$$f(x+\Delta x)\approx f(x)+J(x{)}^T\Delta x(4)$$

这里$J(x{)}^T$为$f(x)$关于x的导数,也是一个雅可比矩阵。

求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出增量方程，那么高斯牛顿法的算法步骤可以写成：

(1).给定初始值$x_0$
(2).对于第k次迭代，求出当前的雅可比矩阵$J(x_k)$和误差$f(x_k)$
(3).求解增量方程
(4).若$\Delta x_k$足够小，则停止。否则，令$x_{k+1}=x_k+\Delta x_k$，返回第二步。

5.具体实现推导

根据公式(3)设误差项如公式(6)所示：
$$e_i=u_i-\frac{1}{s_i}Kexp({\xi }^{\wedge })P_i(6)$$

e使用齐次坐标形式是三维，但由于最后一维始终为1，所以实际上是2维，由于要进行求导所以进行了一次$T$的指数映射，也就是$T=exp({\xi }^{\wedge })$，这里$\xi$是T对应的李代数，维度是$6\times 1$。

根据高斯牛顿法对误差项进行一阶泰勒展开，也就是线性化的过程化：

$$e(x+\Delta x)\approx e(x)+J^T\Delta x(7)$$

由于在高斯牛顿法中，关键是解出增量方程，而增量方程中含有$J$，所以$J^T$是关键。我们固然可以使用数值导数，但如果能够推导出解析形式，则优先考虑解析导数。
现在，由于e是像素坐标的误差(2维)，x为相机的姿态，由于是李代数的形式，所以是6维，根据矩阵乘法，$J^T$是一个$2\times 6$的矩阵。我们使用IPad进行$J^T$形式的推导。

根据公式(14)我们便可以求出$J^T$，，然后根据增量方程求解$\Delta x$，那么$\Delta x=\delta \xi$，然后优化T，使用李代数$T=exp(\delta {\xi }^{\wedge })\cdot T$，具体步骤在高斯牛顿法处介绍过。

如果需要优化特征点的空间位置，需要讨论e关于空间点P的导数，这个导数矩阵相对容易。仍利用链式法则，有：
$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{{}^{\prime }}}\frac{\partial P^{{}^{\prime }}}{\partial P}(15)$$

第一项在前面已经做了推导，关于第二项，按照定义：
$$P^{{}^{\prime }}=(TP{)}_{1:3}=RP+t(16)$$

$P^{{}^{\prime }}$对$P$求导后只剩下R。于是:
$$\frac{\partial e}{\partial P}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{{}^{\prime }}}& 0& -\frac{f_x{X}^{{}^{\prime }}}{Z^{{}^{\prime }2}}\\ 0& \frac{f_y}{Z^{{}^{\prime }}}& -\frac{f_y{Y}^{{}^{\prime }}}{Z^{{}^{\prime }2}}\end{array}\right](17)$$

$后面博客写一篇代码实现。$