《基于RGBD相机的Active SLAM的研究》-PNP方法

针对map_frame到camera_frame的转换的问题，打算采用基于PNP的ORB_SLAM2,故有必要对PNP好好回顾一下。

1，P3P

在 SLAM 当中,通常的做法是先使用 P3P/EPnP 等方法估计相机位姿,然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment)。
顾名思义，P3P仅仅使用3对匹配点，如图

由余弦定理可知
$$\begin{gathered} x^2+y^2-2xycos⟨a,b⟩=A{B}^2/O{C}^2 \\ {y}^2+1-2ycos⟨b,c⟩=B{C}^2/O{C}^2 \\ {x}^2+1-2xcos⟨a,c⟩=A{C}^2/O{C}^2 \end{gathered}$$
第二个公式和第三个公式分别除第一个公式，得到

$$\begin{gathered} (1-u)y2-ux2-cos⟨b,c⟩y+2uxycos⟨a,b⟩+1=0 \\ (1-w)x2-wy2-cos⟨a,c⟩x+2wxycos⟨a,b⟩+1=0 \end{gathered}$$
其中
$x=OA/OC$待求
$y=OB/OC$待求
$u=B{C}^2/A{B}^2$已知
$w=A{C}^2/A{B}^2$已知

使用吴消元法解出，比较复杂，但是有现成的包，可以快速的求解出来。最后得出在当前相机坐标系下面，landmark的3D坐标。两个问题

1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时,难以利用更多的信息。
2. 如果 3D 点或 2D 点受噪声影响,或者存在误匹配,则算法失效。

2，EPnP

EPnP 更加复杂，需要明确几个概念

1，参考点与控制点
参考点是指用于参与到相机坐标系下坐标求解的点
控制点是EPnP 的特定的一个概念,由$c_i$ $i=1,2,3,4$表示。参考点与控制点的关系为
$p_i^w={\sum }_{j=1}^4{\alpha }_{ij}{c}_j^w,with{\sum }_{j=1}^4{\alpha }_{ij}=1$
$p_i^c={\sum }_{j=1}^4{\alpha }_{ij}{c}_j^c$

2，Barycentric Coordinates
这个指的是${\alpha }_{ij}$，由于参考点都是非齐次的（3维），并且满足约束关系${\sum }_{j=1}^4{\alpha }_{ij}=1$，所以是数值唯一的。

3，控制点的选择，这个无需多深究，只要不妨碍有关Barycentric的求解

4，求解特征向量$v_i$相机坐标系下的坐标，比较复杂，目前就大概了解吧

先求出0空间对应的特征向量$v_i$，当N=4的时候，可以近似求解出${\beta }_1,{\beta }_2,{\beta }_3,{\beta }_4$的数值，之后使用高斯牛顿法对${\beta }_1,{\beta }_2,{\beta }_3,{\beta }_4$进行优化。优化的目标函数为
$Error(\beta )={\sum }_{(i,j)s.t.i<j}(\Vert \Vert c_i^c-c_j^c\Vert \Vert {}^2-\Vert \Vert c_i^w-c_j^w\Vert {\Vert }^2{)}^2$
最后得到控制点在现有相机坐标系下面的表示

也就得到参考点在现有相机坐标系下的表示

参考

3，Bundle Adjustment

把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题，首先使用G-N的基本步骤

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

在BA算法中，误差函数为$u_i-Kexp(\xi$^$)P_i$，优化变量为ξ，所以最小二乘的问题化成了

然而，高翔仅仅只有对单个点提出解释，对于单个点的相机位姿可以了，但是对于n个点估计来讲，没有占到什么便宜，并且对同时优化参考点的位姿没有解释清楚，针对这两点，我做了相关调研。

查了一圈，貌似这个在g2o中是通过增加边来解决的，即如果有N个点，就要有N个边，限于水平，具体图优化的算法不深入探究。此外，优化变量也可以添加到参考点的坐标。