LVI-SAM可以认为是LIO-SAM和VINS-MONO的合体，在此基础上的修改不大。

github： https://github.com/TixiaoShan/LVI-SAM

paper: LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

有一个注释版的代码：https://github.com/electech6/LVI-SAM_detailed_comments

这个注释版代码中一些关于坐标系的注释我认为是有错误的，大家擦亮眼睛。LIO-SAM和VINS-MONO我也分别写过比较详细的代码解析，详情见链接。

1. 综述

 这个是论文里的流程图，很贴心的介绍了LVI-SAM改了哪些地方：

1.VINS-MONO的初始化由LIO-SAM的imuPreintefration提供；

2.VINS-MONO的feature_tracker同时和来自LIO-SAM的imageProjection的lidar进行了深度关联，就不用自己进行三角化；

3.LIO-SAM的imageProjection订阅的里程计来自VINS-MONO的estimator的imucallback提供的高频里程计；

4.VINS只进行回环检测，不做重定位，回环检测的结果提供给LIO-SAM的mapOptimization进行icp匹配+全局图优化。

代码流程图如下：

在代码里，论文中指出的来自vio的between factor并没有加上：

2.注意事项

2.1 官方数据集的坐标系统

代码里坐标系系统比较混乱，官方数据集的坐标系是这样的：

红色是相机坐标系，蓝色是lidar坐标系，绿色是LVI-SAM的坐标系，橙色是VINS的坐标系，也就是IMU坐标系。官方配置文件中params_camera.yaml里的lidar_to_cam_XX外参指蓝色和绿色之间的外参，并不是蓝色和红色之间的外参。

此外，Feature_tracker_node的get_depth()中给特征点赋予lidar深度时，忽略了cam和lidar之间的平移，即image特征的单位球和点云的单位球球心不统一，分别是cam和IMU，rotation是统一的，都是为lidar的R。

3. LVI-SAM主要改了哪些地方

3.1. image feature的数据关联

在一个位于相机中心的单位球体上投影视觉特征和激光雷达深度点，借助球面2D kdtree找到球体上最近的三个深度点。特征深度是由视觉特征和照相机中心Oc形成的线的长度，它与笛卡尔空间中由三个深度点形成的平面相交。

考虑到每帧image都会对应积攒了一段时间的lidar点云，持续被追踪的特征点可能会投影上不同的深度值。所以他们会检查一个特征点附近的深度值差判断是否采用该深度值。

lidar点云的积攒：visual_odometry/visual_feature/feature_tracker_node.cpp:lidar_callback()

深度关联：visual_odometry/visual_feature/feature_tracker.h:get_depth()

3.2 VINS的初始化

整个系统最开始初始化的地方是LIO-SAM的mapOptimization的updateInitialGuess()这里，第一帧会使用9轴IMU的pitch，roll信息确定R，首帧lidarpose确定之后提供给IMUPreintegration联合imu预积分量进行图优化，然后再发布高频的imuPose被vins-estimator-node订阅，用它计算得到的PQV+bias对VINS系统进行初始化。

获取LIO-SAM的数据： visual_odometry/visual_estimator/initial/initial_alignment.h:Class odometryRegister

VINS初始化：visual_odometry/visual_estimator/estimator.cpp: initialStructure()

3.3 其它

还有就是VINS给LIO-SAM提供pose初值，还有回环检测，这些内容都比较简单，就不贴地址了。