【论文翻译】Tracking monocular camera pose and deformation for SLAM inside the human body

人体内单目SLAM的相机位姿和形变跟踪

Tracking monocular camera pose and deformation for SLAM inside the human body
Juan J. Gómez Rodrı́guez, J.M.M. Montiel, Member, IEEE and Juan D. Tardós, Fellow, IEEE
论文链接: https://arxiv.org/abs/2204.08309

摘要

形变场景下的单目SLAM技术将为内镜计算机辅助导航、自动给药和自主机器人手术等多种医学应用开辟道路。在本文中，我们提出了一种新的方法，在不考虑任何环境拓扑或形状的情况下，同时跟踪相机位姿和三维场景变形。该方法采用光度一致性方法，并结合重投影误差和形变的时间空间正则化，跟踪图像特征、估计相机运动和场景变形。在模拟结肠镜中的实验结果表明，该方法在强形变的复杂场景下具有较高的准确性和鲁棒性。我们从 Endomapper 数据集获得的人体结肠镜实验结果表明，该方法能够成功应对真实内镜的挑战 : 形变、低纹理和强光照变化。我们还在 Hamlyn 数据集的简单场景中与以前的跟踪方法进行了比较，在 Hamlyn 数据集中我们获得了具有竞争力的，而不需要任何拓扑假设。

Ⅰ. 引言

静态环境下的SLAM和VO是近几十年来的研究热点，提出了许多方法，利用特征[1]、直接法[2]或特征-直接法混合技术[3]来解决这些问题，具有出色的准确性和鲁棒性。这些技术的日益普及提高了人们对在更复杂场景中解决SLAM的期望。例如，人们可以想到SLAM在微创手术(MIS)中的许多应用，比如通过AR注释引导外科医生在之前的探查中发现息肉的位置，以及自动测量息肉来分析其变化。此外，手术机器人将大大受益于人体内的SLAM，因为它们将更安全、更健壮、更准确，而且它们将能够结合以前的探索或来自其他传感器的信息，如CT等。
然而，人体内部的视觉SLAM带来了巨大的挑战，如弱纹理、光照变化、镜面反射和缺乏刚性(图1)。弱纹理和镜面反射阻碍了基于特征匹配的数据关联算法，阻止了像 ORB-SLAM3[4] 这样的方法在这种场景下工作。
另一方面，光照变化给直接法如 DSO[2] 或 DSM[5]，和混合方法如 SVO[3] 带来了很大困难，因为直接法本身假设环境的光度一致性。相比之下，我们使用改进的 Lucas-Kanade算法解决数据关联，该算法首次在[6]中提出，能够应对局部光照变化。
但SLAM在人体内部需要解决的主要挑战是可变形的场景，因为打破刚性假设会破坏环境重建和相机位姿跟踪。最近的 DefSLAM[7] 是第一个能够进行跟踪和建图的单目可形变SLAM系统，但它强烈依赖于光滑连续平面的假设，这在结肠镜中并不成立(见图1)

图1 : 从 Endomapper 数据集跟踪人体结肠镜中的相机位姿和场景形变。上图 : 视频中的图像。底部 : 蓝色为相机轨迹，黑色为未变形的地图点，红色为地图点变形轨迹

在本文中，我们提出了第一种纯单目方法，能够初始化一个地图，并在人体内部的一般场景中跟踪相机位姿和场景形变(图1)，不需要任何拓扑或形状假设。我们的主要贡献是一个简单的公式，结合了特征跟踪、基于重投影误差优化和表达环境形变局部假设的空间时间正则化器，赋予我们的算法足够的表达能力来建模复杂的场景，并在实际内窥镜中跟踪形变。
我们提供了对 现实肠镜模拟[8] 的定量评估和对 Endomapper[9] 的真实人体肠镜的定性结果，这是以前的技术无法达到的。我们在 Hamlyn数据集[10] 中给出了几乎平面场景的定量比较，在那里我们获得了有竞争力的性能，尽管没有使用任何场景拓扑或形状的假设。

Ⅱ. 相关工作

在过去的几年里，计算机视觉和机器人社区已经开发了优秀的刚性视觉SLAM系统[2][3][4]。虽然所有这些算法都使用不同的技术，但它们都依赖于一个重要而简单的假设：环境是静态的。相比之下，完全打破刚性假设的可变形SLAM仍然是一个具有挑战性的研究课题。
许多研究都试图通过使用提供完整三维环境信息的传感器(如立体或RGB-D相机)来解决可变形SLAM问题。这就是影响深远的 DynamicFusion[11] 的情况，它使用RGB-D图像重建高度形变的环境，使用迭代最近点(ICP)算法和空间正则化器来约束接近点的形变，我们在工作中采用了这种方法。之后，有人对DynamicFusion开发了一些扩展，其中最著名的 VolumeDeform[12] ，它结合了SIFT特征和稠密ICP的重误差投影，以减少漂移和增加鲁棒性。在[13]中，他们制定了一种采用RGB-D图像重建形变环境的变分法。这项工作后来在[14]中进行了扩展，引入了相机位姿计算。
在微创手术(MIS)中，由于RGB-D传感器的缺乏，SLAM技术越来越受到人们的关注。因此，像[15][16]这样的工作使用双目图像的深度信息和一个结合了重投影误差和正则化的误差函数来进行可形变的SLAM，像之前一样使用其他3D信息，如ICP或Point-to-Plane误差来降低重投影误差。关于使用的正则化器，它们类似于 DynamicFusion 中引入的正则化器，用于表示局部发生的形变，使用相邻点之间的成对形变项，这防止了重建中单个点的发散。
然而，双目相机并不适合于某些应用，如结肠镜检查，在这种情况下，有足够基线的双目机可能无法放入体腔。在这种情况下，只能进行单目可形变SLAM。这是一个更困难的问题，因为从单一视图中无法获得真正的3D信息，无法观察到尺度，并且结合一个形变场景的多个视图更是一个未解决的问题。第一个单目可变形SLAM系统是 DefSLAM[7] ，它将可变形SLAM问题分成两个线程，一个用于跟踪，一个用于建图。他们使用ORB特征和最小化重投影误差项与变形度量项，惩罚成像表面的拉伸和弯曲。但是，由于ORB特征在体内图像序列中非常不稳定，SD-DefSLAM[6] 通过集成光照一致性的 Lucas-Kanade 跟踪器来进行数据关联，将 DefSLAM 扩展为半直接方法，获得了更好的鲁棒性和准确性。关键的是，这两种方法都假设曲面具有平面拓扑结构，并使用三角形网格对曲面进行建模，这对环境施加了一个强大的全局条件: 成像曲面必须是连续的，没有孔。这是一个很强的假设，它严重限制了两种算法可以处理的场景类型，例如结肠镜检查(图1)。
为了解决这一限制，[17]提出了一种完整的光度算法，在假定局部等距的前提下，使用稀疏的3D曲面(表面元素)来跟踪相机的姿态和形变，使用相互之间无约束的曲面可以建模任何类型的拓扑。虽然在医疗场景中获得了很好的效果，但该方法仍然需要来自双目相机的3D信息来初始化曲面。此外，使用大曲面(实际中使用 23 X 23 的正方形像素块)很容易违反局部等距假设，而且效率低，因为使用太多的相邻像素点会提供冗余信息，在精度上几乎没有提高[2]。此外，所使用的正则化器对静止位姿施加了小的变形，这在许多应用中是不合适的。
相比之下，本文提出了一种单目跟踪相机姿态和形变的方法。在慢形变的假设下，我们进行自动的单目地图初始化。在之前的工作[3]和[6]之后，我们使用光度特征跟踪来实现鲁棒性和准确性，并在优化过程中使用重投影误差来实现收敛性和效率。此外，为了约束重构问题，我们集成了两个正则化项，分别表达了平滑形变和缓慢形变的假设。

Ⅲ. 可形变跟踪

本节主要介绍我们的跟踪算法。首先提出控制我们系统的假设，接着介绍跟踪的数据关联，然后探讨了们的单目地图初始化算法，最后提出我们的优化主干，它的公式表达了我们的每个假设。

A. 假设

在处理可变形场景时，最大的困难是违背了刚性假设。这使得相机姿态和形变预测成为不可分离的问题，产生无限解，即不是所有的自由度(DoF)都是可观测的。
当使用单目相机时，这甚至更糟糕，因为尺度也是未知的。尽管如此，人们必须在问题中加入一些先验知识，以便将可能的解决方案限制为能正确代表环境真实性质的解的简化集。在本文中，我们提出以下假设来约束我们的重构问题：

1. 局部等距：假设曲面点的附近遵循等距模型，即局部距离保持不变。
2. 平滑形变：假设在空间上接近的点必须经历相似的形变。
3. 慢形变：假设形变在一段时间内缓慢发生。
4. 相机运动比形变快：假设相机的运动比形变快，所以我们把刚性运动归于摄像机，计算的形变尽可能小

假设(1)使我们能够使用光特征跟踪器，在每个被跟踪点周围定义一个假定为局部刚性的小邻域。这允许我们采用类似于[18]的方法来执行短期数据关联。假设(2)在观测到的变形中引入局部约束，而不引入全局变形或曲面模型。这有效地使我们的系统足够通用，可以为任何环境建模。假设(3)允许我们在表面点的位置上施加时间连续性，减少图像和数据关联噪声的影响。最后，假设(4)允许我们分离相机运动和环境形变。在所有的SLAM系统中，传感器提供相对信息，因此，摄像机和环境的绝对姿态是无法观察到的。在刚性SLAM中，这可以通过选择一个任意的全局姿态来解决，例如，第一个相机姿态被选择为零点。在可变形的SLAM中，这是不够的，因为相机运动在所有环境都严格移动的假设情况下难以识别，这被称为[17]中的浮动地图模糊性。这个假设允许我们使用正则化器，在相机运动上惩罚形变。这样，将环境与相机相对运动的刚性部分归为相机运动，得到尽可能小的形变。
值得注意的是，上述假设都没有对表面拓扑、平滑度或形变施加全局约束，这使我们能够模拟一般的变形和环境。

B. 数据关联

我们之前在 单目可变形SLAM[6] 方面的经验证明，精确的数据关联对于达到良好的精度和鲁棒性至关重要。事实上，其他的工作已经显示了直接法在这一任务中的潜力，如[2]，其中光度项允许作为跟踪特征关联的副产品。这是通过对所有点进行全局刚变换来实现的，因为假设环境是静止的。然而，这在可形变的SLAM中无法实现。实际上，不能对数据关联步骤施加任何全局约束，因为它很容易被形变违反。
为此，我们提出先使用 Shi-Tomasi特征[19] 进行光度数据关联，再使用文献[6]中提出的改进的多尺度 Lucas-Kanade 算法进行相机位姿和形变的估计：

其中 $P(u)$ 是一个以关键点 $u$ 为中心的像素小块；$I^0$ 是第一帧，点在这里被初始化；$I^t$ 是当前帧，当前时间为 $t$ 。这些像素块每5张图像更新一次，以适应大范围的变化或旋转。该算法已被证明在短时间内跟踪图像特征时，即使存在形变或局部照明变化(图2)也能取得良好的效果。其性能的关键在于不使用全局模型：每个点都可以相对于其他点自由移动。通过计算每个点的局部增益$\alpha$和偏置$\beta$，得到局部光照一致性。

图2：上图，从我们的 EndoMapper 数据集中分离出两张间隔3帧的图像。下图:从这些跟踪初始化的地图

为了去除任何可能的离群轨迹，我们计算参考 $x$ 和已跟踪的 $y$ 像素块之间的结构相似指数(SSIM)[20]来识别任何离群轨迹

其中 ${\mu }_x$ 和 ${\sigma }_x$ 为像素小块的均值和协方差，${\sigma }_{x}y$ 为两个小块之间的互协方差，$C_1$ 和 $C_2$ 为常数值，以避免在均值和协方差趋于零时的不稳定性。对于像素小块，这已经被证明是一个很好的相似度量，因为它结合了亮度、对比度和结构的度量。

C. 单目地图初始化

刚性环境中的单目地图初始化是 SfM 中众所周知的，在形变环境中，NRSfM 技术可以被使用。然而它们需要假设，例如在地图初始化时，光滑的场景表面具有平面拓扑结构，这在真实的结肠镜检查中是不满足的(图1和图2)。
我们提出利用假设(4)使用两个接近的帧，其中环境可以被认为是准刚性的，大多数的图像改变可以归因于相机的运动。这允许应用 SfM 来获得建图的第一个估计，即假设它是刚性的，并将任何形变视为小噪声。
理想情况下，该方法应该独立于针孔或鱼眼相机模型。我们提出通过使用图像中特征的归一化投影计算两个接近帧之间的本质矩阵 $E$ 来初始化单目建图。我们提出的初始化算法经过以下步骤：

1. 利用 Lucas-Kanade 光流法提取均匀分布在参考帧 $I^0$ 中的 Shi-Tomasi 特征，并在当前帧 $I^t$ 中跟踪。利用相机模型反投影函数将匹配的特征反投影到归一化坐标 $x_i^0$ 和 $x_i^0$ 中
2. 计算两帧图像位姿转换的的本质矩阵 $E$：
   
   同时使用 RANSAC 算法，排除异常点影响。
3. 从 $E$ 中恢复位姿转换矩阵 $T_{C^t{C}^0}$，将产生4个运动假设(2个旋转和2个平移)。我们使用相近帧来初始化，因此相机旋转应该很小，这样我们可以安全地选择最小的旋转 $R$ 来解决旋转模糊性。最后，我们选择了在两个相机前产生最多点数的位移矩阵 $t$ 。
4. 利用相机运动重建环境。我们使用 逆深度加权中点[21] 对跟踪特征点进行三角定位，因为它在小视差场景中提供了低 3D-2D 转换误差。

D. 相机位姿预测

为了表达假设(4)，在估计任何形变之前，我们首先估计时间 $t$ 的相机在世界坐标下的位姿 $T_{C^{t}W}$ 。我们假设相机遵循恒定速度的物理模型。这为我们提供了一个相机位姿的初始预测，然后使用非线性最小二乘 (NLLS)，利用前一帧 $t-1$ 中观察到的重投影误差来进行优化。这可以看作是一种把大多数图像变化归因于相机运动的方式。请注意，这不是我们计算的最终位姿，只是形变和相机位姿的全局优化种子，将在下一节详细介绍。

E. 跟踪相机位姿和环境形变

我们的目标是，给定当前帧 $u_i^t$ 的一些特征匹配和上一时刻的3D点 $X_i^{t-1}$，估计当前帧的相机位姿 $T_{C^{t}W}$ 和每个点的变形 ${\delta }_i^t$ ，其中当前帧的3D点可通过 $X_i^t=X_i^{t-1}+{\delta }_i^t$ 预测。
为此，我们引入一个重投影数据项 $E_{i,rep}^t$ 和2个正则项 $E_{i,spa}^t$ 和 $E_{i,tmp}^t$ 来约束全局代价函数中的形变部分，对于 $t$ 时刻的 ${\delta }_i^t$，定义为：

其中 $P$ 表示当前帧中被观察到的点集。我们的全局问题可以用非线性平方优化来解决：

接下来我们定义损失函数 ${\delta }_i^t$ 的各项

1. 重投影项：用[6]中提出的改进 Lucas-Kanade 算法获得当前帧的特征匹配 $u_i^t$ ，以此计算重投影误差如下：
   
   其中 $\rho$ 是 Hubber 鲁棒损失函数， ${\hat{u}}_i^t$ 和 $u_i^t$ 分别是当前图像 $I_t$ 中匹配特征 $i$ 的坐标和该特征的投影坐标，${\hat{u}}_i^t$ 由下式得到：
   
   间接法的准确性受到特征检测器分辨率的限制(通常不小于1像素)。而用光度法得到的匹配具有小于1像素的精度，在保持良好收敛区间的前提下，提高了重投影的精度。
2. 空间正则化：遵循[11]，为了表示假设(2)的平滑形变，我们使用一个空间正则化器约束局部变形：
   
   这里 $G$ 表示一个加权图，它对需要把变形一起进行正则化的点进行编码。两个连接点 $i$ 和 $j$ 在 $G$ 中的权重为 $w_i{j}^t$ ，它取决于前一时刻 $t-1$ 两点之间的欧氏距离，根据以下公式计算：
   
   其中 $\sigma$ 是径向基权值，控制着每个点的影响半径。这个空间正则化是至关重要的，因为它强制执行尽可能刚性的形变，并有助于形变的全局一致性。
3. 时间正则化：最后，我们在形变上添加了一个时间正则化来表示形变随着时间的推移而缓慢发生(假设(3))：
   
   这个空间正则化也与假设(4)相互作用，因为它惩罚了可以用相机运动解释的大变形。

Ⅳ. 实验

我们在微创手术的结肠镜检查序列中评估我们的方法，这类序列构成了巨大的挑战，因为它们展示了持续的变形、糟糕的纹理和恶劣的光照条件。我们使用逼真的合成数据测试并提供定量结果，在人体内结肠镜检查中进行定性实验。为了进行比较，我们还在 Hamlyn 数据集中测试了我们的方法，使用它的立体设置来评估我们的重建与其他最先进的方法。我们的主要结果总结如图3所示:

(a)

(b)

(c)

(d)

图3：不同序列的算法结果。每两行表示一个序列。第一行显示3D重建：黑点为未变形的地图点，红线为地图点形变轨迹，蓝色为相机轨迹。第二行是带有绿色跟踪特征点的 RGB 帧。从上到下依次为：(a) EndoMapper 真实活体序列，(b)模拟序列， (c) Hamlyn 20序列，(d) Hamlyn 21序列。所有数据集都只使用单目图像进行处理。

A 实现细节

我们用c++语言实现了单目地图初始化、相机姿态和形变估计。对于非线性平方优化，我们使用在 g2o 库[22] 中实现的 Levenberg-Marquart 算法。对于特征提取和匹配，我们实现了自己的 Shi-Tomasi 特征提取器 和 Lucas-Kanade跟踪器 (Eq.1)。我们为 SSIM 得分设置了 0.8 的阈值 (Eq.2)，以检测和拒绝虚假的特征点轨迹。关于优化，我们设 ${\Sigma }_{rep}$ 为一个像素，${\Sigma }_{spa}$ 和 ${\Sigma }_{tmp}$ 为10 mm(毫米)。由于 ${\Sigma }_{spa}$ 和 ${\Sigma }_{tmp}$ 度量了 $E_{spa}$ 和 $E_{tmp}$ 项，我们将它们各自的 $\lambda$ 设置为1。最后，Hubber 损失阈值使用95%的 ${\chi }^2$，其中 $E_{rep}$ 为2自由度，$E_{spa}$ 和 $E_{tmp}$ 为3自由度。
在图 $G$ 中，对于每个点，我们只添加 $K=20$ 个最接近3D点的正则化项，且在单目相机初始化时 $\sigma$ 设为15 mm，使用立体视觉进行地图重建时设为 $\sigma$ 设为55 mm。这样做是为了保证每个点始终是正则化的，同时忽略对当前点影响不大的点，以减少计算量。

B 模拟结肠数据集

我们使用 VR-Caps[8] 生成CT三维结肠模型的光感合成图像序列。因为这是一个模拟，所以我们拥有相机位姿和3D地图点的所有真值。事实上，我们可以生成具有不同相机轨迹和形变程度的序列，使我们能够分别测试系统的每个组组件。
为了进行评估，我们模拟了一个具有不同变形程度的插入操作(图3-b)。我们使用沿着模拟结肠传播的正弦波来模拟变形。根据下面的公式，我们将这个形变应用到模拟蠕动运动的表面点的y坐标上：

其中 $V_x^0$ ，$V_y^0$ 和 $V_z^0$ 是曲面点静止时的坐标。我们可以分别根据参数 $A$ 和 $\omega$ 来控制变形的大小和速度。表一给出了我们系统在不同形变速度和振幅的模拟序列下的重建精度。所示误差为所有帧重构点的均方根误差 (RMSE)：

由于这是一个完整的单目公式，我们发现对于每一帧，存在一个最优的尺度因子 $s^t$，使我们的重构与真值相一致。结果表明，即使存在形变，我们的公式也能达到 2-3 mm左右的良好重构误差。一个有趣的结果是，由于假设(3)慢形变，相比形变大小，我们的系统对形变速度更敏感。

表一：模拟结肠镜[23]中不同变形类型的重建 RMSE (mm)

C Hamlyn 数据集

我们也在真实的内窥镜序列中测试我们的构想。为此，我们使用 Hamlyn数据集[10] 的序列20(图3-c)和21(图3-d)。序列20(的第750帧)对应于缓慢形变的腹部探查。序列21(的第750帧)描绘了一个肝脏，它有两个叶，每个叶都在独立运动，这可以被认为是一个关节运动。在这两个序列中，表面纹理较差，光照条件不佳。该数据集是用双目内窥镜记录的，允许我们用视差估计环境结构真值。
为了进行比较，我们在两种情况下评估了我们的构想(表二)。第一个情况，我们用第一个双目图像初始化我们的系统并执行单目跟踪，以便与之前的方法 ORB-SLAM[1]、SD-DefSLAM[6] 和直接稀疏可变形跟踪(DSDT)[17] 进行比较。因为我们是使用双目图像初始化的，所以在计算 RMSE 时我们不用执行任何尺度对齐。我们在重建误差方面取得了具有竞争力的结果，获得了1.5 mm左右的一致误差。由于我们没有对表面拓扑结构或形变施加任何限制，与 SD- DefSLAM 相比，我们实现了明显更小的误差。在序列21中，两个叶瓣独立运动，这一点特别明显，这限制了 SD-DefSLAM 的准确性。与 DSDT 的比较表明，我们的正则化器是通用的，我们能够更好地表达序列20的空间平滑性，实现更低的误差，同时在序列21的硬不连续方面仍然具有竞争力。DSDT 能够保持更长的跟踪时间，因为与 DSDT 相比，我们的方法仍然没有实现任何策略来恢复跟踪期间丢失的点。
第二个情况使用纯单目处理，包括我们的单目初始化(第III-C节)，并计算每帧尺度校正 (Eq.12) 后的 RMSE。在这种情况下，我们的系统达到了2.8-3.3 mm左右的误差，这与在显著形变下的模拟数据集中获得的误差一致。与第一种情况相比，误差的增加是由于地图初始化的质量不再依赖于完美的双目初始化。
还需要注意的是，在这些序列中观察到的表面形状和变形与在模拟数据集中看到的完全不同，这证明我们可以处理一般的表面形状和形变。

表二：Hamlyn数据集[17] 中序列20和21与其他方法的比较。我们记录了重构的 RMSE(mm)和处理帧数

D 真实的内窥镜序列

我们在 EndoMapper[9] 中提供了真实人体肠镜序列的定性结果。这些序列显示了真正的结肠镜所面临的巨大挑战，如形变、图像中几乎没有纹理、不同帧间的光照变化、反射和鱼眼光学(图1)。
在这种情况下，没有真值来比较，因为数据集只是记录标准的单目内窥镜探查，因此我们只提供定性的结果。图2显示了我们从非常接近(间隔3帧)的两帧中，提取高密度的点初始化地图，成功捕捉了结肠的管状拓扑结构。在图1和图3-a中，可以看到我们的算法如何能够捕获场景形变和内窥镜轨迹，能够在两个不同患者的真实结肠镜检查示例中跟踪超过30帧的地图点。

Ⅴ. 结论

本文我们提出了一种单目相机跟踪和形变估计的方法，无需对环境形状或拓扑的假设。相反，我们成功地用简单的正则化器编码了内窥镜检查中常见的形变类型的假设。与目前的技术相比，我们的方法，包括地图初始化，适用于更广泛的形状和拓扑，如结肠镜检查，并且在更多标准的几乎平面场景拥有相近的准确性。
提出的单目初始化和跟踪有助于实现一个完全可形变的SLAM系统。使用我们的贡献，随着相机探索新的区域，通过可形变建图拓展地图逐渐可实现，在短期内是未来工作的一个有前途的方向。从中期来看，多地图可变形SLAM为未来的工作提供了有利的条件，因为它能够应对真实结肠镜检查中普遍存在的闭塞和跟踪损失。