企业官网建设流程全解析

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 2021；IROS 2021 相关工作版本常被引用

一句话总结

LVI-SAM 把 LiDAR、视觉和 IMU 放进一个互相增强的紧耦合 SLAM 框架：视觉惯性子系统负责高频视觉跟踪和回环候选，激光惯性子系统负责几何稳定的 scan-to-map 与全局优化。工程上它不是简单把 VINS 和 LIO-SAM 串起来，而是让两套子系统通过深度关联、初值、因子图和回环精化共享状态。

工程视角：这篇论文要实现什么

从代码角度看，LVI-SAM 要实现一个 ROS 在线多传感器 SLAM 系统。输入包括 3D LiDAR 点云、IMU、单目或双目相机图像，以及相机-IMU-LiDAR 外参。输出包括高频 odometry、关键帧轨迹、稀疏视觉地图、局部/全局点云地图、回环优化后的全局位姿和 TF。

它的运行时可以拆成两条主线：

1. VIS：visual-inertial system，处理图像特征、IMU 预积分、视觉滑窗优化、视觉回环。
2. LIS：lidar-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::vector<cv::Point2f>keypoints;std::vector<int>feature_ids;std::vector<double>lidar_depth;// LiDAR 投影/关联得到的特征深度SE3 T_w_c;};structVisualWindowState{std::deque<VisualFrame>frames;std::vector<SE3>poses;// T_w_i 或 T_w_cstd::vector<Vec3>velocities;std::vector<Vec3>bias_acc;std::vector<Vec3>bias_gyro;std::vector<InverseDepth>feature_depths;};

LIS 侧核心状态更接近 LIO-SAM：

structLidarKeyFrame{doubletimestamp;SE3 T_w_l;Vec3 velocity;ImuBias bias;PointCloud corner_points;PointCloud surface_points;PointCloud raw_or_deskewed_cloud;};structLidarFactorGraphState{std::vector<LidarKeyFrame>keyframes;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{ThreadSafeQueue<ImuMsg>imu_queue;ThreadSafeQueue<ImageMsg>image_queue;ThreadSafeQueue<PointCloudMsg>lidar_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 rendering

VIS 后端是滑窗非线性优化，核心 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 functor：

residual=observed_uv-project(T_c_w*P_w);problem.AddResidualBlock(newReprojectionFactor(obs_i,obs_j),loss,pose_i,pose_j,inv_depth,extrinsic);

如果 LiDAR 给出了特征深度，单目特征的深度变量可以被初始化得更准，或者加入深度 residual：

r_depth = z_lidar_feature - z_visual_feature_estimate

LIS 的 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(BetweenFactor<Pose3>(X(i-1),X(i),lidar_delta,noise));initialEstimate.insert(X(i),pose_guess);isam.update(gtSAMgraph,initialEstimate);currentEstimate=isam.calculateEstimate();

LVI-SAM 的“紧耦合”不是单个巨大优化器同时优化所有视觉、LiDAR 和 IMU 变量，而是两个强耦合子系统通过状态、深度、初值和闭环约束进行信息交换。这种设计更容易做到实时，也更符合 ROS 多节点工程组织。

关键创新点

1. LiDAR-Visual-Inertial 双子系统互相增强：VIS 和 LIS 都能独立工作，但会共享初始化、初值、深度和回环信息。
2. 用 LiDAR 深度增强视觉特征：降低单目 VIO 初始化和尺度估计难度，对低纹理/高速运动更稳。
3. 用视觉估计辅助 LiDAR scan matching：在 LiDAR 几何退化或初值较差时，VIS 位姿能提高 scan-to-map 收敛概率。
4. 视觉回环 + LiDAR 精化：视觉负责快速召回候选，LiDAR 负责几何验证和精确相对位姿，减少错误闭环。
5. 基于 smoothing and mapping 的可增量优化框架：LIS 侧使用 iSAM2/GTSAM，适合在线加入 IMU、LiDAR、GPS 和 loop factor。
6. 子系统失败容忍设计：某个传感器或子系统短时失效时，另一个子系统仍可维持状态估计。

实验与结果

论文在多平台、多尺度、多环境数据上评估 LVI-SAM，比较重点是轨迹精度、建图质量和在纹理弱、几何退化、室内外切换等场景下的鲁棒性。原文表格中给出具体 ATE/RPE 等数值，本文不复述具体数值。定性结论是：相比单独的视觉惯性或激光惯性系统，LVI-SAM 在跨环境数据上更稳定，尤其在单一传感器容易退化的场景中收益明显。

复现路线：从零写一个最小版本

最小复现不要一开始追求完整 LVI-SAM。建议按下面路径拆解：

1. 环境：Ubuntu 18.04/20.04 + ROS Melodic/Noetic，安装 GTSAM、PCL、OpenCV、Eigen、Ceres（视觉侧可能需要），优先用 Docker 固定依赖。
2. 数据：先用官方推荐 rosbag 或自采 LiDAR+camera+IMU 数据，确认话题名、频率、时间戳、外参和内参。
3. MVP 1：只跑 LiDAR-IMU，完成 deskew、feature extraction、scan-to-map、GTSAM/iSAM2 关键帧图优化。
4. MVP 2：只跑视觉前端，完成特征跟踪、IMU propagation、滑窗姿态估计或至少视觉 odometry。
5. MVP 3：把 LiDAR 点投影到相机图像，为视觉特征补深度，验证特征深度分布是否合理。
6. MVP 4：让 VIS 输出 pose 初值给 LIS，比较 scan matching 迭代次数和失败率。
7. 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

建议阅读顺序：

1. README.md：确认依赖、数据、运行命令和传感器要求。
2. config/params_camera.yaml、config/params_lidar.yaml：理解话题名、外参、噪声、LiDAR 参数和阈值。
3. launch/run.launch：看哪些节点一起启动，节点之间如何通信。
4. src/lidar_odometry/utility.h：公共参数、消息类型、点类型和工具函数。
5. src/lidar_odometry/imageProjection.cpp：点云 deskew、range image 投影、IMU 队列使用。
6. src/lidar_odometry/featureExtraction.cpp：edge/surface 特征选择逻辑。
7. src/lidar_odometry/imuPreintegration.cpp：IMU 预积分、odometry propagation、GTSAM 因子接口。
8. src/lidar_odometry/mapOptmization.cpp：scan-to-map、关键帧、因子图、回环和地图发布，是 LIS 主干。
9. src/visual_odometry/visual_feature：图像特征提取和跟踪。
10. src/visual_odometry/visual_estimator：视觉滑窗估计、IMU/视觉约束和与 LiDAR 深度的关系。
11. src/visual_odometry/visual_loop：视觉回环检测、候选管理和回环信息输出。

阅读时建议开两个终端分别跟踪 ROS topic 和源码调用：一个跑rqt_graph/rostopic hz，一个按节点入口看 publish/subscribe。LVI-SAM 的难点不是单个函数，而是两个子系统在时间戳和坐标系上的状态交换。

工程调参与踩坑

1. 外参错误是第一大坑。相机-IMU、LiDAR-IMU、LiDAR-camera 任一方向弄反，都会表现为视觉深度关联混乱、scan matching 初值漂移或地图重影。
2. 时间同步必须严格检查。LiDAR 一帧扫描内部点时间、IMU 高频时间戳、图像曝光时间不一致，会直接破坏 deskew 和视觉-激光融合。
3. LiDAR 深度投影到图像时要处理遮挡。最近点不一定是特征真实深度，尤其在物体边缘、玻璃、动态行人和稀疏点云区域。
4. VIS 和 LIS 坐标系要统一。ROS 常见camera optical frame、imu frame、lidar frame、map/odom轴向不同，建议每次发布 TF 后用可视化检查。
5. scan-to-map 初值不要盲信 VIS。视觉在快速旋转、低纹理、强光照变化下可能给出错误初值，需要质量评分或退化保护。
6. 回环必须双重验证。视觉 place recognition 容易在重复走廊、楼梯、停车场误召回，LiDAR ICP fitness 和位姿跳变阈值必须严格。
7. GTSAM 噪声参数会显著影响结果。IMU noise、bias random walk、LiDAR odom noise、loop factor noise 需要根据传感器质量调，不宜直接照搬。
8. 实时性瓶颈通常在局部地图构建、最近邻搜索、iSAM2 更新和视觉回环。大场景要控制关键帧数量、voxel leaf size 和回环频率。
9. 动态物体会污染视觉特征和点云面特征。实际部署需要前端剔除或鲁棒核保护。
10. Docker 复现时要提前处理 GUI/RViz、PCL/GTSAM ABI、ROS 版本和显卡/USB 设备映射问题。

局限性与改进方向

LVI-SAM 的系统复杂度较高，对标定、同步和计算资源要求明显高于单一传感器 SLAM。它依赖传统特征和几何前端，在强动态、雨雾、低照度、强反光和高重复结构环境下仍可能失败。视觉和 LiDAR 子系统之间的信息交换也增加了调试难度：错误可能来自任意一个坐标变换、时间戳、初值或噪声模型。

可改进方向包括：

1. 加入更鲁棒的多相机或鱼眼视觉模型，扩大视场并提升回环召回。
2. 使用语义/动态物体剔除，减少动态场景对视觉和 LiDAR residual 的污染。
3. 用更现代的 LiDAR 前端，例如 ikd-tree 增量地图或 FAST-LIO2 风格直接点面配准，提高实时性。
4. 将视觉回环替换为更强的全局描述子或学习式 place recognition，但仍保留几何验证。
5. 建立自动化标定和在线外参微调机制，降低部署成本。
6. 把地图表示从关键帧点云扩展到可压缩的 voxel/submap/TSDF/semantic map，服务长期运行。

延伸阅读

1. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping。理解 LIS 因子图和 scan-to-map 主体。
2. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator。理解 VIS 滑窗、IMU 预积分和视觉回环。
3. LOAM: Lidar Odometry and Mapping in Real-time。理解 LiDAR edge/surface 特征和 scan matching 基础。
4. iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree。理解 GTSAM 增量优化的理论和工程接口。
5. Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map。适合理解如何增强 LiDAR 回环检测。