LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping日期2026-08-15论文链接https://arxiv.org/abs/2104.10831代码仓库https://github.com/TixiaoShan/LVI-SAM作者与机构Tixiao Shan, Brendan Englot, Carlo Ratti, Daniela Rus作者来自 Stevens Institute of Technology、MIT Senseable City Lab / CSAIL 等相关团队发表 venue / 年份arXiv 2021IROS 2021 相关工作版本常被引用一句话总结LVI-SAM 把 LiDAR、视觉和 IMU 放进一个互相增强的紧耦合 SLAM 框架视觉惯性子系统负责高频视觉跟踪和回环候选激光惯性子系统负责几何稳定的 scan-to-map 与全局优化。工程上它不是简单把 VINS 和 LIO-SAM 串起来而是让两套子系统通过深度关联、初值、因子图和回环精化共享状态。工程视角这篇论文要实现什么从代码角度看LVI-SAM 要实现一个 ROS 在线多传感器 SLAM 系统。输入包括 3D LiDAR 点云、IMU、单目或双目相机图像以及相机-IMU-LiDAR 外参。输出包括高频 odometry、关键帧轨迹、稀疏视觉地图、局部/全局点云地图、回环优化后的全局位姿和 TF。它的运行时可以拆成两条主线VISvisual-inertial system处理图像特征、IMU 预积分、视觉滑窗优化、视觉回环。LISlidar-inertial system处理点云 deskew、特征提取、scan-to-map、IMU 预积分、GTSAM/iSAM2 因子图和平面/边缘地图。两条主线不是互不相干。VIS 会用 LIS 的位姿帮助初始化和尺度稳定VIS 的特征会借助 LiDAR 深度获得更可靠的 3D 约束LIS 会用 VIS 的估计作为 scan matching 初值回环先由视觉做 place recognition再由 LiDAR ICP/scan matching 做几何精化。核心数据结构与状态量LVI-SAM 的状态量可以按 VIS、LIS 和共享层理解。VIS 侧核心状态类似 VINS 系列structVisualFrame{doubletimestamp;cv::Mat image;std::vectorcv::Point2fkeypoints;std::vectorintfeature_ids;std::vectordoublelidar_depth;// LiDAR 投影/关联得到的特征深度SE3 T_w_c;};structVisualWindowState{std::dequeVisualFrameframes;std::vectorSE3poses;// T_w_i 或 T_w_cstd::vectorVec3velocities;std::vectorVec3bias_acc;std::vectorVec3bias_gyro;std::vectorInverseDepthfeature_depths;};LIS 侧核心状态更接近 LIO-SAMstructLidarKeyFrame{doubletimestamp;SE3 T_w_l;Vec3 velocity;ImuBias bias;PointCloud corner_points;PointCloud surface_points;PointCloud raw_or_deskewed_cloud;};structLidarFactorGraphState{std::vectorLidarKeyFramekeyframes;gtsam::ISAM2 isam;gtsam::Values current_estimate;VoxelMap local_corner_map;VoxelMap local_surface_map;};共享层需要维护跨传感器外参和同步缓存structSensorExtrinsics{SE3 T_lidar_imu;SE3 T_cam_imu;SE3 T_lidar_cam;};structSharedEstimationBus{ThreadSafeQueueImuMsgimu_queue;ThreadSafeQueueImageMsgimage_queue;ThreadSafeQueuePointCloudMsglidar_queue;AtomicPose latest_vis_pose;AtomicPose latest_lis_pose;LoopCandidateQueue visual_loop_candidates;};实际官方仓库里LiDAR 侧核心文件包括src/lidar_odometry/imageProjection.cpp、featureExtraction.cpp、imuPreintegration.cpp、mapOptmization.cpp视觉侧在src/visual_odometry/visual_feature、visual_estimator、visual_loop下组织。算法主流程整体 pipeline 可以写成下面的伪代码启动 ROS 节点 读取 params_camera.yaml / params_lidar.yaml 初始化相机、LiDAR、IMU 外参 启动 VIS 线程、LIS 线程、回环线程、发布线程 每来一包 IMU 放入 VIS 和 LIS 的 IMU 队列 做短时 propagation用于高频 odometry 发布 每来一帧图像 提取/跟踪视觉特征 从附近 LiDAR 点云投影或关联特征深度 加入视觉滑窗 用 IMU 预积分 视觉重投影 LiDAR 深度约束优化 VIS 状态 输出 latest_vis_pose 检查视觉回环候选 每来一帧 LiDAR 用 IMU deskew 点云 range image 投影 提取 edge/surface 特征 取 latest_vis_pose 或 IMU propagation 作为初值 scan-to-map 优化当前 LiDAR 位姿 判断是否生成关键帧 加入 IMU 因子、LiDAR odom 因子、GPS/回环因子 iSAM2 增量优化 输出 latest_lis_pose 和局部/全局地图 回环线程 VIS 先用视觉词袋/特征匹配找候选 LIS 使用点云配准精化相对位姿 将 loop factor 加入 LIS 因子图 触发全局位姿和地图更新关键模块实现细节前端数据关联 / 特征提取 / scan matching / 渲染匹配视觉前端负责图像特征跟踪和回环候选发现。它需要维护 feature id使同一个角点跨帧形成 track。普通 VIO 中单目特征深度需要多帧三角化初始化脆弱LVI-SAM 的关键工程点是利用 LiDAR 点云给视觉特征补深度。实现时通常把 LiDAR 点云通过T_cam_lidar投影到图像平面在特征附近查找深度一致的点或者构造局部深度图。这样视觉 residual 可以更快形成三维约束减少纯视觉初始化等待。LiDAR 前端沿用 LIO-SAM/LOAM 风格。imageProjection.cpp做点云投影和 deskew依赖 IMU 对一帧扫描中的每个点按时间补偿运动畸变。featureExtraction.cpp根据 range image 中邻域曲率选择 edge 和 surface 点并过滤遮挡、平行光束、异常跳变区域。scan-to-map 阶段使用当前帧 edge/surface 点对局部地图做最近邻搜索构造点到线和点到面的残差。后端优化 / BA / pose graph / factor graph / differentiable renderingVIS 后端是滑窗非线性优化核心 residual 包括 IMU 预积分、视觉重投影和带 LiDAR 深度的视觉几何约束。窗口边缘化后保留 prior以控制计算量。LIS 后端使用 GTSAM/iSAM2 因子图。关键帧状态通常包含 pose、velocity、IMU bias。因子包括IMU preintegration factor约束相邻关键帧的运动连续性。LiDAR odometry factor来自 scan-to-map 的相对位姿或当前关键帧位姿约束。Loop closure factor视觉识别、LiDAR 精化后的闭环相对约束。可选 GPS factor户外场景下给全局漂移提供弱约束。工程上要注意 VIS 和 LIS 的优化频率不同。视觉滑窗通常图像帧率运行LiDAR 图优化按关键帧触发。两者共享位姿时必须考虑时间戳对齐不能把不同时间的 pose 直接混用。地图更新 / keyframe 管理 / voxel 管理 / submap 管理LIS 地图由关键帧点云拼接形成。每次 scan-to-map 不应使用全局所有点云而是取当前位姿附近的关键帧构造局部 corner/surface map并使用 voxel downsample 控制点数。关键帧策略通常基于平移、旋转和时间间隔if distance(T_current, T_last_keyframe) trans_threshold or angle(T_current, T_last_keyframe) rot_threshold or time_delta max_interval: create_lidar_keyframe() add_factors_to_graph() update_local_map()视觉地图主要是滑窗内特征 tracks 和回环数据库。长期地图不需要保存所有视觉点但回环模块需要保存关键帧图像、描述子、位姿和必要的特征观测。初始化、重定位、回环或失败恢复LVI-SAM 的鲁棒性来自两个子系统互相托底。纹理差时 VIS 可能失败LIS 仍可依赖 LiDAR 几何和 IMU 运行几何退化或 LiDAR 特征少时VIS 可提供相对运动初值。初始化时LIS 的尺度稳定位姿能帮助 VIS 避免长时间等待单目初始化。回环时VIS 做快速候选召回LIS 做几何验证避免纯视觉感知混淆造成错误闭环。失败恢复需要在工程上显式处理特征数量过低、scan matching Hessian 退化、IMU 时间戳跳变、外参不合理、点云 deskew 失败都应输出日志并降低该子系统对共享状态的影响权重。数学模型到代码的对应关系VIS 的视觉重投影 residual 可以理解为给定特征三维点或逆深度、相机位姿和外参投影到当前图像与观测像素相减。代码里通常是一个 cost functorresidualobserved_uv-project(T_c_w*P_w);problem.AddResidualBlock(newReprojectionFactor(obs_i,obs_j),loss,pose_i,pose_j,inv_depth,extrinsic);如果 LiDAR 给出了特征深度单目特征的深度变量可以被初始化得更准或者加入深度 residualr_depth z_lidar_feature - z_visual_feature_estimateLIS 的 scan-to-map residual 分两类。edge 点找局部地图中的线特征构造点到线距离surface 点找局部平面构造点到面距离r_edge distance(transform(p_edge, T_w_l), line(q1, q2)) r_surf n^T * transform(p_surf, T_w_l) d优化变量是当前帧T_w_l。优化完成后把相邻关键帧之间的运动、IMU 预积分和回环相对位姿写入 GTSAM 因子图。iSAM2 的代码结构通常是gtSAMgraph.add(ImuFactor(X(i-1),V(i-1),X(i),V(i),B(i-1),preint));gtSAMgraph.add(BetweenFactorPose3(X(i-1),X(i),lidar_delta,noise));initialEstimate.insert(X(i),pose_guess);isam.update(gtSAMgraph,initialEstimate);currentEstimateisam.calculateEstimate();LVI-SAM 的“紧耦合”不是单个巨大优化器同时优化所有视觉、LiDAR 和 IMU 变量而是两个强耦合子系统通过状态、深度、初值和闭环约束进行信息交换。这种设计更容易做到实时也更符合 ROS 多节点工程组织。关键创新点LiDAR-Visual-Inertial 双子系统互相增强VIS 和 LIS 都能独立工作但会共享初始化、初值、深度和回环信息。用 LiDAR 深度增强视觉特征降低单目 VIO 初始化和尺度估计难度对低纹理/高速运动更稳。用视觉估计辅助 LiDAR scan matching在 LiDAR 几何退化或初值较差时VIS 位姿能提高 scan-to-map 收敛概率。视觉回环 LiDAR 精化视觉负责快速召回候选LiDAR 负责几何验证和精确相对位姿减少错误闭环。基于 smoothing and mapping 的可增量优化框架LIS 侧使用 iSAM2/GTSAM适合在线加入 IMU、LiDAR、GPS 和 loop factor。子系统失败容忍设计某个传感器或子系统短时失效时另一个子系统仍可维持状态估计。实验与结果论文在多平台、多尺度、多环境数据上评估 LVI-SAM比较重点是轨迹精度、建图质量和在纹理弱、几何退化、室内外切换等场景下的鲁棒性。原文表格中给出具体 ATE/RPE 等数值本文不复述具体数值。定性结论是相比单独的视觉惯性或激光惯性系统LVI-SAM 在跨环境数据上更稳定尤其在单一传感器容易退化的场景中收益明显。复现路线从零写一个最小版本最小复现不要一开始追求完整 LVI-SAM。建议按下面路径拆解环境Ubuntu 18.04/20.04 ROS Melodic/Noetic安装 GTSAM、PCL、OpenCV、Eigen、Ceres视觉侧可能需要优先用 Docker 固定依赖。数据先用官方推荐 rosbag 或自采 LiDARcameraIMU 数据确认话题名、频率、时间戳、外参和内参。MVP 1只跑 LiDAR-IMU完成 deskew、feature extraction、scan-to-map、GTSAM/iSAM2 关键帧图优化。MVP 2只跑视觉前端完成特征跟踪、IMU propagation、滑窗姿态估计或至少视觉 odometry。MVP 3把 LiDAR 点投影到相机图像为视觉特征补深度验证特征深度分布是否合理。MVP 4让 VIS 输出 pose 初值给 LIS比较 scan matching 迭代次数和失败率。MVP 5加入视觉回环候选和 LiDAR ICP 精化向因子图添加 loop factor。建议验证指标轨迹 ATE/RPE、局部建图重影程度、scan matching 迭代次数、视觉跟踪特征数、LiDAR 深度关联成功率、每模块耗时、回环误检率。调试日志至少包括每帧图像特征数、关联到 LiDAR 深度的特征数、每帧点云 deskew 时间、edge/surface 特征数、scan-to-map residual 均值、关键帧数量、iSAM2 update 时间、回环候选和 ICP fitness。代码阅读指南官方仓库https://github.com/TixiaoShan/LVI-SAM建议阅读顺序README.md确认依赖、数据、运行命令和传感器要求。config/params_camera.yaml、config/params_lidar.yaml理解话题名、外参、噪声、LiDAR 参数和阈值。launch/run.launch看哪些节点一起启动节点之间如何通信。src/lidar_odometry/utility.h公共参数、消息类型、点类型和工具函数。src/lidar_odometry/imageProjection.cpp点云 deskew、range image 投影、IMU 队列使用。src/lidar_odometry/featureExtraction.cppedge/surface 特征选择逻辑。src/lidar_odometry/imuPreintegration.cppIMU 预积分、odometry propagation、GTSAM 因子接口。src/lidar_odometry/mapOptmization.cppscan-to-map、关键帧、因子图、回环和地图发布是 LIS 主干。src/visual_odometry/visual_feature图像特征提取和跟踪。src/visual_odometry/visual_estimator视觉滑窗估计、IMU/视觉约束和与 LiDAR 深度的关系。src/visual_odometry/visual_loop视觉回环检测、候选管理和回环信息输出。阅读时建议开两个终端分别跟踪 ROS topic 和源码调用一个跑rqt_graph/rostopic hz一个按节点入口看 publish/subscribe。LVI-SAM 的难点不是单个函数而是两个子系统在时间戳和坐标系上的状态交换。工程调参与踩坑外参错误是第一大坑。相机-IMU、LiDAR-IMU、LiDAR-camera 任一方向弄反都会表现为视觉深度关联混乱、scan matching 初值漂移或地图重影。时间同步必须严格检查。LiDAR 一帧扫描内部点时间、IMU 高频时间戳、图像曝光时间不一致会直接破坏 deskew 和视觉-激光融合。LiDAR 深度投影到图像时要处理遮挡。最近点不一定是特征真实深度尤其在物体边缘、玻璃、动态行人和稀疏点云区域。VIS 和 LIS 坐标系要统一。ROS 常见camera optical frame、imu frame、lidar frame、map/odom轴向不同建议每次发布 TF 后用可视化检查。scan-to-map 初值不要盲信 VIS。视觉在快速旋转、低纹理、强光照变化下可能给出错误初值需要质量评分或退化保护。回环必须双重验证。视觉 place recognition 容易在重复走廊、楼梯、停车场误召回LiDAR ICP fitness 和位姿跳变阈值必须严格。GTSAM 噪声参数会显著影响结果。IMU noise、bias random walk、LiDAR odom noise、loop factor noise 需要根据传感器质量调不宜直接照搬。实时性瓶颈通常在局部地图构建、最近邻搜索、iSAM2 更新和视觉回环。大场景要控制关键帧数量、voxel leaf size 和回环频率。动态物体会污染视觉特征和点云面特征。实际部署需要前端剔除或鲁棒核保护。Docker 复现时要提前处理 GUI/RViz、PCL/GTSAM ABI、ROS 版本和显卡/USB 设备映射问题。局限性与改进方向LVI-SAM 的系统复杂度较高对标定、同步和计算资源要求明显高于单一传感器 SLAM。它依赖传统特征和几何前端在强动态、雨雾、低照度、强反光和高重复结构环境下仍可能失败。视觉和 LiDAR 子系统之间的信息交换也增加了调试难度错误可能来自任意一个坐标变换、时间戳、初值或噪声模型。可改进方向包括加入更鲁棒的多相机或鱼眼视觉模型扩大视场并提升回环召回。使用语义/动态物体剔除减少动态场景对视觉和 LiDAR residual 的污染。用更现代的 LiDAR 前端例如 ikd-tree 增量地图或 FAST-LIO2 风格直接点面配准提高实时性。将视觉回环替换为更强的全局描述子或学习式 place recognition但仍保留几何验证。建立自动化标定和在线外参微调机制降低部署成本。把地图表示从关键帧点云扩展到可压缩的 voxel/submap/TSDF/semantic map服务长期运行。延伸阅读LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping。理解 LIS 因子图和 scan-to-map 主体。VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator。理解 VIS 滑窗、IMU 预积分和视觉回环。LOAM: Lidar Odometry and Mapping in Real-time。理解 LiDAR edge/surface 特征和 scan matching 基础。iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree。理解 GTSAM 增量优化的理论和工程接口。Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map。适合理解如何增强 LiDAR 回环检测。