1 摘要：
相机运动估计是三维重建和SLAM的关键技术。为了使其可行，之前的工作通常使相机慢速运动，这限制了它在许多实际情况下的使用。文章提出了一种结合color、RGB-D和IMU的端到端三维重建系统，以实现快速传感器运动的鲁棒重建。系统使用EKF融合这三种数据，并采用迭代法联合优化匹配特征、相机姿态和几何场景。本文还提出了一种基于几何感知的patch变形算法来适应图像域内的外观特征，从而在快速的摄像机运动下实现更精确的特征匹配。实验表明，我们的patch变形算法提高了特征跟踪的精度，并且我们的三维重建方法在快速运动下优于现有的解决方案。
2 实现方法
图1. 红、绿、蓝箭头分别表示当前帧的输入、迭代操作和上一帧的patch
该方法主要由四部分组成:几何感知特征跟踪，探索滤波效果并进行斑块变形;滤波框架，解释卡尔曼预测和更新步骤;模型融合和patch更新。
- 2.1 几何感知特征跟踪
特征点法在图像模糊或者弱纹理条件下提取的特征数量不足，因此，提出了一种基于patch匹配方法进行特征跟踪（直接法的变形，2D->3D）， 该方法考虑了较大的图像区域，但是大patch可能包含不同深层次的物体，会导致连续帧间的外观发生变化，尤其当相机运动较快时，会导致特征跟踪不准确。为了解决这个问题，文章将颜色与深度信息结合起来，将2D patch投影到3D中，并使用初始的相机运动将它们重投影到下一帧相机中。投影可以帮助我们对原始的patch进行变形来模拟外观的变化，通过变形后的patch可以方便、准确地实现对patch的跟踪。
2.1.1 SE效应和patch变形
当相机移动时，在不同的帧中会从不同的角度看到一个特征patch，因此不同帧中特征patch在图像坐标中的形状和位置会有所不同。与只考虑patch的2D平面信息不同，文章利用patch的3D几何来确定快速相机运动记录的图像之间的2D形状变形。
根据patches的不同几何形状和相机运动的不规则性，patches可以在连续帧中产生不同的变形。图2显示了三种典型的patch变形情况：
图2. 展示了相机运动和patches几何形状引起的patch SE效果。
case 1：如果patch内的像素深度没有显著差异，那么无论摄像机运动多么剧烈，在连续的两帧中，patch的总体形状都将保持不变；
case 2：当相机缓慢移动时，即使patch存在较大的深度方差，patch的二维形状仍然保持不变；
case 3：与case 2不同，如果相机主动移动，patch的强度分布和形状会发生变化。如果相机从V0移动到V2视点，黄色区域会被遮挡，patch会在当前帧缩小。此外，当相机移动到V1时，被V0遮挡的黑色区域就可以看到，因此patch形状在当前帧中会扩展。这些现象称为收缩效应和延伸效应，所以我们把它们统称为SE效应。
图3. SE patch变形过程
定义上一帧k-1中提取的patch中的每个像素为：
其中，Pi为像素i在patch中的图像坐标，Ii,di,ni表示像素i的强度、深度和3D法线。di和ni是从深度图像中获得，由于我们有3D信息，我们把我们的patch称为3D patch特征。我们首先将每个patch投影到3D世界坐标系中：
其中，Π(.)表示3D相机坐标系到2D像素坐标系的投影，Π(.)^(-1)表示逆投影；Tk-1为坐标系k-1的相机位姿(R,t)，表示从相机坐标系到世界坐标系的转换。因此，Li在世界坐标系，从第k-1帧中的一个像素i开始索引。所以，
从Pi到Li像素强度保持不变。然后我们将它投影到当前帧k的像素坐标系中：
注意，‘ 表示被投影到该坐标系，而不是从这个坐标系出发；Tk是未知的，它会影响投影的2D位置。经过投影后，如果两个投影的像素恰好在同一个像素坐标，那么认为在这个patch中出现了收缩效应。当靠近相机的区域覆盖了远处的区域时，通常会发生这种情况。因此，我们移除与远处区域相对应的像素，然后评估延伸效应是否发生。我们将投影patch的形状设置为所有投影像素的边界框，如果投影patch的高度或宽度大于原patch，则验证该扩展效果。在接下来的特征跟踪步骤中，无论发生哪种效果，我们都使用Pk’作为变形的patch。
2.1.2 目标
在特征跟踪中，将受SE效应影响的patch替换为对应的投影patch。然后通过强度和深度信息对投影的patch特征进行跟踪。
每个投影patch的光度误差计算如下。我们首先在当前图像的投影位置提取与Pk相同大小的patch，然后计算提取的patch与投影patch的强度差。光度误差可以表示为：
其中，Y为patch中的像素个数，I表示一个patch对应的强度信息。然后计算点对平面的几何误差：
给定Ep和Eg, patch跟踪的代价函数公式为:
其中，j表示第j个patch，M表示patches的数量。
2.2 EKF滤波器框架
文章EKF耦合color、depth和IMU信息。具体来说，将每一帧的相机位姿建模为EKF的状态量，并通过EKF求解。EKF的观测量包括彩色图和深度图，状态量和观测量之间的关系由公式5计算。如果一种状态完全符合观测量，能量为零。另一方面，在EKF预测步骤中利用IMU建立运动预测模型。
采用传统的EKF来定义变量，一个非线性离散时间系统，状态量为x，观测量为z，过程噪声w(0,Q)，更新噪声u(0,U)，第k帧可以写成：
在我们的框架中，状态量x为 变换矩阵T = (R，t)，与世界坐标系有关。世界坐标系指定为第一帧时刻的相机坐标系。
定义，上标’+‘表示Kalman更新步骤计算的变量的后验估计，下标’-‘表示Kalman预测步骤计算的a-先验估计。
2.2.1 EKF预测和状态传播
给定协方差Pk-1^(+)的后验估计Xk-1 ^(+), EKF预测会在下一帧产生先验估计
其中，雅可比矩阵为：
卡尔曼预测步骤的关键是定义函数f。在EKF框架中，在定义中使用惯性测量。参考文献[10]，我们从IMU中得到实际的传感器加速度a和角速度w。假设IMU与相机机同步，并以比摄像机小得多的时间间隔τ 来获得测量值。因此，N表示连续两帧内获得的IMU测量值的个数，然后通过参考文献[11]的预积分法将它们融合，来预测连续两帧间的相机旋转R和平移t：
在上述三个方程中，下标’n‘表示相机相邻帧间第n个IMU输入的变量。此外，?v表示从相机上一帧到当前帧的IMU累积线速度，Φ 是从IMU坐标系到相机坐标系的外参矩阵，g为重力加速度，Exp(·)为李代数到李群的指数映射。KF预测详细推导可参考文献[10]。最后，在当前帧k中状态预测量可表示为：
2.2.2 EKF更新和迭代
在传统的EKF更新步骤中，测量值与预测值间的残差建模为:
其中，0表示我们直接用Xk^(-)来计算残差，不添加任何高斯噪声。更新状态方程表示为:
其中Kk为卡尔曼增益。在我们的方法中，我们将残差定义为patch的光度误差和几何误差(公式5)，因此残差可以表示为：
注意，通过方程17计算Yk时使用的SE patch变形很大程度上受到相机位姿影响。因此，在通过式16得到更新后的相机位姿后，利用更新后的相机位姿迭代计算SE patch变形，再次通过式16对相机位姿进行细化。这样，可以估计一个更准确的Xk^(+)。使用m表示迭代次数，因此有：
每一次迭代的卡尔曼增益为：
如2.2节开始所定义的，Uk是噪声uk的协方差矩阵。每次迭代更新的雅可比矩阵公式为:
然后计算每次迭代的更新状态量:
注意X(k,0)^(+)表示Xk ^(-)。最后，当
的绝对值低于某个阈值时，迭代终止。
协方差矩阵只在迭代后收敛时更新:
2.3 模型融合和patch更新
我们使用TSDF递增地将每个连续的深度帧Dk融合到一个3D几何模型Mk(X)中，并使用来自EKF更新的 Rk+和tk+。关于深度融合的细节可以在参考文献[13]中找到。
重建完成后，我们需要更新patch特征，用于后续跟踪。我们使用平均像素强度误差去除坏特征，对受SE效应影响的非正方形形状的patch特征重新提取平方patch特征。然后，通过FAST角点对具有明显强度梯度和足够深度信息的新特征和具有深度信息的像素个数进行评估。最后，利用当前颜色更新patch强度信息，从3D几何模型中获取比当前深度图质量更好的深度信息。
3 实验效果
- 通过SE效果和patch变形，评估几何感知特征跟踪方法的有效性
- 评估IMU的好处
- 评估快速运动时的三维重建效果
3.1 评估
特征跟踪
图4. 对比直接法和本文提出算法在两个连续帧内的patch特征跟踪结果。patch尺寸10*10，并且在每一帧中提取不超过100个patch。
IMU评估
3.2 比较
补充视频https://www.youtube.com/watch?v=Jy3SGqWuhp8
参考资料：
Zhu Z , Xu F . Real-time Indoor Scene Reconstruction with RGBD and Inertia Input[J]. 2018.
链接：https://xueshu.baidu.com/usercenter/paper/show?paperid=1y1s0pf0by4r06x0417j0m10uj172726&site=xueshu_se&hitarticle=1