机器人在弱结构化、弱纹理环境中的状态估计是SLAM中的难点，考虑到不同传感器之间的互补，多传感器融合是必不可少的。然而这些传感器数据的频率、时间可能不一致，一种常见的解决方案是对来自不同传感器的同一时刻的测量值或估计姿态做插值处理以进行融合。连续时间轨迹表示可以避免插值，并允许在任何给定时间进行姿态查询，从而有助于融合来自各种传感器的高频率和异步观测数据。然而在许多应用中，没有关于要估计的轨迹的复杂性的先验信息。无法动态调整控制点的分布，均匀B样条易于参数化不足或过度。Coco-LIC是一种采用非均匀B样条参数化的连续时间紧耦合激光惯性相机里程计。与均匀B样条不同，非均匀B样条允许动态控制点分布，而不是固定频率。与R3LIVE 和FAST-LIVO 不同，这种方法通过在固定的时间间隔内同时融合来自三个传感器的数据，而不是分别融合激光雷达-惯性或视觉-惯性数据。
在本文提出了一种有效的连续时间激光雷达-惯性相机里程计，利用非均匀B样条紧耦合来自激光雷达、惯性测量单元和相机的测量值。与基于均匀B样条的连续时间方法相比，我们的非均匀B样条方法在实现实时效率和高精度方面具有显著的优势。这是通过动态和自适应地放置控制点来实现的，同时考虑到运动的变化。为了能够在短滑动窗口优化内有效融合异构激光雷达-惯性相机数据，我们使用来自全局激光雷达地图的对应地图点为视觉像素分配深度，并为当前图像帧中的相关像素构建帧到地图的重投影因子。这种方式避免了视觉像素深度优化的必要性，视觉像素深度优化通常需要具有大量控制点的冗长滑动窗口来进行连续时间轨迹估计。我们在真实数据集上进行了专门的实验，以证明采用非均匀连续时间轨迹表示的优势和有效性。我们的激光雷达-惯性相机里程计系统也在传感器退化的挑战性场景和大规模场景下进行了广泛评估，并显示出与最先进的方法相当或更高的精度。
Coco-LIC在退化场景的里程计和建图结果，其中当激光雷达面向地面一段时间时，会发生严重的激光雷达退化。Coco-LIC克服了退化，成功地回到了原点。
主要贡献
- 我们提出了一个激光雷达-惯性相机里程计系统称为Coco-LIC，采用了非均匀B样条参数化的连续时间轨迹。与基于均匀B样条的方法相比，这里的控制点是通过我们提出的简单而有效的分布策略动态放置的。
- 融合了激光雷达、惯性单元、相机数据，无需任何插值。基于重建的激光雷达点云地图和视觉像素的光流跟踪，我们构建了当前图像帧的帧到图重投影误差，不包括视觉像素的深度估计和优化。
- 在真实场景中控制点非均匀放置的必要性，并证明了控制点间距策略的有效性。此外，对整个系统在几个具有挑战性的数据集上进行了广泛的测试，证明了其实时性能和高精度。
算法详解
系统概述
非均匀连续时间轨迹生成
自适应非均匀技术
激光-惯性-相机优化
- 激光帧图匹配因子
在连续时间框架中，我们估计整个激光扫描周期内的轨迹，而不是仅仅估计单个时刻的姿态，这使得能够同时进行运动失真消除和位姿估计。对于t时刻的激光雷达平面点，可以将它转到全局坐标系：
- 视觉帧图匹配因子
另外，在优化过程中对重投影使用了柯西鲁棒核函数进行外点剔除。基于已知深度地图点的光流跟踪的方式，可以避免视觉特征的三角测量和滑动窗口优化，从而将滑动窗口保持在[tκ1，tκ]内，以实现高效率和准确性，而不涉及大量控制点。
- IMU因子和Bias因子
注意：这里并不是用的预积分，而是使用的原始观测数据构建的因子。
IMU因子公式：
- 图优化和边缘化
注意：整个优化过程是调用Ceres的LM算法进行求解的，为了加速求解，采用了解析求导的方式。
与视觉重投影相关的激光地图
实验对比
- 控制点的均匀放置和非均匀放置效果对比
使用的测试数据集介绍：
smooth case: 地面车辆在平稳的道路上行驶，清洁机器人在办公园区工作。
violent case: 四足机器人反复撞击地面，空中机器人快速躲避障碍物。
hybrid case: 手持扫描设备构建环境地图。
APE结果的RMSE和不同控制点分布的LIO优化的耗时(单位:米/毫秒)。最好的结果用粗体标出。uni-x表示x个控制点，而non-uni表示动态放置控制点。fail意味着RMSE已经超过1米。
混合序列上控制点非均匀分布的LIO轨迹估计。轨迹的不同颜色对应于控制点的不同密度。从蓝色到红色，控制点从稀疏到密集。
uni-1不足以精确拟合剧烈运动的轨迹段，会导致估计失败。添加足够的控制点可以提高精度，然而对于简单和平滑的运动轮廓来说，这似乎是不必要的，并且会导致额外的计算消耗。通过在剧烈运动期间增加控制点并在平滑运动期间减少控制点，non-uni以几乎最低的时间成本获得了最高的精度。
顶部是Hybrid1中的IMU数据，中间是通过自适应技术每个时间段设置的控制点数量。底部显示了基于不同控制点分布的姿态误差曲线。由于采用了自适应非均匀技术，non-uni在整个轨迹上保持小的姿态误差。
LIC里程计的鲁棒性和精度评估
数据集：Challenging Degenerate Dataset（来自R3LIVE和FAST-LIVO，这些序列表现出严重的退化，例如小FoV的固态激光雷达面向地面或墙壁，以及面向无纹理表面或导致相机图像模糊的剧烈运动）；large-scale UrbanNav dataset（城市大尺度场景数据集）。
对比方案：LVI-SAM, R3LIVE, FAST-LIVO, CLIC, FAST-LIO2, VINS-Mono
R3LIVE和FAST-LIVO所有数据集上的起点到终点漂移误差(单位:米/度)。seq 00、seq 01和seq 02来自R3LIVE。激光雷达退化和视觉退化数据集来自FAST-LIVO。
由于强烈的运动和变化的照明，VINS-Mono不能在视觉挑战序列中工作，因为它不能稳定地跟踪视觉特征；FAST-LIO无法处理激光雷达贴近墙面的情况，R3LIVE和FAST-LIVO可以处理大部分场景，但也有都存在显著漂移的情况。然而，Coco-LIC在所有数据集上都表现出优越的性能。值得注意的是，尽管CLIC融合了所有信息，但它仍然无法在激光雷达退化的情况下工作。这是因为CLIC强制固定滑动窗口中的大多数控制点，导致没有充分利用可视范围内的数据。
在UrbanNav数据集上APE结果的RMSE (m)
在UrbanNav数据集上，由于运动物体引起的错误特征跟踪，VINS-Mono表现不佳。LIO和LICO在没有退化激光雷达场景的情况下取得了令人满意的结果。然而，FAST-LIVO的轨迹误差略高，可能是由于其稀疏的直接视觉对准受到移动物体和照明变化的影响。值得注意的是，在高动态的运动场景中，CLIC和CocoLIC的精度更高，这归因于它们的连续时间轨迹表示，并有效地融合了高频率IMU数据。此外，Coco-LIC重用高质量的地图点构建视觉重投影残差，因此性能比CLIC更好。
FAST-LIO2、CLIC、Coco-LIC不同模块在数据集上的平均时间消耗(毫秒)。
总结与展望
这篇文章中作者提出一种连续时间激光雷达-惯性相机里程计，使用非均匀B样条紧耦合来自激光雷达、惯性测量单元和相机的信息。该方法通过在运动剧烈的地方放置更多的控制点，在运动平稳的地方放置更少的控制点，以适中的时间消耗实现了更高的姿态估计精度。此外，它还利用激光雷达地图点来构建帧到地图的视觉残差因子，这避免了通过多个视觉关键帧进行深度估计和优化。作者还在真实数据集实验证明了非均匀控制点放置的重要性和我们的非均匀连续时间方法的有效性。鲁棒性和准确性评估表明，Coco-LIC优于其他SOTA LIC里程计，即使在严重退化的情况下。在未来，研究更有效的地图管理策略并整合事件摄像机等互补传感器应该会很有意义。
总而言之，Coco-LIC是一项从真正意义上紧耦合激光、相机、IMU数据的多传感器融合SOTA方案，值得深入研究！
