作者robot L
主要内容
1. 在任意两帧双目图片中寻找特征点的数据关联；在任意两帧LiDAR点云中寻找点面数据关联；
2. 基于上述关联结果，对于双目图片，构建特征重投影误差约束和深度约束；对于LiDAR点云，构建点面距离约束。并将上述三个约束放入到统一的概率框架中联合优化；
3. 通过LiDAR点云的准确深度精修双目点云：去除双目点云中深度不确定性大的点，并填补其平面表面的空洞；
优点
本文符号简单，可读性很高，但又不失准确和完整性，值得学习。
缺点
该文章使用的是批处理方法：在所有位姿对之间寻找数据关联，并一次优化所有位姿。由于位姿数量随时间增加，因此很难扩展到实时应用。能够实时运行的序列数据处理方法需要需要进一步被研究。
W. Zhen, Y. Hu, J. Liu, S. Scherer. A Joint Optimization Approach of LiDAR-Camera Fusion for Accurate Dense 3-D Reconstructions. IEEE Robotics and Automation Letters, 4(4), 3585-3592, 2019.
摘要
融合相机和激光的数据十分吸引人，因为它们有互补的性质。例如，相机有更高的分辨率和色彩，LiDAR数据有更准确的距离信息，并且有更大的视场。但是，传感器融合问题仍然充满挑战，因为很难在不同特征中（几何vs纹理，稀疏vs稠密）找到一个可靠的关系。Zhen等人提出了一个离线LiDAR-相机融合方法以建立稠密，准确的3D模型。具体地，作者提出的方法解决了一个BA问题和一个点云配准问题来计算相机位姿和外参标定。实验中，作者展示该方法达到了2.7mm的平均准确率和70个点/平方厘米的分辨率。此外，外参标定超过了SOTA方法。
1. 介绍
本工作致力于建立准确稠密的3D模型通过融合多帧LiDAR和相机数据如图1所示。激光扫描一个物体表面的3D点，并且被获取的数据在幅度上是准确的，并且对低纹理条件具有鲁棒性。但是，LiDAR数据包含有限的纹理信息（只有密度），并且非常稀疏由于内部激光之间的物理空间。不同地，一个相机提供了更加丰富的纹理数据但是没有直接测量距离。即使一个双目系统通过三角化测量深度，它可能会失败在低纹理或者重复模式的区域中。那些互补的性质使得融合LiDAR和相机用于构建稠密的纹理3D模型变得非常具有吸引力。
图1. 一个商用LiDAR-双目系统被用于收集双目图片（只有左图片被可视化）和LiDAR点云。我们的算法估计相机位姿，生成一个被扫描环境的具有纹理的稠密3D模型和点云地图。
大多数提出的传感器融合算法一般通过LiDAR深度增强图片。然后稀疏的深度图片被上采样来得到一个深度估计，或者被用于促进双目三角化过程。但是，我们观察到这些策略的两个缺点。第一个是深度增强需要传感器外参标定，其和双目相机的标定相比，准确性更低因为匹配结构和纹理特征可能是不可靠的。例如（见图2），许多外参标定过程使用目标的边缘作为点云和图片间的对应，其可能产生问题：1）点云边缘由于遮挡不会很干净而是混合的，2）边缘点不是实际的边缘由于数据稀疏性，而知识松弛分散的。第二个缺点是上采样或者LiDAR引导的双目三角化技术基于局部平滑性假设，其变得不合法如果原始深度过于稀疏。被融合的深度地图的准确性因此降低，其可能是有用的对于避障，但是对于建图的目的来说不理想。为了上述讨论的原因，，我们选择组合一个旋转LiDAR和一个宽基线高分辨率双目系统来增加原始数据的密度。此外，我们致力于融合传感器数据并同时恢复外参标定。
图2. 不准确边缘提取的图示。左：混合边缘点（绿色）具有深度误差。右：松弛边缘点（绿色）具有角度误差。
本文的主要贡献是一个离线的方法来处理多帧双目和点云数据并联合地优化相机位姿和传感器外参变换。提出的方法具有几个优点：
- 它不依赖不可靠的结构和纹理数据的关联，而只强加传感器之间的几何约束，其实我们不需要手工启发式地关联来自不同域的信息。
- 它联合BA和点云配准问题在一个概率框架中，完成了传感器不确定性的合适处理。
- 它能够执行准确的自标定，使得物理上吸引人的。
本文的剩余部分如下所述：第2节展示了在LiDAR-相机融合技术上的相关工作。第3节详细描述了提出的方法。实验结果被展示在第4节中。结论和未来工作在第5节中被讨论。
2. 相关工作
在本节中，我们简要地总结了在LiDAR-相机外参标定和融合领域的相关工作。对于外参标定，提出的方法能够被粗略地分类根据标定目标的使用。例如，单个[1]或者多个[2]标定板被使用作为匹配图片和点云的平面特征。此外，人们还使用特定的标定物体，例如一个箱子[3]，一个具有形状洞的板或者一个三面体[5]，其中被提取的特征也包括角点和边缘。目标的使用简化了问题，但是仍然不方便当目标不可用时。因此，无目标的方法被发展使用自然特征（例如，边缘），其在环境中是丰富的。例如，Levinson和Thrun[6]利用LiDAR和相机数据的不连续性，并通过一个基于采样的方法精修了初始猜测。该方法被成功应用到一个自动驾驶汽车中来跟踪标定漂移。Pandey等人[7]发展了一个基于互信息的框架，其考虑LiDAR密度的不连续性。但是，该方法的表现依赖密度数据的质量，其可能是差的在没有用于LiDAR模型的标定时。不同地，[8]-[10]基于独立传感器的自运动恢复外参变换。这些方法近似相关一个手眼标定问题[11]，并且不依赖特征匹配。但是，运动估计和外参标定被分别解决，并且传感器不确定性没有被考虑。替代地，我们构建一个代价函数，其联合了两个问题在一个概率一致的形式并且一起优化所有参数。
可靠的融合算法大多数被设计用于LiDAR-单目或者LiDAR-双目系统，并假设外参标定已知。对于一个LiDAR-单目系统，图片经常被增强通过投影的LiDAR深度。被融合的数据能够被用于多个任务。例如，Dolson等人[12]上采样范围数据用于在动态环境中安全导航的目的。Bok等人[13]和Vechersky等人[14]使用相机纹理为范围数据上色。Zhang和Singh[15]展示了被深度增强后，在视觉里程计的鲁棒性和准确性上的显著提高。对于LiDAR-双目系统[16]-[19]，LiDAR一般被用于直到双目匹配算法，因为一个深度先验能够显著降低视差搜索范围并帮助拒绝外点。例如，Miksik等人[17]在LiDAR点之间插值在双目匹配前得到一个深度先验。Maddern和Newman[18]提出了一个概率框架，编码LiDAR深度为一个先验信息，并达到了实时表现。额外地，在测量领域[20]-[22]，点云被配准基于使用相机的运动估计。我们的方法不同于这些工作，LiDAR点云没有被投影到图片中，因为外参变换被假设是未知的。替代地，我们使用LiDAR数据来精修双目重建在标定被恢复后。
3. 联合估计和建图
A. 概述
在介绍提出的算法流程之前，我们澄清一些在剩余部分使用的定义。在符号方面，我们使用粗体小写字母（例如， 
）来表示向量或者元祖，粗体大写字母（例如， 
）来表示矩阵，图片或者地图。额外地，书法符号被用于表示集合（例如， 
表示一组变换）。并且标量被表示为浅色字母（例如， 
）。
作为基本的概念，一个图片路标 
被定义为一个至少在两张图片中观测到的3D点。然后，一个相机观测被表示为一个5元组 
，其中，元素分别是相机id，路标id，图片坐标，深度和路标的加权因子。此外，一个LiDAR观测被定义为一个6元组 
，包含目标点云id 
，源点云id 
，源点云中的一个关键点，它在目标点云中的最近邻，近邻的法向量和一个加权因子。换句话所，一个LiDAR观测关联一个3D点到一个局部平面并且点面距离将会被最小化在后续的联合优化步骤中。
提出方法的完整流程如图3所示。给定双目图片和LiDAR点云，我们首先提取和匹配特征来准备三组观测，称为路标集合 
，相机观测集合 
，和LiDAR观测集合 
。观测然后被喂入联合优化模块来估计最优相机位姿 
和外参变换 
。基于相机的最新位姿，LiDAR观测被重新计算并且优化被重复。在一些迭代后，参数收敛到局部极小值。最终，精修和建图模块联合来自双目图片和LiDAR点云的深度信息来产生3D模型。在本节的剩余部分，每个成分将会被单独地详细描述。
B. 相机观测提取
给定一个双目图片对，我们首先使用[23]中被提出的半全局匹配（SGM）执行双目三角化来获得视差图片。视差图片被表示在左图片中。然后SURF[24]特征从左图片中被提取。注意到我们的算法本身不需要特定类型的特征来工作。之后，一个特征点被关联深度值如果在小半径内一个合适的视差值被发现（在我们的应用中是2个像素）。只有具有深度的关键点被保留用于进一步的计算。上述步骤对所有位置重复来获得多组具有深度的特征。一旦深度关联被解决后，一个全局特征关联模块被用于发现所有可能图片组合之间的关联。我们采用一个简单的匹配方法，增量增加新观测和路标都 
和 
中。算法1展示了细节的过程。基本上，我们通过迭代所有可能的组合基于对应描述子的欧氏距离来匹配图片特征。
和 
将会被更新根据是否一个合法匹配被发现。
额外地，一个编码图片相关性的邻接矩阵 
可以被得到。因为相机FOV是狭窄的，可能相机位姿图没有被全连接。因此，额外的连接需要被增加到图中，这是融合点云的好处之一。
C. LiDAR观测提取
即使许多3D局部表面描述子已经被提出（一个综述被给出在[25]中），它们很少是稳定的并且与图片特征描述子相比是不准确的。实际上，使用3D描述子用于粗糙的配准并使用更慢的但是更准确的方法例如ICP[26]来精修是更可取的。我们的工作遵循类似的想法。具体地，二值形状环境（BSC）描述子[27]被使用来匹配和粗糙配准点云来计算点云变换 
。作为一个3D表面描述子，BSC在特征点的三个正交投影方向上编码了点云密度和距离统计量。进一步地，它表示局部几何为一个二值字符串，其在现代CPU上达到了快速的差异比较。图4-左展示了一个提取BSC特征的例子。但是，基于特征的配准是低准确的。如图4的右部分所示，误匹配可以被观测在粗糙的配准地图中。作为一个对比，通过我们方法的高质量精修的地图也被可视化。
图4. 左：一个从点云（灰色）中提取BSC特征（红色）的例子。中间：基于匹配特征的配准点云地图。右：在一个放大的窗口中的粗糙配准（上-右）和精修配准（下-右）。
在粗糙的配准后，编码匹配点云的另一个邻接矩阵 
也被获得。我们使用融合的邻接矩阵 
来定义最终的位姿图，其中 
表示逐元素或者逻辑运算。
为了获得 
，一组点从每个点云中被随机采样作为关键点。注意精修配准的关键点比特征更稠密。对于每对在 
中连接的点云，具有更小下标的被定义为目标，另一个被定义为源。然后在源中的每个关键点和它在目标中某个给定阈值范围内的最近邻和局部法向量关联起来。最终，所有点匹配被构建为LiDAR观测并堆叠到 
中。
D. 联合优化
给定观测 
和 
(1)
其中 
是一组相机位姿，其中 
，并且 
是外参变换。假设观测是条件独立的，我们有
(2)
(3)
其中 
是相机ids， 
是路标id，其通过观测 
或 
被给定。一个观测的概率被近似为高斯分布如下：
(4)
(5)
其中 
和 
是相机和LiDAR观测的加权因子。并且残差 
和 
编码了路标重投影和深度误差，同时 
表示点到面距离误差。那些残差被定义为
(6)
(7)
(8)
这里， 
和 
是路标 
被观测到的图片坐标和深度。
是从目标点云 
到源点云 
的变换。函数 
投影路标到图片 
中，通过输入矩阵 
和变换 
。函数 
变换3D点使用输入变换。
分别表示提取特征，双目深度和LiDAR范围的测量不确定性。
将(2)-(8)代回(1)中，并取负对数得到代价函数
(9)
其被使用LM算法在参数 
上迭代地解决。
为了滤除在图片和点云中的不正确观测，我们在优化收敛后检查相机的重投影误差 
和深度误差 
，并检查LiDAR观测的距离误差 
。误差大于预先设定阈值的观测被标记为外点，并分配零权重。代价函数(9)被重复优化直到没有外点被检测到。阈值可以被手动调整，并且在实验中，我们分别使用3像素，0.01m和0.1m。
类似ICP算法， 
被重新计算根据最新的估计 
，同时 
保持不变。一旦 
被更新后，外点检测和优化步骤像如上提到的那样被重复。
需要被重复计算多次（4次在我们的实验中）来达到好的精度。
额外地，指定不确定性参数的策略如下。基于双目配置，三角化深度误差 
和双目匹配误差 
被联系起来通过一个尺度因子：
，其中 
是基线， 
是焦距长度并且 
是深度。假设特征匹配和双目匹配的不确定性是等价的，我们有 
。因此，我们可以设置 
是单位值（即，1），并设置 
通过一个比例因子。另一方面， 
的值被手动调整以至于相机和LiDAR观测的整体代价近似于相同量级。在实验中，我们设置 
和 
在 
之间能够产生充分好的结果。
E. 建图
有了被估计的相机位姿，构建一个最终的3D模型变成了简单地将所有双目点云对齐到一起。但是，双目深度地图由于三角化失败一般包含外点和空洞。为了精修双目深度地图，我们进一步对每一帧执行LiDAR和相机数据的简单但有效的两步融合。在第一步中，双目深度和投影的LiDAR深度比较，并被移除如果有巨大的差异。在第二步，LiDAR深度被选择性地使用来调补双目深度中的空洞。特别地，我们只使用局部平滑（以致于局部平滑性假设是合法的），和很好被观测（避免退化的视角）的区域。局部表面的曲率被用于计算视角。图5展示了精修双目点云的例子。它可以被观测到，在平滑表面的空洞可以被成功地填充，同时靠近边缘的遗失点没有被对待来避免引入新的外点。
图5. 一个精修双目深度的例子。外点被首先滤除通过限制它和LIDAR深度的不同。然后空洞被填充通过周围的LiDAR深度，只有在局部表面具有近似零的曲率时才这样做。
F. 唯一性的条件
提出的方法依赖独立传感器的自运动来恢复外参标定 
，使得 
可能不完全可观如果运动退化。已经被证明同样的问题可能出现在手眼标定中，其中一个抓手和相机之间的外参变换从两个运动序列中被估计。这里我们讨论全部可观 
的条件通过借助手眼标定中的知识，其经典构架如下
(10)
其中 
表示手和相机相对于他们原始坐标系的相对运动。组合多个序列将会得到一组(10)，然后 
可以被解决。根据[11]，如下的两个条件必须被满足以保证 
唯一的解决方案：
1）至少两个运动对 
被观测到。等价地，至少3帧是需要的，其中一帧时基准帧。
2） 
的旋转对于不同的对是不共线的。
在我们的例子中，机器人手坐标系被代替为LiDAR坐标系。因此，每一帧的配置必须满足上述唯一性条件。者提供了有效收集数据的正式指导。从我们配置被发展系统的经验来看，一个没有CV特别是在SfM的充分背景的操作员，很有可能会遗忘这两个条件，只在竖直轴上旋转传感器，器可能导致外参标定不可观。
上述内容，如有侵犯版权，请联系作者，会自行删文。
