SLAM系列--Camera Pose的求解(Direct Method)
作者：Frank
时间：2016-09-16
在SLAM前端，即视觉里程计(Visual Odometry)中，主流的求解相机姿态(Camera Pose)的方式有以下两种，一种是基于特征的(Feature-Based)VO，一种是直接法(Direct Method)求VO，本节从比较Feature-Based VO和Direct-Method VO出发，开始讲解利用Direct Method求解Camera Pose的过程。
1. VO对比
Feature-Based VO和Direct-Method VO之间的最大区别就是其求取相机姿态时构建最优化表达式所基于的假设不同。在Feature-Based VO中，是利用最小化图像点的重投影误差来求取的；而在Direct-Method VO中，则是利用最小化图像点和投影后的对应点的图像像素差来得到相机姿态的。
1.1. Feature-Based VO
Featured-Based VO在很多年来都是VO方向的经典解法。其基本思想是对前后两帧图像做特征提取和匹配，得到一系列特征的匹配对，然后利用匹配对中前帧中的特征点和初始的相机姿态估计，将这些特征点利用初始的姿态投影到第二帧，然后求投影后的点和第二帧中的匹配点之间的误差，并对其做非线性优化，得到最终的相机姿态，其中，特征点的投影和重投影过程利用的公式就是在张正友标定法中用到的投影方程，如下所示：
$${1 \over Z_c}\left[ {\matrix{
u \cr
v \cr
1 \cr
} } \right] = \left[ {\matrix{
{f \over S_x} & r & u_0 \cr
0 & {f \over S_y} & v_0 \cr
0 & 0 & 1 \cr
} } \right] \cdot \left[ {\matrix{
R_{3 \times 3} & T_{3 \times 1} \cr
} } \right] \cdot \left[ {\matrix{
X_w \cr
Y_w \cr
Z_w \cr
1 \cr
} } \right] = K_{3 \times 3} \cdot \left[ {\matrix{
R_{3 \times 3} & T_{3 \times 1} \cr
} } \right] \cdot \left[ {\matrix{
X_w \cr
Y_w \cr
Z_w \cr
1 \cr
} } \right] \tag{1}$$
公式最左边的$\left[ \matrix{
u \cr
v \cr
1 \cr
} \right]$表示图像上的特征点所在的齐次坐标，而公式右边则是投影到世界坐标系后对应坐标点的齐次表示。利用该公式可以将第一帧图像上的点先投影到世界坐标系中，然后从世界坐标系中投影回第二帧图像中，之后我们计算投影后的坐标点和匹配子在第二帧坐标点之间的误差来构造能量函数，并对其做非线性优化来最小化重投影误差。其投影和重投影的示意图如下所示：
其构造的能量函数为：
$$T_{k,k-1}=arg \min \sum_{i}||u^{‘}_i- \pi(p_i)||_{\sum}^2 \tag{2} $$
1.2. Direct-Method VO
Direct-Method （直接法）是近几年新出现的VO的解法，伴随着SVO，LSD-SLAM等SLAM项目的发布而流行。Direct-Method的思想是其并不会对前后两帧的图像做特征匹配，也许会有特征的提取（如SVO），但避过了耗时的特征匹配过程，而是利用如下假设：在微小的运动中，前后帧对应点之间的灰度变化很微小，即灰度恒定的假设，因此，直接法采用的方式是将当前帧的显著像素点通过式(1)投影到第二帧，然后取投影后的点的像素值，和当前帧对应的像素值，同样构造能量函数，然后对构造的能量函数做非线性优化来最小化图像测量误差。其示意图如下：
利用该方式构造的通用能量函数为：
$$T_{k,k-1}=arg \min \sum_{i}||I_k(u^{‘}_i)- I_{k-1}(u_i)||_{\sum}^2 \tag{3} $$
其中,$u^{‘}_i= \pi (T \cdot (\pi ^{-1}(u_i) \cdot d))$。
1.3. 两种方式的对比
下图是一幅关于两种方式的对比图：
从图中我们可以知道：
- 基于特征匹配的VO能适用于大场景的帧间运动，并且可以利用Bundle Adjustment对Struct和Motion进行优化得到姿态的精确解，这是其优势，但是由于其涉及特征提取和特征匹配，因此其非常耗时，并且对特征匹配而言，其可能存在相当一部分的误匹配，因此还需要对匹配后的结果进行滤除（RANSAC），这也是一个耗时的过程。
- 基于直接法的VO因为不需要提取特征，所以能省略这一部分的时间。而且像素恒定的方式能利用上图像中几乎所有的有用信息，因此会更精确，也会更鲁棒。但灰度恒定的假设只能容忍微小的图像运动，因此该方式对相机的运动速度有很大的限制，另一方面，由于该方式利用了几乎所有的像素点，其对于稠密重建和运动的计算和优化非常的耗时，甚至DTAM在PC上也需要GPU和CUDA的配合才能实现实时重建。
2. Direct-Method
直接法的SLAM是利用Minimizing the Photometric error(最小化测量误差)来实现对姿态的求解。其基本假设是：同一个空间点在各视角下，测到的灰度保持恒定不变。
2.1. 问题描述
对于给定的两幅图像，$I_1$和$I_2$，我们需要求解从$I_1$到$I_2$之间的转换Pose，即$T_{21}$。
假设对于$I_1$中任意像素点$u_i$，我们将它反投影到空间点的坐标为：$p_i$，然后将$p_i$投影到第二个图中，得到$u_i$在第二幅图像中的对应点$u_i^{‘}$，因此，根据假设我们可以得到该像素点的图像测量误差为：$e_i=I_1(u_i)-I_2(u^{‘}_i)$，我们对所有像素点构造器L2范数并对其求和可以得到如下的全局能量函数：
$$min J=\sum_{i=1}^N e_i^T e_i \tag{4} $$
2.2. 模型建立
由像素点的投影关系：
$$u_i=\left[\matrix{u \cr v \cr 1 \cr}\right]_i=C(Rp_i+t)=\left[\matrix{f_x & 0 & c_x \cr 0& f_y & c_y \cr 0&0&1 \cr}\right][R,t]\left[\matrix{p_x \cr p_y \cr p_z \cr 1 \cr}\right] \tag{5} $$
我们不妨假设$I_1$的相机Pose为I，$I_2$的相机Pose为R,t，那么，我们可以得到如下关系：
$$ \matrix{u_i=Cp_i \cr u^{‘}_i=C(Rp_i+t)} \tag{6} $$
则目标函数的形式为：
$$ min J =\sum _{i=1}^N ||I_1(Cp_i)-I_2(C(Rp_i+t))||_2^2 \tag{7} $$
2.3. 模型求解
根据上述建立的模型，我们的直接法解姿态就变成了以非线性优化方法来解式(7)。而对于目标函数的取值，其只与R和t有关，和其他表示量都没有关系，因此我们只对R和t进行优化；另一方面，在李群上，R和t是没有加法的，而且其也不好求导，因此我们将R和t转换到李代数上求解，而使用李代数$\xi$来表达相机姿态可以得到如下形式：
$$T=\left[\matrix{R & t \cr 0^T &1 \cr}\right]=exp(\xi ^\wedge) \tag{8} $$
因此目标函数可以转换为：
$$ min J =\sum _{i=1}^N ||I_1(Cp_i)-I_2(Cexp(\xi ^\wedge)p_i)||_2^2 \tag{9} $$
对于该目标函数的求解，我们之前已经大致的介绍过了利用梯度下降法或者Gauss-Newton法和LM方法来进行求解，这里会对这些方法进一步的深化。梯度下降法的思想是利用误差项对自变量求梯度，通过梯度来获得自变量的增量更新。在该目标函数中，我们设单个误差项为：
$$ e_i=I_1(Cp_i)-I_2(Cexp(\xi ^\wedge)p_i) \tag{10} $$
误差项对自变量的梯度为：$J_i={\partial e_i \over \partial \xi }$， 利用该梯度对误差项进行更新，而Gauss-Newton迭代则是：
$$(\sum _{i=1}^N J_i^T J_i) \delta \xi ^* =-\sum _{i=1}^N J_i e_i \tag{11}$$
或者可以表示为：$H \delta \xi ^* =-b$，这里是利用$J_i$的转置乘以自身来近似目标函数的二阶导数项以减小计算量。
目标函数的非线性优化问题概念本身不难，但是在该目标函数中，$J_i={\partial e_i \over \partial \xi }$的计算则是一个关键的问题。对于单个误差项而言，第一项与$\xi$无关，因此只需要对第二项求导数。这里我们假设
$$ \matrix{ q=exp(\xi ^\wedge)p \cr u=Cq \cr} \tag{12} $$
其中第一个为变换方程，从世界坐标系变换到本地；第二个为投影方程，从空间点投影到像素点。
根据链式法则，有：
$$ J_i=-{\partial I_2 \over \partial u}{\partial u \over \partial q}{\partial q \over \partial \xi} \tag{13}$$
公式中三项分别为：图像对像素求导，像素对空间点求导和空间点对李代数求导。
我们对上式从右到左分别进行分析：
对于q，我们对$exp(\xi)$ 左乘一个$exp(\partial \xi ^\wedge)$，得到：
$$ {\partial q \over \partial (\partial \xi)}={exp(\partial \xi ^\wedge)exp(\xi ^\wedge)p-exp(\xi ^\wedge)p \over \partial \xi}=(exp(\xi ^\wedge)p)^ \odot \tag{14} $$
$\odot$运算符把$ 4 \times 1$的向量变成$4 \times 6$的矩阵：
$$q^ \odot =\left[\matrix {q_3 \cr 1 \cr }\right]^ \cdot =\left[\matrix{I_{3 \times 3} & -q_3^\wedge \cr 0_{1 \times 3}^T & 0_{1 \times 3}^T \cr}\right] \tag{15} $$
舍弃掉q最后的1，导数变为：
$$ {\partial q \over \partial (\partial \xi)}=[I_{3 \times 3}-q_3^\wedge] \tag{16} $$
这是一个$3 \times 6$的阵。对于中间项$\partial u \over \partial q $，由
$$s \left[\matrix{ u \cr v \cr 1 \cr}\right]=\left[\matrix{f_x & 0&x_x \cr 0 &f_y&c_y \cr 0&0&1 \cr}\right]\left[\matrix{x \cr y \cr z \cr}\right] \tag{17} $$
可以得到：
$$ \matrix{
u={f_x x \over z}-c_x \cr
v={f_y y \over z}-c_y \cr
} \tag{18} $$
由此可得：
$$ {\partial u \over \partial q}=\left[\matrix{\partial u \over \partial x & \partial u \over \partial y & \partial u \over \partial z \cr \partial v \over \partial x &\partial v \over \partial y& \partial v \over \partial z \cr}\right] =\left[\matrix{f_x \over z & 0 & -f_x x \over z^2 \cr 0 & f_y \over z & -f_y y \over z^2 \cr}\right] \tag{19} $$对于左边项$ \partial I_2 \over \partial u$，其为像素梯度。
$$ {\partial I_2 \over \partial u}=\left[ \matrix{I_2(u+1,v)-I_2(u,v) & I_2(u,v+1)-I_2(u,v) \cr}\right] \tag{20} $$
由于[u,v]通常为浮点数，因此需要对图像进行插值。
对于以上三项而言，因为后面两项和图像没有关系，因此通常把后面两项乘到一起，得到如下公式：
$${\partial u \over \partial q} {\partial q \over \partial \xi}=\left[\matrix{f_x \over z & 0 & -f_x x \over z^2 \cr 0 & f_y \over z & -f_y y \over z^2 \cr}\right][I_{3 \times 3}-q_3^\wedge] =\left[\matrix{
f_x \over z & 0 & -f_x x \over z^2 & f_x xy \over z^2 & -f_x-{f_x x^2 \over z^2} &-f_x y \over z \cr
0 & f_y \over z & -f_y y \over z^2 & f_y-{f_y y \over z} & -f_y xy \over z^2 &-f_y y \over z \cr
}\right] \tag{21} $$
其物理意义为当像素点变化时，图像的灰度值的变化情况。
计算了上述三项后，我们就得到了梯度的表达式，可以利用该表达式结合GN或者LM方法对相机姿态进行非线性优化了。
转载请注明出处
