点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
论文作者 | Xiaolei Lang
编辑 | 自动驾驶之心
笔者个人理解
机器人在弱结构化、弱纹理环境中的状态估计是SLAM中的难点，考虑到不同传感器之间的互补，多传感器融合是必不可少的。然而这些传感器数据的频率、时间可能不一致，一种常见的解决方案是对来自不同传感器的同一时刻的测量值或估计姿态做插值处理以进行融合。连续时间轨迹表示可以避免插值，并允许在任何给定时间进行姿态查询，从而有助于融合来自各种传感器的高频率和异步观测数据。然而在许多应用中，没有关于要估计的轨迹的复杂性的先验信息。无法动态调整控制点的分布，均匀B样条易于参数化不足或过度。Coco-LIC是一种采用非均匀B样条参数化的连续时间紧耦合激光惯性相机里程计。与均匀B样条不同，非均匀B样条允许动态控制点分布，而不是固定频率。与R3LIVE 和FAST-LIVO 不同，这种方法通过在固定的时间间隔内同时融合来自三个传感器的数据，而不是分别融合激光雷达-惯性或视觉-惯性数据。代码已经开源。
原文摘要
在本文中，我们提出了一种有效的连续时间激光雷达-惯性相机里程计，利用非均匀B样条紧耦合来自激光雷达、惯性测量单元和相机的测量值。与基于均匀B样条的连续时间方法相比，我们的非均匀B样条方法在实现实时效率和高精度方面具有显著的优势。这是通过动态和自适应地放置控制点来实现的，同时考虑到运动的变化。为了能够在短滑动窗口优化内有效融合异构激光雷达-惯性相机数据，我们使用来自全局激光雷达地图的对应地图点为视觉像素分配深度，并为当前图像帧中的相关像素构建帧到地图的重投影因子。这种方式避免了视觉像素深度优化的必要性，视觉像素深度优化通常需要具有大量控制点的冗长滑动窗口来进行连续时间轨迹估计。我们在真实数据集上进行了专门的实验，以证明采用非均匀连续时间轨迹表示的优势和有效性。我们的激光雷达-惯性相机里程计系统也在传感器退化的挑战性场景和大规模场景下进行了广泛评估，并显示出与最先进的方法相当或更高的精度。
主要贡献
我们提出了一个激光雷达-惯性相机里程计系统称为Coco-LIC，采用了非均匀B样条参数化的连续时间轨迹。与基于均匀B样条的方法相比，这里的控制点是通过我们提出的简单而有效的分布策略动态放置的。
融合了激光雷达、惯性单元、相机数据，无需任何插值。基于重建的激光雷达点云地图和视觉像素的光流跟踪，我们构建了当前图像帧的帧到图重投影误差，不包括视觉像素的深度估计和优化。
在真实场景中控制点非均匀放置的必要性，并证明了控制点间距策略的有效性。此外，对整个系统在几个具有挑战性的数据集上进行了广泛的测试，证明了其实时性能和高精度。
算法详解
系统概述
系统输入：激光雷点云、IMU数据、相机图像数据；初始时刻，假设系统是静止的，通过IMU观测数据初始化IMU的bias和重力向量。之后连续时间轨迹会以激光雷达的频率进行扩展。假设我们已经估计了之前的轨迹，那么一旦该时间间隔中的LIC数据准备好，将估计中的轨迹，其中。随后，新增加的控制点被初始化，并使用激光雷达平面点，IMU原始测量值和中的最新图像以因子图的方式进一步优化。。最后，在边缘化之后，我们为scan2map匹配更新局部地图，为图像和激光地图关联更新全局激光地图。
系统的优化状态量：
注意：状态量中的第一项是用于非均匀B样条的控制点，第二项中的是k-1和k时刻IMU的陀螺仪bias和加速度计bias，它们服从高斯随机游走。
将时间范围内的激光雷达平面点定义为，IMU数据，最新的图像数据
非均匀连续时间轨迹生成
假设有k个控制点，连续时间轨迹在时间 的6-DoF姿态表示:
其中，表示控制点（）。表示B样条中两个相邻的节点；是向量的第个元素；
是累积混合矩阵，是动态变化的，其计算公式：
B样条的时间导数推导公式：
自适应非均匀技术
在 中计算全局坐标系下的平均角速度和线加速度的范数:
其中， 和 是来自IMU观测的角速度和线速度。表示IMU观测的数量。是全局坐标系下的重力向量。然后根据图2决定控制点的数量，新的控制点初始值是最近一次优化中的最后一个控制点值。它们均匀分布在中，因此可以查询任意时刻的位姿。
激光-惯性-相机优化
激光帧图匹配因子
在连续时间框架中，我们估计整个激光扫描周期内的轨迹，而不是仅仅估计单个时刻的姿态，这使得能够同时进行运动失真消除和位姿估计。对于t时刻的激光雷达平面点，可以将它转到全局坐标系：
激光匹配残差构建：在局部激光地图中找距离 最近的5个点，然后拟合平面，计算法向量以及点 到该平面的距离。
注意：点到面的距离残差，其中是平面的单位法向量，是平面到原点的距离；
视觉帧图匹配因子
假设在最后一帧图像中关联到了地图点 和对应的像素点，然后在 中用KLT光流追踪，得到。然后用基础矩阵RANSAC和PnP去除外点，最后得到与当前图像帧像素点成功关联的地图点。
追踪到的地图点在当前帧图像中的重投影误差公式：
另外，在优化过程中对重投影使用了柯西鲁棒核函数进行外点剔除。基于已知深度地图点的光流跟踪的方式，可以避免视觉特征的三角测量和滑动窗口优化，从而将滑动窗口保持在[tκ1，tκ]内，以实现高效率和准确性，而不涉及大量控制点。
IMU因子和Bias因子
注意：这里并不是用的预积分，而是使用的原始观测数据构建的因子。
IMU因子公式：
Bias因子公式:
其中，和 是在 t 时刻IMU的原始角速度和线速度观测，和可以从连续时间轨迹的导数中计算得到：
图优化和边缘化
图优化最小二乘问题构建：
其中，是边缘化先验，是及激光匹配残差，是视觉重投影残差，是IMU因子，是Bias因子，是各自对应的协方差矩阵。
在对新增加的控制点和IMU偏差进行优化之后， 将边缘化在接下来的 内不涉及的状态，并且获得约束状态的先验因子:
注意：整个优化过程是调用Ceres的LM算法进行求解的，为了加速求解，采用了解析求导的方式。
与视觉重投影相关的激光地图
为了快速查询与图像相关联的地图点，维护以体素形式存储的全局激光地图。在对中的轨迹进行优化之后，使用来自第k帧的激光雷达平面点更新全局激光地图。为了实现精准的关联，追踪到的地图点如果在第k帧图像中的重投影误差较大，则将其剔除。然后，将当前FoV中的地图点投影到第k帧图像上，并将新的成功关联的地图点添加到成功关联的地图点集合 中。同时，确保跟踪的地图点均匀分布在图像上，并优先保留更靠近相机的地图点，以缓解遮挡问题。
实验对比
控制点的均匀放置和非均匀放置效果对比
使用的测试数据集介绍：
smooth case: 地面车辆在平稳的道路上行驶，清洁机器人在办公园区工作。
violent case: 四足机器人反复撞击地面，空中机器人快速躲避障碍物。
hybrid case: 手持扫描设备构建环境地图。
优化的时间消耗随着更多的控制点而显著增加，这可能是由于状态维数的增加。相比之下，在大多数情况下，non-uni自适应地以每秒1个的密度放置控制点，从而获得与uni-1相当的精度和计算时间。
uni-1不足以精确拟合剧烈运动的轨迹段，会导致估计失败。添加足够的控制点可以提高精度，然而对于简单和平滑的运动轮廓来说，这似乎是不必要的，并且会导致额外的计算消耗。通过在剧烈运动期间增加控制点并在平滑运动期间减少控制点，non-uni以几乎最低的时间成本获得了最高的精度。
LIC里程计的鲁棒性和精度评估
数据集：Challenging Degenerate Dataset（来自R3LIVE和FAST-LIVO，这些序列表现出严重的退化，例如小FoV的固态激光雷达面向地面或墙壁，以及面向无纹理表面或导致相机图像模糊的剧烈运动）；large-scale UrbanNav dataset（城市大尺度场景数据集）。
对比方案：LVI-SAM, R3LIVE, FAST-LIVO, CLIC, FAST-LIO2, VINS-Mono
由于强烈的运动和变化的照明，VINS-Mono不能在视觉挑战序列中工作，因为它不能稳定地跟踪视觉特征；FAST-LIO无法处理激光雷达贴近墙面的情况，R3LIVE和FAST-LIVO可以处理大部分场景，但也有都存在显著漂移的情况。然而，Coco-LIC在所有数据集上都表现出优越的性能。值得注意的是，尽管CLIC融合了所有信息，但它仍然无法在激光雷达退化的情况下工作。这是因为CLIC强制固定滑动窗口中的大多数控制点，导致没有充分利用可视范围内的数据。
在UrbanNav数据集上，由于运动物体引起的错误特征跟踪，VINS-Mono表现不佳。LIO和LICO在没有退化激光雷达场景的情况下取得了令人满意的结果。然而，FAST-LIVO的轨迹误差略高，可能是由于其稀疏的直接视觉对准受到移动物体和照明变化的影响。值得注意的是，在高动态的运动场景中，CLIC和CocoLIC的精度更高，这归因于它们的连续时间轨迹表示，并有效地融合了高频率IMU数据。此外，Coco-LIC重用高质量的地图点构建视觉重投影残差，因此性能比CLIC更好。
总结与展望
这篇文章中作者提出一种连续时间激光雷达-惯性相机里程计，使用非均匀B样条紧耦合来自激光雷达、惯性测量单元和相机的信息。该方法通过在运动剧烈的地方放置更多的控制点，在运动平稳的地方放置更少的控制点，以适中的时间消耗实现了更高的姿态估计精度。此外，它还利用激光雷达地图点来构建帧到地图的视觉残差因子，这避免了通过多个视觉关键帧进行深度估计和优化。作者还在真实数据集实验证明了非均匀控制点放置的重要性和我们的非均匀连续时间方法的有效性。鲁棒性和准确性评估表明，Coco-LIC优于其他SOTA LIC里程计，即使在严重退化的情况下。在未来，研究更有效的地图管理策略并整合事件摄像机等互补传感器应该会很有意义。
总而言之，Coco-LIC是一项从真正意义上紧耦合激光、相机、IMU数据的多传感器融合SOTA方案，值得深入研究！
参考
[1] Coco-LIC: Continuous-Time Tightly-Coupled LiDAR-Inertial-Camera Odometry using Non-Uniform B-spline
① 全网独家视频课程
BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、协同感知、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）
视频官网：www.zdjszx.com
② 国内首个自动驾驶学习社区
近2000人的交流社区，涉及30+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！
③【自动驾驶之心】技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）
④【自动驾驶之心】平台矩阵，欢迎联系我们！
