直接法FC(Forward Compositional )&IC( Inverse Compositional )&ESM求解方法介绍

直接法FC(Forward Compositional )&IC( Inverse Compositional )&ESM求解方法介绍

论文：Efficient Compositional Approaches for Real-Time Robust Direct Visual Odometry from RGB-D Data
以下公式与原文出入较大，批判阅读

一、介绍

这篇论文讲述使用RGBD实现直接法VO的求解思路

基于直接法的求解方法：
提出三种求解思路：

• Forward Compositional (FC)；
• Inverse Compositional (IC)；
• Efficient Second order Minimization (ESM)。

二、约束定义

即：$\underset{p}{\mathrm{argmin}}||I^{\ast }(\omega (0,X))-I(\omega (p,X))|{|}^2$

其中，X为参考帧相机坐标系下的三维空间点。
上述方程就是典型的直接法的约束方程，使用的是FC的求解思路。

已知参考帧的灰度图像$I^{\ast }$与深度图$D^{\ast }$，以及当前帧的灰度图像$I$,以及对应的深度图$D$，

目标是计算出参考帧与当前帧6自由度相机的运动，设为$T\left(p\text{p}p\right),p\text{p}p\in se\left(3\right)$，表示将当前帧中的三维点转化到参考帧下。即：

${\sum }_i{p}_i{G}_i$构成李代数p对应的反对称矩阵。后续简化该写法为：

设$X$为参考帧下的一个三维坐标点，

$\omega \left(p,X\right)$的定义如下：

即$\omega \left(p,X\right)$可以将参考帧下的三维点X转换为当前帧的图像坐标。
从而可知$I\left(\omega \left(p,X\right)\right)$，为与参考帧三维点X对应的当前帧图像灰度值。

三、求解思路：

目前很多求解使用的高斯牛顿的求解方法，不用求解H矩阵。求解的迭代公式如下：这里与原文一致，使用右乘思路。

其中：

3.1 Forward Compositional (FC)

求解思路：在参考帧($I^{\ast }$)中提取关键点，利用已知深度，得到参考帧相机坐标系下的空间三维点$X$，求解参考帧($I^{\ast }$)到当前帧($I$)之间的相机位姿变换矩阵，使得将三维点$X$从参考帧映射到当前帧$X^{\prime }$后，最后映射到当前帧图像，所得的当前帧图像灰度值相匹配(等($I^{\ast }=I$))。

FC为普通，最常用的求解思路，定义约束方程如下：

即：$\underset{p}{\mathrm{argmin}}||I^{\ast }(\omega (0,X))-I(\omega (p,X))|{|}^2$
令：
$$r(p)=I(\omega (p,X))-I^{\ast }(\omega (0,X))$$
$r$雅可比求解如下所示：

3.2 Inverse Compositional (IC)；

求解思路，在当前帧($I$)中提取关键点，利用已知深度，得到当前帧相机坐标系下的空间三维点$X^{\prime }$，求解当前帧($I$)到参考帧($I^{\ast }$)之间的相机位姿变换矩阵，使得将三维点从当前帧映射到参考帧后，最后映射到参考帧图像，所得的图像灰度值相匹配(等($I^{\ast }=I$))。

将原约束方程等价如下：

$$\underset{p}{\mathrm{argmin}}||I^{\ast }(\omega (p^{\ast },X^{\prime }))-I(\omega (0,X^{\prime }))|{|}^2$$
其中：
$X^{\prime }$为当前帧下的三维空间点；
${p\text{p}p}^{\ast }\circ p\text{p}p=I\text{I}I\to T\text{T}T({p\text{p}p}^{\ast })\bullet T\text{T}T(p)=I$，李代数的交错性，可知${p\text{p}p}^{\ast }=-p\text{p}p$；
则对应的雅可比矩阵求解应为： 以下的$J^2$不是平方，是指代第二个方法的雅可比矩阵的意思

---

但是，在匹配过程中，我们通常提取参考帧的特征点，在当前帧中去对应，并解算得到相机两帧之间的位姿变换。因此，一种新的迭代方法被提出，约束方程定义如下：


雅可比矩阵$J_{ic}(0)$分为两部分，第一部分是特征点的图像梯度，第二部分则可以看作是相机移动与特征点图像坐标的变化的关系。

以上约束方程的解释：
当得到最优解后，$\delta \to 0，I=I^{\ast }$，设$p^{\ast }$为最后最优解。
$$T\left(p^{\ast }\right)X=X^{\prime }$$

在迭代过程中，$p$尚未使约束方程达到最优解，使用$\delta$确定$p$的下降方向。此时$p$为上一步迭代预估值，使用$p$将参考帧对应的相机坐标系下的三维点X映射到当前坐标系下，设为$X^{\prime \prime }$，注意：$X$在参考坐标系，$X^{\prime }$与$X^{\prime \prime }$均在当前坐标系。
$$T\left(p\right)X=X^{\prime \prime }$$

为了满足IC约束，$\delta$需使$I^{\ast }$也对应到$X^{\prime \prime }$，因此$I$与$I^{\ast }$去匹配$X^{\prime \prime }$。即使：
$$T\left(\delta \right)T\left(p^{\ast }\right)X=X^{\prime \prime }=T\left(p\right)X$$
得：
$$T\left(\delta \right)T\left(p^{\ast }\right)=T\left(p\right)\Rightarrow T\left(p^{\ast }\right)=T^{-1}\left(\delta \right)T\left(p\right)=T\left(-\delta \right)T\left(p\right)$$

即使用上式能使，$p$向$p^{\ast }$靠拢。

利用此解法，有一个重要的优点，因为$I^{\ast }$对$\delta$是位于0点求导，因此该约束方程的雅可比矩阵直接固定，只用计算残差$(I(p)-I^{\ast })$，而不像FC或者IC的前一种解法一样，每次迭代，雅可比均需要更新。

---

3.3 Efficient Second order Minimization (ESM)

可以粗略看成，将FC与IC结合，求平均的解法。
即整合FC与IC的约束方程：
$$\underset{p}{\mathrm{argmin}}||I^{\ast }(\omega (0,X))-I(\omega (p,X))|{|}^2+||I^{\ast }(\omega (-p,X^{\prime }))-I(\omega (0,X^{\prime }))|{|}^2$$

论文中有证明，此处只有最后用法：
$$J^2=\frac{1}{2}\left(J^1+J^2\right)$$
而由于上约束方程减号两侧一个选用参考帧的点作为关键点，一个选用当前帧的点作为关键点，所以可以用FC结合IC的第二求解思路，得到新的雅可比计算思路：

$$J^2=\frac{1}{2}\left(J^1+J_{IC}(0)\right)$$