LVI-SAM_ROS2 精密的多传感器融合系统

LVI-SAM_ROS2 是一个精密的多传感器融合系统，专为 ROS2 实现了紧耦合的激光雷达-视觉-惯性 SLAM。该系统结合了基于 LiDAR 的建图、视觉惯性里程计和闭环检测的优势，在具有挑战性的环境中提供鲁棒且精确的状态估计。特征提取系统采用模块化设计模式，具有清晰的关注点分离。主类继承自，通过一系列明确定义的阶段协调整个处理流程：系统处理以距离图像格式组织的点云，利用CloudInfo消

2301_79815382

934人浏览 · 2025-11-28 20:02:34

2301_79815382 · 2025-11-28 20:02:34 发布

概述

LVI-SAM_ROS2 是一个精密的多传感器融合系统，专为 ROS2 实现了紧耦合的激光雷达-视觉-惯性 SLAM。该系统结合了基于 LiDAR 的建图、视觉惯性里程计和闭环检测的优势，在具有挑战性的环境中提供鲁棒且精确的状态估计。

系统架构

LVI-SAM_ROS2 架构围绕两个紧密耦合的子系统构建，这些子系统协同工作以提供全面的环境理解和精确定位：

核心组件

激光雷达-惯性系统 (LIS)

LIS 子系统通过精密的流程处理 LiDAR 点云：

图像投影 (imageProjection.hpp)：使用 IMU 和里程计数据进行运动补偿，将 3D 点云投影为距离图像
特征提取 (featureExtraction.hpp)：基于曲率分析从投影的距离图像中提取边缘和表面特征
IMU 预积分 (imuPreintegration.hpp)：使用 GTSAM 进行因子图优化的 IMU 预积分
地图优化 (mapOptmization.hpp)：执行扫描到地图的优化，包含闭环检测和全局一致性维护

LIS 流水线通过顺序处理链处理激光雷达点云和 IMU 测量值：

ImageProjection 模块作为激光雷达数据的入口点，负责点云去畸变和投影 imageProjection.hpp。该模块处理：

基于 IMU 的运动补偿以消除点云畸变
里程计集成以提高去畸变精度
距离图像生成以实现高效处理

FeatureExtraction 模块处理去畸变后的点云以提取几何特征 featureExtraction.hpp。它实现：

基于曲率的点分类
角点和表面特征提取
体素网格过滤以提高计算效率

IMUPreintegration 模块提供激光雷达帧之间的高频运动估计 imuPreintegration.hpp。关键组件包括：

基于 GTSAM 的 IMU 测量预积分
偏差估计和校正
故障检测机制
变换广播 (map→odom→base_link)

mapOptimization 模块执行扫描到地图匹配和全局优化 mapOptmization.hpp。该模块处理：

GTSAM 因子图优化
回环检测和校正
关键帧管理
GPS 集成（当可用时）

特征提取与点云处理

LVI-SAM_ROS2 中的特征提取模块实现了一套精密的点云处理流程，用于从 LiDAR 数据中识别几何显著特征，为后续的里程计估计提供支持。该模块作为激光雷达-惯性系统（LIS）的关键组件，将原始点云转换为结构化特征集，从而实现鲁棒的运动估计和建图。

架构概述

特征提取系统采用模块化设计模式，具有清晰的关注点分离。主类 FeatureExtraction 继承自 ParamServer，通过一系列明确定义的阶段协调整个处理流程：

系统处理以距离图像格式组织的点云，利用 CloudInfo 消息结构，该结构包含每个点的关键元数据，如环索引、列位置和距离值 CloudInfo.msg。

核心处理流程

输入处理和初始化

特征提取节点通过 /lidar/deskew/cloud_info 主题订阅来自图像投影模块的去畸变点云数据每个传入的 CloudInfo 消息通过 laserCloudInfoHandler 方法触发完整的处理流程

系统初始化几个关键数据结构：

点云容器：用于存储提取的角点特征、平面特征的独立容器
处理数组：曲率计算、邻域选择标记和点标签
体素网格滤波器：用于平面特征的可配置下采样

平滑度计算

特征提取的基础在于计算每个点的局部表面平滑度。算法使用 10 点窗口（每个目标点前后各 5 个点）计算曲率：

float diffRange = cloudInfo.point_range[i-5] + cloudInfo.point_range[i-4]
                + cloudInfo.point_range[i-3] + cloudInfo.point_range[i-2]
                + cloudInfo.point_range[i-1] - cloudInfo.point_range[i] * 10
                + cloudInfo.point_range[i+1] + cloudInfo.point_range[i+2]
                + cloudInfo.point_range[i+3] + cloudInfo.point_range[i+4]
                + cloudInfo.point_range[i+5];

该计算有效测量点与其局部平面邻域的偏离程度 [featureExtraction.cpp#L58-L63]。较高的曲率值表示潜在的边缘特征，而较低的值则表示平面表面。

遮挡和平行光束检测

为确保特征质量，系统实现了复杂的过滤机制：

遮挡检测：如果在 10 像素列差内相邻点之间存在显著的深度不连续（>0.3m），则标记为遮挡点。这可以防止选择 LiDAR 光束可能被部分阻挡的物体边缘特征。

平行光束过滤：当相邻两个点的深度差都超过当前点距离的 2% 时，标记这些点为潜在不可靠点。这过滤掉了可能位于几乎平行于激光束表面上的点，因为这种情况下距离测量会变得不准确。

特征提取算法

边缘特征选择

算法将每个扫描环分为 6 个段处理，以确保特征均匀分布。在每个段内，点按平滑度降序排序，优先处理高曲率点。边缘特征基于以下条件选择：

曲率阈值：点必须超过 edgeThreshold 才能作为边缘特征
最大数量：每段最多 20 个边缘点，防止过度集中
空间分离：选定的点标记其 ±5 列内的邻域点，确保特征间距

平面特征选择

平面特征采用互补的选择策略：

曲率阈值：低于 surfThreshold 的点作为平面特征
邻域标记：类似的空间分离确保均匀分布
体素过滤：平面特征使用体素网格滤波器进行额外下采样，减少计算负载

特征组织

提取的特征保持其原始扫描环结构，这对后续的扫描到地图优化至关重要。系统通过以下方式保持点之间的关系：

基于环的处理：N_SCAN 中的每个环独立处理
分段：每个环分为 6 个角度段，确保均匀覆盖
标签保留：点用标签标记（1 表示角点，-1 表示平面，0 表示未选择）

性能特征

特征类型	每段最大数量	选择标准	处理顺序
角点	20	曲率 > edgeThreshold	平滑度从高到低
平面点	无限制	曲率 < surfThreshold	平滑度从低到高

系统每个点云大约处理 N_SCAN × 6 个段，每个段独立提取特征。这种并行友好的设计使系统能够高效处理高密度 LiDAR 传感器，同时保持实时性能要求。

IMU预积分与运动估计

系统架构

IMU 预积分模块采用双线程架构运行，维护两个独立的预积分积分器，分别用于优化和实时预测。该架构利用 GTSAM 的因子图框架实现紧耦合传感器融合，并支持高效的增量优化。

核心组件

预积分积分器

系统维护两个具有不同用途的 GTSAM 预积分对象：

imuIntegratorOpt_: 处理优化周期之间的 IMU 测量值，用于因子图构建 imuPreintegration.hpp#L49
imuIntegratorImu_: 执行实时积分，用于高频里程计预测
两个积分器均配置了基于系统参数的噪声模型，包括加速度计协方差、陀螺仪协方差和积分不确定性 imuPreintegration.cpp#L31-L36。

因子图优化

优化过程构建包含以下关键因子的因子图：

IMU 因子: 编码连续状态间的预积分测量值 imuPreintegration.cpp#L147
偏差间因子: 将偏差演化建模为随机游走 imuPreintegration.cpp#L148-L149
位姿先验因子: 融入激光雷达里程计校正 imuPreintegration.cpp#L151

系统使用 ISAM2 进行增量优化，可在新测量值到达时高效更新 imuPreintegration.hpp#L67。

坐标系处理

实现的一个关键方面是 IMU 与激光雷达坐标系之间的坐标变换。系统定义了一个中间的 "imulidar" 坐标系，其坐标轴与激光雷达坐标系平行，原点位于 IMU 中心 imuPreintegration.hpp#L80-L90。

处理流程

里程计处理器

当激光雷达里程计到达时，系统执行：

系统初始化: 首次接收时设置初始位姿、速度和偏差先验 imuPreintegration.cpp#L75-L105
IMU 积分: 积分自上次校正以来的 IMU 测量值 imuPreintegration.cpp#L120-L132
因子构建: 构建优化因子并执行 ISAM2 更新 imuPreintegration.cpp#L145-L165
状态传播: 使用优化后的偏差重新传播 IMU 积分 imuPreintegration.cpp#L175-L195

IMU 处理器

对于每条 IMU 消息，系统执行：

队列管理: 将测量值添加到优化和预测队列 imuPreintegration.cpp#L254-L255
实时积分: 积分测量值用于即时预测 imuPreintegration.cpp#L261-L263
状态预测: 预测当前导航状态 imuPreintegration.cpp#L266
输出发布: 发布里程计、路径和 TF 变换 imuPreintegration.cpp#L269-L330

故障检测

系统实现了鲁棒的故障检测机制，用于识别退化优化结果：

速度检查: 标记超过 30 m/s 的速度为无效 imuPreintegration.cpp#L197-L201
偏差检查: 检测大于 0.1 单位的加速度计或陀螺仪偏差 imuPreintegration.cpp#L203-L208

基于GTSAM的Scan-to-Map优化

扫描到地图优化模块是 LVI-SAM 的核心定位与建图组件，它采用 GTSAM（Georgia Tech Smoothing and Mapping）库实现了一套复杂的因子图优化框架。该模块通过将当前 LiDAR 扫描与全局地图对齐来实现精确的姿态估计，同时通过回环检测和多传感器融合来保持一致性。

GTSAM 因子图架构

优化系统利用 GTSAM 的因子图框架来维护所有传感器测量的全局一致性。核心组件包括用于约束表示的 NonlinearFactorGraph、用于变量存储的 Values，以及用于增量优化的 ISAM2

扫描到地图优化流程

优化过程遵循系统化的方法以实现精确的姿态估计：

局部地图提取

系统首先在当前机器人位置周围的可配置搜索半径内提取相关地图点。extractNearby() 函数使用 k-d 树半径搜索来识别周围的关键帧姿态，然后转换并融合对应的角点和表面特征点云，创建用于对齐的局部地图。

基于特征的对应关系

优化利用两种类型的几何特征：

边缘特征：使用特征值分解的线到线对应关系来寻找边缘方向
平面特征：用于表面对齐的点到平面对应关系

cornerOptimization() 函数对边缘特征执行并行处理，计算点到线的距离并提取优化对应的系数

Levenberg-Marquardt 优化

核心优化使用 Levenberg-Marquardt 求解器来最小化当前扫描特征与地图特征之间的重投影误差。LMOptimization() 函数构建雅可比矩阵并使用 QR 分解求解姿态更新

因子图集成

里程计因子

系统通过里程计因子保持时间一致性。对于第一帧，添加具有高不确定性的先验因子。后续帧添加连接连续姿态的因子间约束，并使用精心调整的噪声模型

回环闭合集成

当检测到回环时，系统添加因子间约束来连接非连续姿态，实现全局一致性校正。addLoopFactor() 函数处理排队中的回环约束并将其集成到因子图中

增量优化

系统使用 GTSAM 的 ISAM2 进行高效的增量优化。添加新因子后，saveKeyFramesAndFactor() 函数更新因子图并获取最新的优化估计

性能优化

并行处理

系统使用 OpenMP 指令实现多线程特征处理，将计算负载分配到可用的 CPU 核心，显著提高了实时性能。

下采样策略

应用多个不同分辨率的体素网格滤波器来平衡计算效率和建图精度：

角点特征：较小的体素尺寸以保持精确的边缘
表面特征：较大的体素尺寸以提高计算效率
关键帧姿态：基于空间分布的自适应下采样

内存管理

系统为最近的关键帧维护循环缓冲区，并通过智能指针实现高效的点云重用，最大限度地减少连续运行期间的内存分配开销。

视觉-惯性系统 (VIS)

VIS 子系统提供具有滑动窗口优化的视觉惯性里程计：

特征跟踪：基于光流的连续帧间特征跟踪
视觉估计器 (estimator.h)：多状态约束卡尔曼滤波器，紧密耦合视觉和惯性测量
闭环检测：基于 DBoW 词袋的视觉闭环检测

VIS 流水线处理相机图像并将其与 IMU 数据集成：

FeatureTracker 模块实现视觉特征跟踪和深度配准 feature_tracker.h。关键功能包括：

使用光流的多特征跟踪
相机模型支持（针孔和鱼眼）
从激光雷达点云进行深度配准
特征匹配和异常值剔除

Estimator 模块实现核心 VINS-Mono 算法并集成激光雷达 estimator.h。它提供：

滑动窗口优化
视觉-惯性束调整
初始化和结构恢复
边缘化策略

LoopDetector 模块使用词袋方法实现视觉回环检测 loop_detection.h。组件包括：

基于 DBoW 词汇表的地点识别
关键帧管理
几何验证
回环约束生成

特征跟踪与光流

特征跟踪与光流模块是 LVI-SAM 视觉惯性系统的视觉基础，负责在图像序列中检测、跟踪并维护稳健的视觉特征。该模块实现了一个复杂的多阶段流水线，结合了经典计算机视觉技术与现代优化策略，为视觉里程计估计提供可靠的特征对应关系。

核心架构与数据流

特征跟踪系统围绕 FeatureTracker 类构建，该类协调从检测到速度计算的完整特征生命周期。系统按时间顺序处理图像，在保持连续帧之间对应关系的同时动态添加新特征以维持最佳特征分布。

跟踪流水线从图像预处理开始，可通过 CLAHE（对比度受限自适应直方图均衡化）应用可选的直方图均衡化，以改善不同光照条件下的特征检测

光流实现

跟踪系统的核心是使用 OpenCV calcOpticalFlowPyrLK 函数实现的 Lucas-Kanade 光流算法。该算法采用金字塔方法（21×21 窗口大小和 3 个金字塔层）将特征点从当前帧跟踪到后续帧

光流跟踪包含几个关键验证步骤：

边界验证：移出图像边界的特征立即被拒绝 [feature_tracker.cpp#L751-L753]
状态过滤：使用状态向量移除光流计算失败的特征
向量缩减：使用 reduceVector 辅助函数同步所有关联向量（先前点、当前点、ID、跟踪计数）

特征质量保证

基础矩阵 RANSAC

为确保对极几何一致性，系统采用基于 RANSAC 的基础矩阵估计。rejectWithF 方法在计算基础矩阵前将特征点投影到归一化坐标系，提高了对异常值的鲁棒性。

该过程包括：

相机模型投影：使用相机模型的 liftProjective 方法将点提升到 3D 空间
归一化：使用焦距和图像中心将 3D 点投影到归一化图像平面
RANSAC 过滤：以 0.99 置信度和可配置阈值计算基础矩阵

特征掩码管理

setMask 方法实现智能特征分布策略，优先考虑长期跟踪的特征，同时确保均匀的空间覆盖。该算法：

优先级排序：按跟踪计数降序对特征进行排序
圆形掩码：对每个接受的特征，在掩码中标记半径为 MIN_DIST 的圆形区域
鱼眼支持：使用预定义掩码模式对鱼眼相机进行特殊处理

特征检测与整合

适合跟踪的特征

当特征数量低于最大阈值（MAX_CNT）时，系统使用 OpenCV 的 goodFeaturesToTrack 函数检测新特征。该算法：

在掩码区域上操作以避免特征聚集
使用 0.01 的质量阈值进行角点检测
遵守特征之间的最小距离约束

特征生命周期管理

addPoints 方法将新检测的特征整合到跟踪系统中。新特征获得：

设置为 -1 的临时 ID（表示未建立对应关系）
初始跟踪计数为 1
整合到主跟踪向量中

去畸变与速度计算

相机模型集成

undistortedPoints 方法处理将畸变图像坐标转换为归一化坐标的关键任务。该过程：

3D 提升：使用相机模型将 2D 图像点提升到 3D 空间
归一化：3D 点除以其 Z 坐标获得归一化图像坐标
映射：哈希图维护特征 ID 与归一化位置之间的对应关系

点速度估计

对于已建立的特征（ID ≠ -1），系统通过比较当前和先前归一化位置计算像素速度

double v_x = (cur_un_pts[i].x - it->second.x) / dt;
double v_y = (cur_un_pts[i].y - it->second.y) / dt;

基于滑动窗口的视觉-惯性里程计

LVI-SAM_ROS2 中的视觉惯性里程计（VIO）系统采用滑动窗口框架实现紧密耦合的优化方法，在确保精度的同时保持实时性能。该系统通过非线性优化将视觉特征与 IMU 测量值融合，利用两种传感器的互补优势实现鲁棒的状态估计。

滑动窗口优化框架

VIO 系统的核心在包含多个连续帧的滑动窗口上运行，在保持有界计算复杂度的同时保留足够的时间信息以确保准确估计。窗口大小可配置，通常设置为维持 10 帧的视觉惯性数据。

Vector3d Ps[(WINDOW_SIZE + 1)];
Vector3d Vs[(WINDOW_SIZE + 1)];
Matrix3d Rs[(WINDOW_SIZE + 1)];
Vector3d Bas[(WINDOW_SIZE + 1)];
Vector3d Bgs[(WINDOW_SIZE + 1)];

系统为窗口中的每一帧维护位置（Ps）、速度（Vs）、旋转（Rs）和加速度计/陀螺仪偏置（Bas, Bgs）。这种状态表示通过联合优化实现视觉和惯性测量的紧密耦合。

因子图优化

VIO 系统构建一个因子图，其中多种类型的约束同时进行优化：

IMU 预积分因子

连续帧之间的 IMU 测量值被预积分以创建独立于优化频率的运动约束：

IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

预积分在 processIMU 函数中执行，该函数处理 IMU 插值和与视觉特征的时间同步。

视觉重投影因子

跨多帧跟踪的视觉特征创建重投影约束以链接相机位姿：

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

当相机-IMU 同步不完美时，系统支持标准投影因子和时间延迟补偿因子

边缘化因子

为维护滑动窗口，较旧的状态被边缘化，其信息被保留为先验约束：

MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks);

边缘化过程确保丢弃状态的信息不会丢失，而是作为紧凑的先验因子被整合。

窗口管理策略

系统基于帧选择实现两种边缘化策略：

MARGIN_OLD 策略

当最旧帧被边缘化时，其所有关联信息被转换为先验约束。此策略用于常规帧，其中最旧状态为当前优化提供的信息最少。

MARGIN_SECOND_NEW 策略

当倒数第二新帧被边缘化时，最新帧成为关键帧。此策略保持时间多样性，当最新帧包含不足视差或运动时使用。

窗口滑动逻辑在 slideWindow() 中实现 [estimator.cpp#L1200-L1300]，该函数根据选定策略处理状态转移和特征管理。

初始化和对齐

VIO 系统需要仔细的初始化以建立适当的尺度和坐标系对齐：

运动恢复结构初始化

当检测到足够的视觉视差时，系统执行全局 SFM 以恢复初始相机位姿和 3D 结构：

GlobalSFM sfm;
if(!sfm.construct(frame_count + 1, Q, T, l, relative_R, relative_T, sfm_f, sfm_tracked_points))
{
    marginalization_flag = MARGIN_OLD;
    return false;
}

视觉惯性对齐

视觉初始化后，系统通过视觉惯性对齐将视觉尺度和坐标系与 IMU 测量值对齐 [estimator.cpp#L400-L500]。

LiDAR 辅助初始化

当 LiDAR 里程计可用时，系统可利用其直接初始化，绕过具有挑战性的视觉惯性对齐过程：

if (lidar_info_available == true)
{
    // Update state from LiDAR odometry
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        Ps[i] = all_image_frame[rclcpp::Time(Headers[i].stamp).seconds()].T;
        Rs[i] = all_image_frame[rclcpp::Time(Headers[i].stamp).seconds()].R;
        // ...
    }
}

深度配准与激光雷达-视觉融合

深度配准与激光雷达-视觉融合模块是 LVI-SAM_ROS2 架构中的关键组成部分，通过复杂的深度关联和多传感器优化策略，实现视觉与激光雷达感知模态的紧密耦合。该系统通过将激光雷达点云测量与视觉特征跟踪集成，实现鲁棒的深度估计，构建了一个利用两种感知模态互补优势的统一感知框架。

深度配准架构

深度配准系统通过分层管道运行，处理视觉特征并将其与对应的激光雷达深度测量相关联。核心架构围绕 FeatureManager 类 feature_manager.h 展开，该类在多个帧之间维护特征轨迹，同时整合来自激光雷达传感器的深度信息。

深度集成发生在 FeaturePerFrame 级别 feature_manager.h，其中每个视觉特征轨迹维护一个从激光雷达测量初始化的关联深度值。depth 字段（第 42 行）存储激光雷达派生的深度信息，对于纯视觉特征初始设置为 -1，在建立有效激光雷达对应关系时更新。

激光雷达-视觉融合管道

融合过程从特征提取和跟踪开始，通过 processImage 接口 estimator.h 在帧间检测和关联视觉特征。该方法接收视觉特征轨迹和激光雷达初始化信息，使系统能够建立模态间的深度对应关系。

深度关联机制利用视觉特征投影与激光雷达点云测量之间的时空一致性。当视觉特征可以与激光雷达点关联时，lidar_depth_flag feature_manager.h 被设置为 true，表示该特征轨迹成功完成深度配准。

多模态因子图优化

融合后的深度信息通过专门的投影因子纳入优化框架，这些因子同时考虑视觉重投影约束和深度约束。系统采用两种主要因子类型：

标准投影因子 projection_factor.h：处理帧间特征观测的视觉重投影约束
时延投影因子 projection_td_factor.h：融入时间同步校正和速度信息以实现更精确的深度估计

这些因子被集成到基于 Ceres 的优化框架 estimator.cpp 中，共同贡献于紧密耦合的束调整，同时优化相机位姿、特征位置和深度估计。

滑动窗口深度管理

系统实现了复杂的滑动窗口方法来管理跨时间帧的深度信息。FeatureManager 在定义的窗口大小内维护特征轨迹，随着新观测的到来持续更新深度估计 feature_manager.h。

关键深度管理操作包括：

三角测量 feature_manager.h：对于纯视觉特征，从多视图几何计算深度
深度传播 feature_manager.h：在窗口滑动操作期间更新深度估计
异常值剔除 feature_manager.h：移除不一致的深度测量

跨模态初始化

融合系统支持跨模态初始化，其中激光雷达里程计为视觉-惯性系统提供初始位姿估计。odomRegister组件 estimator.h 维护一个激光雷达里程计测量队列，当有足够的特征轨迹和深度对应关系可用时，用于初始化视觉系统。

这种初始化策略通过利用激光雷达传感器的绝对尺度信息，同时保持视觉-惯性系统的高频位姿估计能力，确保了系统启动的鲁棒性。

性能优化策略

深度配准系统实施了多种优化策略以维持实时性能：

选择性深度关联：仅考虑具有足够视差和可见性的特征进行激光雷达深度关联
边缘化：高效边缘化旧的特征状态，同时在 Schur 补中保留信息
并行处理：视觉特征跟踪和激光雷达处理在独立线程中运行，进行同步数据交换

系统的架构实现了激光雷达传感器的深度信息与视觉特征跟踪的无缝集成，构建了一个在多变环境条件和传感器配置下保持精度的鲁棒感知框架。

基于Ceres的因子图优化

探讨了 LVI-SAM_ROS2 中使用 Ceres Solver 框架实现的复杂因子图优化系统。该实现展示了采用边缘化策略的紧耦合视觉惯性里程计方法，以实现实时性能。

架构概述

因子图优化集中在 Estimator 类 estimator.h 中，该类协调多种因子类型以实现鲁棒的状态估计。系统采用滑动窗口优化方法，为不同的传感器模态精心设计残差块。

核心因子类型

IMU 预积分因子

IMUFactor imu_factor.h 实现了连续 IMU 状态之间的预积分，为位姿、速度和偏置提供约束。该因子扩展自 ceres::SizedCostFunction<15, 7, 9, 7, 9>，具有一个连接两个连续状态的 15 维残差：

参数：帧 i 和帧 j 的位姿（7）、速度偏置（9）
残差：位置（3）、速度（3）、旋转（3）、加速度偏置（3）、陀螺仪偏置（3）
积分：使用 IntegrationBase 在关键帧之间进行预积分

视觉重投影因子

ProjectionFactor projection_factor.h 处理跨帧的视觉特征观测：

class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>

参数：位姿_i（7）、位姿_j（7）、外参（7）、逆深度（1）
残差：归一化坐标中的 2D 重投影误差
实现：将特征从相机 i 坐标系变换到相机 j 坐标系 projection_factor.cpp

该因子执行完整的变换链：

相机 i 坐标 → IMU i 坐标
IMU i 坐标 → 世界坐标
世界坐标 → IMU j 坐标
IMU j 坐标 → 相机 j 坐标

边缘化因子

MarginalizationFactor marginalization_factor.h 使用 Schur 补技术维护先前优化状态的信息。这使得高效的滑动窗口优化不会丢失历史约束。

优化流程

1. 问题设置

ceres::Problem problem;
ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);

2. 参数块

位姿：para_Pose[i] 配合 PoseLocalParameterization 用于流形优化
速度/偏置：para_SpeedBias[i] 使用标准欧几里得参数化
特征点：para_Feature[i] 表示逆深度
外参：para_Ex_Pose[i] 用于相机-IMU 变换

3. 残差块添加

// 来自前一窗口的边缘化
if (last_marginalization_info) {
    MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
    problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks);
}
 
// 连续帧之间的 IMU 因子
for (int i = 0; i < WINDOW_SIZE; i++) {
    IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
    problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
 
// 视觉重投影因子
for (auto &it_per_id : feature_manager.feature) {
    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
    problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
}

4. 求解器配置

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;

消息接口

系统使用自定义消息类型 CloudInfo.msg 在 LiDAR 处理节点间进行高效通信：

字段	类型	描述
header	std_msgs/Header	时间戳和帧信息
start_ring_index/end_ring_index	int32[]	点云环索引
point_col_ind/point_range	int32[]/float32[]	点列索引和距离
imu_available/odom_available	int64	传感器可用性标志
cloud_deskewed/cloud_corner/cloud_surface	sensor_msgs/PointCloud2	处理后的点云

关键特性

特性	实现	优势
紧耦合融合	优化中的直接传感器融合	提高准确性和鲁棒性
多模态闭环	视觉和 LiDAR 双重检测	增强全局一致性
运动补偿	基于 IMU 的去畸变	减少运动伪影
因子图优化	GTSAM + Ceres 求解器	全局最优解
实时性能	多线程处理	适用于自主导航