1. LIO-SAM 整体框架：前端和后端先分清楚
LIO-SAM 全称是 LiDAR-Inertial Odometry via Smoothing and Mapping，可以理解成“基于平滑与建图的激光-惯性里程计”。这里的 LiDAR 是激光雷达，负责提供环境几何结构；IMU 是惯性测量单元，负责提供角速度和加速度；Odometry 里程计 是连续估计机器人运动；Smoothing and Mapping 平滑与建图 指的是把多帧、多传感器约束放进因子图里统一优化，而不是只看当前帧。
LIO-SAM 的整体流程可以压缩成下面这个框：
┌──────────────────────────────────────────────────────────────┐
│ LIO-SAM 主流程 │
│ │
│ 1. imageProjection.cpp │
│ 原始点云整理、IMU/Odom 时间同步、点云去畸变、range image 投影 │
│ │
│ 2. featureExtraction.cpp │
│ 计算曲率、遮挡剔除、分段提取 corner / surface 特征 │
│ │
│ 3. mapOptimization.cpp │
│ 构建局部地图、scan-to-map 匹配、保存关键帧、大因子图优化 │
│ │
│ 4. imuPreintegration.cpp │
│ IMU 预积分、小因子图优化、高频 odometry、估计 IMU bias │
│ │
│ 5. GPS / Loop │
│ GPS 提供绝对位置约束，回环提供历史帧与当前帧的相对位姿约束 │
└──────────────────────────────────────────────────────────────┘
从“前端 / 后端”角度看，可以这样划分：
┌──────────────────────────────────────────────────────────────┐
│ 前端 Frontend │
│ │
│ imageProjection.cpp： │
│ 把原始点云变成时间对齐、去畸变、结构化的点云。 │
│ │
│ featureExtraction.cpp： │
│ 从去畸变点云里提取角点和平面点。 │
│ │
│ mapOptimization.cpp 的 scan-to-map 部分： │
│ 当前帧特征点和局部地图匹配，得到当前帧位姿。 │
│ │
│ 后端 Backend │
│ │
│ mapOptimization.cpp 的 factor graph 部分： │
│ 加 LiDAR、GPS、Loop 等因子，优化关键帧轨迹。 │
│ │
│ imuPreintegration.cpp： │
│ 用 IMU 预积分构建小因子图，估计 pose、velocity、bias。 │
└──────────────────────────────────────────────────────────────┘
所以要注意：mapOptimization.cpp 既有前端功能，也有后端功能。里面的 scan-to-map 是前端匹配；里面的 GTSAM / iSAM2 因子图是后端优化。
2. 前端第一步：imageProjection，点云整理和去畸变
imageProjection.cpp 是 LIO-SAM 前端入口。它收到一帧原始 LiDAR 点云后，不是马上拿去匹配，而是先做这几件事：
┌──────────────────────────────────────────────────────────────┐
│ imageProjection.cpp 主要工作 │
│ │
│ 1. 缓存当前 LiDAR 点云 │
│ 2. 取出当前点云起止时间 timeScanCur / timeScanEnd │
│ 3. 从 IMU 队列中找覆盖这一帧的 IMU 数据 │
│ 4. 从 odom 队列中找这一帧起止时刻附近的 odom 数据 │
│ 5. 对每个点做 deskew 点云去畸变 │
│ 6. 根据 ring 和水平角投影成 range image │
│ 7. 提取有效点并打包成 cloud_info 发给 featureExtraction │
└──────────────────────────────────────────────────────────────┘
2.1 为什么要点云去畸变？
机械式 LiDAR 一帧点云不是同一瞬间采集出来的。比如 10Hz 雷达，一帧扫描周期是 0.1 秒。第一颗点可能在：
100.000s
采集，最后一颗点可能在：
100.100s
采集。如果机器人在这 0.1 秒里前进或者转弯，那么这一帧里的点其实来自不同的机器人位姿。也就是说，这一帧点云本身是“运动中扫出来的”，不是一个瞬时刚体快照。
假设：
timeScanCur = 100.000; // 当前帧起始时间
timeScanEnd = 100.100; // 当前帧结束时间
point.time = 0.045; // 当前点相对本帧起点的时间
那么这个点的真实采集时间是：
pointTime = timeScanCur + point.time;
// pointTime = 100.045
这个点不是在 100.000s 采集的，而是在本帧开始后 0.045s 采集的。如果这 0.045 秒内小车已经转了角度、发生了平移，那么这个点就不能直接当作起始时刻的点使用。点云去畸变的目标就是：
┌──────────────────────────────────────────────────────────────┐
│ deskew 的目标 │
│ │
│ 把一帧点云中每个不同时刻采集的点，全部补偿到同一个时刻。 │
│ 在 LIO-SAM 中，通常是补偿到当前帧起始时刻 LiDAR 坐标系下。 │
└──────────────────────────────────────────────────────────────┘
如果不做 deskew，直墙可能被拉弯，柱子边缘会错位，后续计算曲率时会误判，scan-to-map 时点到线、点到面的残差会变大。
2.2 IMU 到底提供什么？
IMU 原始消息一般是：
sensor_msgs::Imu imuMsg;
里面关键字段是：
imuMsg.header.stamp
imuMsg.angular_velocity.x
imuMsg.angular_velocity.y
imuMsg.angular_velocity.z
imuMsg.linear_acceleration.x
imuMsg.linear_acceleration.y
imuMsg.linear_acceleration.z
imuMsg.orientation.x
imuMsg.orientation.y
imuMsg.orientation.z
imuMsg.orientation.w
解释放在一个框里：
┌──────────────────────────────────────────────────────────────┐
│ IMU 数据字段解释 │
│ │
│ header.stamp： │
│ IMU 时间戳，用来和 LiDAR 点的 pointTime 对齐。 │
│ │
│ angular_velocity.x/y/z： │
│ 角速度 wx、wy、wz，单位通常是 rad/s，表示绕 IMU 三轴转多快。 │
│ │
│ linear_acceleration.x/y/z： │
│ 加速度 ax、ay、az，单位通常是 m/s²，里面通常包含重力。 │
│ │
│ orientation.x/y/z/w： │
│ 姿态四元数，有些 IMU 会直接输出，但实际使用时仍需注意外参。 │
└──────────────────────────────────────────────────────────────┘
在 imageProjection 的 deskew 中，IMU 主要用于提供旋转变化。比如机器人在一帧扫描期间绕 z 轴转了 2°，IMU 角速度积分就能告诉系统从帧起点到某个点采集时刻，机器人已经转了多少。
直观积分形式是：
delta_roll += wx * dt;
delta_pitch += wy * dt;
delta_yaw += wz * dt;
其中：
wx, wy, wz：IMU 角速度
dt：相邻两条 IMU 数据的时间间隔
delta_roll / pitch / yaw：累计姿态变化
实际源码里会考虑 IMU 坐标系、LiDAR 坐标系外参、四元数和 bias，但理解时可以先抓住本质：IMU 提供一帧扫描期间的姿态变化，尤其是旋转变化。
2.3 平移信息从哪里来？
平移通常来自 odom 里程计。这个 odom 在 LIO-SAM 中常见来源是 imuPreintegration.cpp 发布的高频 IMU odometry，也可以来自轮速里程计或其他外部 odom。
odom 消息一般是：
nav_msgs::Odometry odomMsg;
odomMsg.header.stamp
odomMsg.pose.pose.position.x
odomMsg.pose.pose.position.y
odomMsg.pose.pose.position.z
odomMsg.pose.pose.orientation.x
odomMsg.pose.pose.orientation.y
odomMsg.pose.pose.orientation.z
odomMsg.pose.pose.orientation.w
在 deskew 中，平移补偿的思路是：
┌──────────────────────────────────────────────────────────────┐
│ odom 平移补偿思路 │
│ │
│ 1. 找当前 LiDAR 帧起始时刻附近的 odom，记作 startOdom。 │
│ 2. 找当前 LiDAR 帧结束时刻附近的 odom，记作 endOdom。 │
│ 3. 计算 startOdom 到 endOdom 的相对位移。 │
│ 4. 对每个点根据 relTime / scanPeriod 做线性插值。 │
└──────────────────────────────────────────────────────────────┘
比如一帧扫描 0.1 秒，起点 odom 位置是：
start position = [0.0, 0.0, 0.0]
终点 odom 位置是：
end position = [0.1, 0.0, 0.0]
说明这一帧里机器人向前走了 0.1m。某个点在本帧中间时刻采集：
relTime = 0.05
scanPeriod = 0.10
ratio = relTime / scanPeriod = 0.5
那么该点时刻的平移近似是：
posXCur = 0.5 * 0.1 = 0.05m
posYCur = 0
posZCur = 0
所以 deskew 里可以概括为：
IMU 提供旋转：
rotXCur, rotYCur, rotZCur
odom 提供平移：
posXCur, posYCur, posZCur
2.4 T_start、T_point、R_point、p_start 到底是什么？
先定义三个坐标系：
C_start：当前 LiDAR 帧起始时刻的 LiDAR 坐标系
C_point：当前点被采集那一刻的 LiDAR 坐标系
W：参考坐标系，可以理解成 odom/map/world 系
一帧 LiDAR 扫描过程中机器人在运动，所以每个点理论上都有自己的采集时刻坐标系。我们要做的是：
把 C_point 下的点，转换到 C_start 下。
变量解释如下：
┌──────────────────────────────────────────────────────────────┐
│ deskew 中几个核心变量 │
│ │
│ p_point： │
│ 当前点在“采集它那一刻”的 LiDAR 坐标系 C_point 下的坐标。 │
│ 比如 p_point = [10, 0, 0]^T，表示点在当时雷达前方 10m。 │
│ │
│ T_start： │
│ 本帧起始时刻 LiDAR 坐标系 C_start 到参考系 W 的变换。 │
│ T_start = [R_start, t_start; 0, 1] │
│ │
│ T_point： │
│ 当前点采集时刻 LiDAR 坐标系 C_point 到参考系 W 的变换。 │
│ T_point = [R_point, t_point; 0, 1] │
│ │
│ R_point： │
│ T_point 里的旋转部分，来自 IMU 插值得到的 roll/pitch/yaw。 │
│ │
│ t_point： │
│ T_point 里的平移部分，来自 odom 在一帧内的平移插值。 │
│ │
│ p_start： │
│ 当前点补偿到本帧起始坐标系 C_start 后的坐标，即 deskew 点。 │
└──────────────────────────────────────────────────────────────┘
你问 T_point 是不是机器人的位姿。可以这样回答：概念上，T_point 可以理解为当前点采集时刻 LiDAR/机器人相对参考坐标系的瞬时位姿；源码实现里，它更多表示当前点时刻相对当前帧参考时刻的运动变换，由 IMU 旋转和 odom 平移构造出来。
R_point 是一个 3×3 旋转矩阵。它由 roll、pitch、yaw 组合而来：
R_point = Rz(yawCur) * Ry(pitchCur) * Rx(rollCur);
这是矩阵相乘，不是普通数字乘法。三个矩阵是：
Rx(roll) =
[ 1 0 0
0 cosr -sinr
0 sinr cosr ]
Ry(pitch) =
[ cosp 0 sinp
0 1 0
-sinp 0 cosp ]
Rz(yaw) =
[ cosy -siny 0
siny cosy 0
0 0 1 ]
注意顺序不能乱换：
Rz * Ry * Rx != Rx * Ry * Rz
三维旋转不满足交换律，所以欧拉角组合必须按固定顺序。
2.5 为什么公式是 p_start = T_start^{-1} * T_point * p_point？
坐标变换链路是：
C_point –T_point–> W –T_start^{-1}–> C_start
当前点一开始在 C_point 下：
p_point
第一步，把它变到参考系 W：
p_world = T_point * p_point
第二步，把参考系 W 下的点变回本帧起始坐标系 C_start：
p_start = T_start.inverse() * p_world
合起来：
p_start = T_start.inverse() * T_point * p_point
放在框里：
┌──────────────────────────────────────────────────────────────┐
│ 点云去畸变核心公式 │
│ │
│ p_start = T_start^{-1} · T_point · p_point │
│ │
│ p_point：当前点在采集时刻 LiDAR 坐标系下的坐标 │
│ T_point：当前点采集时刻 LiDAR 坐标系到参考系的变换 │
│ T_start：本帧起始时刻 LiDAR 坐标系到参考系的变换 │
│ T_start^{-1}：参考系到本帧起始 LiDAR 坐标系的反向变换 │
│ p_start：补偿到本帧起始时刻后的点坐标，也就是 deskew 后的点 │
└──────────────────────────────────────────────────────────────┘
T_start.inverse() 是矩阵逆。如果：
T_start
表示：
C_start -> W
那么：
T_start.inverse()
表示：
W -> C_start
如果：
T =
[ R t
0 1 ]
那么：
T^{-1} =
[ R^T -R^T t
0 1 ]
这里 R^T 是旋转矩阵转置。旋转矩阵有性质：
R^{-1} = R^T
所以 T_start.inverse() 的作用就是把参考系中的点变回本帧起始 LiDAR 坐标系。
2.6 deskewPoint 代码和每个变量解释
代码形态如下：
PointType deskewPoint(PointType *point, double relTime)
{
// 当前点真实采集时间
double pointTime = timeScanCur + relTime;
float rotXCur, rotYCur, rotZCur;
float posXCur, posYCur, posZCur;
// 从 IMU 队列中插值得到当前点时刻的姿态变化
findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
// 从 odom 一帧内增量中插值得到当前点时刻的平移
findPosition(relTime, &posXCur, &posYCur, &posZCur);
// 构造当前点时刻的变换 T_point
Eigen::Affine3f transFinal =
pcl::getTransformation(posXCur, posYCur, posZCur,
rotXCur, rotYCur, rotZCur);
// 当前点时刻坐标系 -> 本帧起始坐标系
Eigen::Affine3f transBt = transStartInverse * transFinal;
PointType newPoint;
newPoint.x = transBt(0,0) * point->x
+ transBt(0,1) * point->y
+ transBt(0,2) * point->z
+ transBt(0,3);
newPoint.y = transBt(1,0) * point->x
+ transBt(1,1) * point->y
+ transBt(1,2) * point->z
+ transBt(1,3);
newPoint.z = transBt(2,0) * point->x
+ transBt(2,1) * point->y
+ transBt(2,2) * point->z
+ transBt(2,3);
return newPoint;
}
变量解释：
┌──────────────────────────────────────────────────────────────┐
│ deskewPoint 变量解释 │
│ │
│ point： │
│ 当前 LiDAR 点，包含 x/y/z/time/ring 等字段。 │
│ │
│ relTime： │
│ 当前点相对本帧起始时刻的时间偏移。 │
│ │
│ pointTime： │
│ 当前点真实采集时间，等于 timeScanCur + relTime。 │
│ │
│ findRotation()： │
│ 根据 pointTime 从 IMU 队列中插值得到 roll/pitch/yaw。 │
│ │
│ rotXCur / rotYCur / rotZCur： │
│ 当前点时刻相对本帧起始时刻的旋转。 │
│ │
│ findPosition()： │
│ 根据 relTime 从 odom 增量中插值得到平移。 │
│ │
│ posXCur / posYCur / posZCur： │
│ 当前点时刻相对本帧起始时刻的平移。 │
│ │
│ transFinal： │
│ 当前点时刻的 4×4 变换矩阵，可以理解为 T_point。 │
│ │
│ transStartInverse： │
│ 本帧起始时刻变换的逆，可以理解为 T_start^{-1}。 │
│ │
│ transBt： │
│ 当前点时刻坐标系到本帧起始坐标系的相对变换。 │
│ │
│ newPoint： │
│ 去畸变后的点，也就是 p_start。 │
└──────────────────────────────────────────────────────────────┘
2.7 一个具体数值例子
假设一帧点云扫描时间是 0.1 秒。当前点在扫描开始 0.05 秒时被采集。小车在这 0.05 秒里：
向前移动 0.05m
yaw 转了 2.5°
roll = 0
pitch = 0
当前点在自己采集时刻的雷达坐标系下是：
p_point = [10, 0, 0]^T
只考虑 yaw，旋转矩阵是：
Rz(2.5°) =
[ cos2.5° -sin2.5° 0
sin2.5° cos2.5° 0
0 0 1 ]
近似：
cos2.5° ≈ 0.999
sin2.5° ≈ 0.0436
所以：
R_point =
[ 0.999 -0.0436 0
0.0436 0.999 0
0 0 1 ]
先旋转：
R_point * p_point =
[ 9.99
0.436
0 ]
再加平移：
t_point = [0.05, 0, 0]^T
得到：
p_start ≈ [10.04, 0.436, 0]^T
如果不去畸变，这个点会被当作：
[10, 0, 0]
但补偿后应该接近：
[10.04, 0.436, 0]
y 方向差了 0.436m。这个误差对于 LiDAR 匹配已经很大，足以让墙面变弯、角点错位。
2.8 range image 投影、columnDiff、cloudNeighborPicked
去畸变后，LIO-SAM 会把点云投影成二维矩阵，叫 range image 距离图像。假设雷达是 16 线：
N_SCAN = 16;
Horizon_SCAN = 1800;
那么 range image 是：
16 × 1800
每一行代表一根激光线，每一列代表一个水平角度位置。
点的行号来自：
rowIdn = point.ring;
ring 表示这个点属于第几根激光线。
点的列号来自水平角：
horizonAngle = atan2(point.x, point.y) * 180 / M_PI;
columnIdn = -round((horizonAngle – 90.0) / ang_res_x)
+ Horizon_SCAN / 2;
其中
horizonAngle：当前点的水平角
ang_res_x：水平角分辨率，比如 360 / 1800 = 0.2°
columnIdn：点在 range image 里的列号
点的距离是：
range = sqrt(point.x * point.x +
point.y * point.y +
point.z * point.z);
然后存入：
rangeMat.at<float>(rowIdn, columnIdn) = range;
fullCloud[index] = point;
遮挡剔除中会出现：
columnDiff = abs(columnIdn[i+1] – columnIdn[i]);
columnDiff 表示第 i 个点和第 i+1 个点在水平角方向上相隔多少列。如果 columnDiff 很小，说明这两个点在角度上很接近。如果角度接近但距离突然变化很大，说明可能是遮挡边界。
例如：
columnDiff = 1
depth1 = 10m
depth2 = 3m
这说明相邻角度上，一个点打在远墙，一个点打在近处柱子。这种地方曲率会很大，但不一定是稳定角点，而可能是遮挡造成的深度跳变。
cloudNeighborPicked 是标记数组：
cloudNeighborPicked[i] = 0 // 这个点可以选特征
cloudNeighborPicked[i] = 1 // 这个点不要选特征
遮挡剔除代码类似：
if (columnDiff < 10)
{
if (depth1 – depth2 > 0.3)
{
cloudNeighborPicked[i-5] = 1;
cloudNeighborPicked[i-4] = 1;
cloudNeighborPicked[i-3] = 1;
cloudNeighborPicked[i-2] = 1;
cloudNeighborPicked[i-1] = 1;
cloudNeighborPicked[i] = 1;
}
else if (depth2 – depth1 > 0.3)
{
cloudNeighborPicked[i+1] = 1;
cloudNeighborPicked[i+2] = 1;
cloudNeighborPicked[i+3] = 1;
cloudNeighborPicked[i+4] = 1;
cloudNeighborPicked[i+5] = 1;
cloudNeighborPicked[i+6] = 1;
}
}
为什么标记一串点？因为遮挡边界影响的不是单个点，而是一小段点。这些点会随着视角变化而跳动、消失或者错位，不适合作为稳定特征。
3. 前端第二步：featureExtraction，提取角点和平面点
featureExtraction.cpp 接收 imageProjection.cpp 输出的 cloud_info。此时点云已经经过 deskew，并且有 range image、每根线的起止索引等辅助信息。它的任务是从点云中提取两类特征：
┌──────────────────────────────────────────────────────────────┐
│ featureExtraction.cpp 提取两类特征 │
│ │
│ corner feature / edge feature： │
│ 角点、边缘点。曲率大，常来自墙角、柱子边缘、门框、物体轮廓。│
│ │
│ surface feature： │
│ 平面点。曲率小，常来自地面、墙面、桌面、平整表面。 │
└──────────────────────────────────────────────────────────────┘
3.1 曲率公式：怎么判断点是突出的还是平滑的？
LIO-SAM 的特征不是靠语义识别出来的，而是靠几何曲率。曲率的直观意思是：
当前点和周围点相比，突不突出，变化大不大。
源码思想是用当前点前后各 5 个邻居点的 range 来计算：
diffRange = range[i-5] + range[i-4] + range[i-3] + range[i-2] + range[i-1]
– 10 * range[i]
+ range[i+1] + range[i+2] + range[i+3] + range[i+4] + range[i+5];
cloudCurvature[i] = diffRange * diffRange;
公式框：
┌──────────────────────────────────────────────────────────────┐
│ 曲率计算公式 │
│ │
│ diffRange = Σ range[i+k] – 10 * range[i] │
│ k ∈ {-5,-4,-3,-2,-1, 1,2,3,4,5} │
│ │
│ curvature[i] = diffRange² │
│ │
│ range[i]：第 i 个点到 LiDAR 的距离 │
│ i-5 ~ i-1：当前点前面 5 个邻居 │
│ i+1 ~ i+5：当前点后面 5 个邻居 │
│ diffRange：当前点与邻域趋势的差异 │
│ curvature：曲率，越大越像边缘，越小越像平面 │
└──────────────────────────────────────────────────────────────┘
如果当前点和周围点都在一面墙上，距离变化平滑：
邻居和 ≈ 10 * 当前点距离
diffRange ≈ 0
curvature ≈ 0
这类点适合做平面点。
如果当前点位于墙角、柱子边缘、门框边缘：
邻居和 和 10 * 当前点距离 差很多
diffRange 很大
curvature 很大
这类点适合做角点候选。
举例：
[10,10,10,10,10,10,10,10,10,10,10]
中间点曲率：
邻居和 = 100
10 * 当前点 = 100
diffRange = 0
curvature = 0
说明它很平滑，适合平面点。
再看：
[10,10,10,10,10,5,10,10,10,10,10]
中间点曲率：
邻居和 = 100
10 * 当前点 = 50
diffRange = 50
curvature = 2500
说明它相对周围突变明显，适合作为角点候选。
3.2 遮挡剔除和邻域抑制
曲率大不一定是好角点，因为遮挡边界也会造成深度突变。比如近处柱子挡住远处墙，雷达扫过边界时，range 可能从 3m 突然跳到 10m。这种点曲率很大，但不稳定，不适合做特征。
所以 featureExtraction 会结合 cloudNeighborPicked 剔除：
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold)
{
// 可以选角点
}
其中：
edgeThreshold：角点曲率阈值
surfThreshold：平面点曲率阈值
cloudNeighborPicked[ind] == 0：该点没有被遮挡剔除，也不是重复邻域点
选中特征点后，还会把附近点标记掉。例如选中了第 1000 个点：
cloudNeighborPicked[1000] = 1;
cloudNeighborPicked[1001] = 1;
cloudNeighborPicked[1002] = 1;
…
意义是避免一堆相邻点都被选成特征。相邻点信息高度重复，全部选上只会增加计算量，不一定提高约束质量。
3.3 为什么要分段选特征？
LIO-SAM 通常会把每根激光线分成 6 段，每段单独选角点和平面点。
假设一根激光线有 1800 个点：
segment 0: 0 ~ 299
segment 1: 300 ~ 599
segment 2: 600 ~ 899
segment 3: 900 ~ 1199
segment 4: 1200 ~ 1499
segment 5: 1500 ~ 1799
代码形态
for (int i = 0; i < N_SCAN; i++)
{
for (int j = 0; j < 6; j++)
{
int sp = startRingIndex[i]
+ (endRingIndex[i] – startRingIndex[i]) * j / 6;
int ep = startRingIndex[i]
+ (endRingIndex[i] – startRingIndex[i]) * (j + 1) / 6 – 1;
sort(cloudSmoothness.begin() + sp,
cloudSmoothness.begin() + ep,
by curvature);
// 从曲率大到小选角点
// 从曲率小到大选平面点
}
}
变量解释：
┌──────────────────────────────────────────────────────────────┐
│ 分段选特征变量解释 │
│ │
│ N_SCAN：LiDAR 线数，比如 16、32、64。 │
│ startRingIndex[i]：第 i 根激光线有效点起始索引。 │
│ endRingIndex[i]：第 i 根激光线有效点结束索引。 │
│ sp：当前小段起始索引。 │
│ ep：当前小段结束索引。 │
│ cloudSmoothness：按曲率保存的点索引结构。 │
└──────────────────────────────────────────────────────────────┘
为什么不全局排序？举个场景：机器人在走廊里，左前方有一根柱子，右侧是一整面墙。柱子边缘曲率特别大。如果全局排序，曲率最大的几十个点可能全部集中在柱子边缘。后端优化时，约束都来自一个局部区域，空间分布很差，yaw、横向位移、走廊方向位移都可能不稳定。
分段以后，每个方向都能选到一些特征：
左侧有一些
右侧有一些
前方有一些
后方有一些
近处有一些
远处有一些
这样后端 scan-to-map 的约束更均匀，位姿优化更稳定。
4. 后端核心：mapOptimization，scan-to-map 匹配 + 因子图优化
mapOptimization.cpp 是 LIO-SAM 里最核心的模块。它做两大类工作：
┌──────────────────────────────────────────────────────────────┐
│ mapOptimization.cpp 两部分 │
│ │
│ 1. scan-to-map 匹配 │
│ 当前帧角点/平面点 和 局部地图角点/平面点 匹配，求当前位姿。 │
│ │
│ 2. 因子图优化 │
│ 保存关键帧，加入 LiDAR / GPS / Loop 因子，优化关键帧轨迹。 │
└──────────────────────────────────────────────────────────────┘
4.1 局部地图怎么构建？
当前帧来了以后，系统已有当前估计位姿：
transformTobeMapped =
[roll, pitch, yaw, x, y, z]
历史关键帧位姿保存在：
cloudKeyPoses3D
cloudKeyPoses6D
其中：
cloudKeyPoses3D：只保存 x, y, z，用于 KD-tree 搜索附近关键帧。
cloudKeyPoses6D：保存 x, y, z, roll, pitch, yaw, time，用于点云变换。
历史关键帧特征点云保存在：
cornerCloudKeyFrames[i]
surfCloudKeyFrames[i]
当前帧先根据当前估计位置，搜索附近关键帧：
PointType currentRobotPosPoint;
currentRobotPosPoint.x = transformTobeMapped[3];
currentRobotPosPoint.y = transformTobeMapped[4];
currentRobotPosPoint.z = transformTobeMapped[5];
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
kdtreeSurroundingKeyPoses->radiusSearch(
currentRobotPosPoint,
surroundingKeyframeSearchRadius,
pointSearchInd,
pointSearchSqDis);
变量解释：
┌──────────────────────────────────────────────────────────────┐
│ 局部关键帧搜索变量解释 │
│ │
│ currentRobotPosPoint：当前机器人估计位置。 │
│ kdtreeSurroundingKeyPoses：历史关键帧位置构成的 KD-tree。 │
│ cloudKeyPoses3D：历史关键帧位置点云。 │
│ surroundingKeyframeSearchRadius：局部地图搜索半径。 │
│ pointSearchInd：搜索到的关键帧编号。 │
│ pointSearchSqDis：搜索到的关键帧距离平方。 │
└──────────────────────────────────────────────────────────────┘
找到附近关键帧后，把这些关键帧的点云变换到地图坐标系：
for (int i = 0; i < pointSearchInd.size(); ++i)
{
int key = pointSearchInd[i];
*laserCloudCornerFromMap += *transformPointCloud(
cornerCloudKeyFrames[key],
&cloudKeyPoses6D->points[key]);
*laserCloudSurfFromMap += *transformPointCloud(
surfCloudKeyFrames[key],
&cloudKeyPoses6D->points[key]);
}
其中：
p_map = R_key * p_local + t_key
p_local 是关键帧局部坐标系下的点，R_key 和 t_key 是该关键帧在地图系下的姿态和平移，p_map 是变换到地图系后的点。
最后得到：
laserCloudCornerFromMap：局部角点地图
laserCloudSurfFromMap：局部平面地图
4.2 当前角点怎么匹配地图里的线结构？
当前帧角点是：
laserCloudCornerLast
角点不是图像角点，而是点云中的边缘点，常见于：
墙角边缘
柱子边缘
门框边缘
桌角边缘
物体轮廓边缘
后端要做的是：
当前帧角点变到地图系后，应该落在历史地图里的某条边缘线附近。
流程：
当前角点 p_current
|
| 当前估计位姿 T_current
v
p_map = T_current * p_current
|
| 在局部角点地图里找最近 5 个点
v
nearest 5 map corner points
|
| PCA 判断这 5 个点是不是线结构
v
如果是线，建立点到线残差
代码形态：
pointSel = pointAssociateToMap(pointOri);
kdtreeCornerFromMap->nearestKSearch(
pointSel,
5,
pointSearchInd,
pointSearchSqDis);
变量解释：
pointOri：当前帧原始角点，在当前帧 LiDAR 坐标系下。
pointAssociateToMap(pointOri)：用当前估计位姿把点变换到地图系。
pointSel：当前角点变换到地图系后的点。
kdtreeCornerFromMap：局部角点地图 KD-tree。
nearestKSearch(pointSel, 5)：找最近 5 个地图角点。
pointSearchInd：近邻点索引。
pointSearchSqDis：近邻点距离平方。
为什么找 5 个点？因为一个点不能定义线，两个点抗噪能力差，多个近邻点可以判断局部是否真的是一条边缘线。
4.3 点到线残差推导
假设找到 5 个地图角点：
p1, p2, p3, p4, p5
先求中心：
c = (p1 + p2 + p3 + p4 + p5) / 5
求协方差矩阵：
C = Σ (pi – c)(pi – c)^T
对 C 做特征值分解：
C v1 = λ1 v1
C v2 = λ2 v2
C v3 = λ3 v3
如果：
λ1 >> λ2, λ3
说明这 5 个点主要沿 v1 方向分布，也就是线结构。
┌──────────────────────────────────────────────────────────────┐
│ PCA 判断线结构 │
│ │
│ p1~p5：当前角点在地图中找到的 5 个最近角点。 │
│ c：这 5 个点的中心。 │
│ C：协方差矩阵，表示这 5 个点在空间中的分布。 │
│ λ1, λ2, λ3：三个主方向上的分布强度。 │
│ v1, v2, v3：三个主方向。 │
│ │
│ 如果 λ1 远大于 λ2、λ3，说明点主要沿 v1 分布，近似一条线。 │
└──────────────────────────────────────────────────────────────┘
构造线上的两个点：
a = c + 0.1 * v1
b = c – 0.1 * v1
当前角点 p 到线的距离：
d_line = || (p – a) × (p – b) || / || a – b ||
解释框：
┌──────────────────────────────────────────────────────────────┐
│ 点到线残差公式 │
│ │
│ d_line = || (p – a) × (p – b) || / || a – b || │
│ │
│ p：当前帧角点变换到地图系后的坐标。 │
│ a,b：地图线上的两个点。 │
│ p-a：从线点 a 指向当前点 p 的向量。 │
│ p-b：从线点 b 指向当前点 p 的向量。 │
│ ×：叉乘，模长等于由两个向量构成的平行四边形面积。 │
│ ||a-b||：线段长度。 │
│ d_line：点到线的垂直距离，也就是角点残差。 │
└──────────────────────────────────────────────────────────────┘
残差越小，说明当前角点越贴近地图中的边缘线。优化器会调整当前帧位姿，让所有角点的 d_line 尽量小。
4.4 当前平面点怎么匹配地图里的平面结构？
当前帧平面点是：
laserCloudSurfLast
平面点通常来自：
地面
墙面
桌面
平整物体表面
后端要做的是：
当前帧平面点变到地图系后，应该落在历史地图里的某个平面附近。
流程：
当前平面点 p_current
|
| 当前估计位姿 T_current
v
p_map = T_current * p_current
|
| 在局部平面地图里找最近 5 个点
v
nearest 5 map surface points
|
| 拟合平面 ax + by + cz + d = 0
v
建立点到平面残差
代码形态：
pointSel = pointAssociateToMap(pointOri);
kdtreeSurfFromMap->nearestKSearch(
pointSel,
5,
pointSearchInd,
pointSearchSqDis);
平面方程：
ax + by + cz + d = 0
其中 [a,b,c] 是平面法向量，d 是平面偏移。
用 5 个近邻点拟合平面，可以写成：
A =
[ x1 y1 z1
x2 y2 z2
x3 y3 z3
x4 y4 z4
x5 y5 z5 ]
A [a b c]^T = [-1 -1 -1 -1 -1]^T
然后令
d = 1
并归一化：
s = sqrt(a*a + b*b + c*c);
a = a / s;
b = b / s;
c = c / s;
d = d / s;
当前点到平面残差：
d_plane = a*xp + b*yp + c*zp + d
解释框：
┌──────────────────────────────────────────────────────────────┐
│ 点到平面残差公式 │
│ │
│ 平面方程： │
│ ax + by + cz + d = 0 │
│ │
│ 当前点到平面的残差： │
│ d_plane = a*xp + b*yp + c*zp + d │
│ │
│ [a,b,c]：平面法向量。 │
│ d：平面偏移。 │
│ [xp,yp,zp]：当前平面点变换到地图系后的坐标。 │
│ d_plane：点到平面的有符号距离残差。 │
└──────────────────────────────────────────────────────────────┘
如果当前位姿正确，当前平面点应该落在地图平面上，所以 d_plane 应该接近 0。
4.5 scan-to-map 优化到底优化什么？
当前帧位姿变量是：
x = [roll, pitch, yaw, tx, ty, tz]^T
目标函数是：
J(x) = Σ d_line_i(x)^2 + Σ d_plane_j(x)^2
框起来：
┌──────────────────────────────────────────────────────────────┐
│ scan-to-map 优化目标 │
│ │
│ x = [roll, pitch, yaw, tx, ty, tz]^T │
│ │
│ minimize J(x)： │
│ J(x) = Σ d_line_i(x)^2 + Σ d_plane_j(x)^2 │
│ │
│ d_line_i：第 i 个角点到地图线的距离。 │
│ d_plane_j：第 j 个平面点到地图平面的距离。 │
│ x：当前帧 6 自由度位姿。 │
└──────────────────────────────────────────────────────────────┘
每次迭代大概做：
1. 用当前位姿把当前帧特征点变到地图系；
2. 角点找地图线，建立点到线残差；
3. 平面点找地图面，建立点到面残差；
4. 组装雅可比和残差；
5. 解线性方程，得到位姿增量；
6. 更新 transformTobeMapped；
7. 判断是否收敛。
代码形态：
void scan2MapOptimization()
{
if (laserCloudCornerFromMap->points.size() > 10 &&
laserCloudSurfFromMap->points.size() > 100)
{
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
for (int iterCount = 0; iterCount < 30; iterCount++)
{
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization();
surfOptimization();
combineOptimizationCoeffs();
if (LMOptimization(iterCount) == true)
break;
}
}
}
其中：
cornerOptimization()：建立角点点到线残差。
surfOptimization()：建立平面点点到面残差。
combineOptimizationCoeffs()：合并角点和平面点残差。
LMOptimization()：求解当前帧位姿增量。
5. mapOptimization 的因子图优化：相对位姿和误差的关系
scan-to-map 得到当前帧位姿后，如果当前帧满足关键帧条件，就保存为关键帧，并加入因子图。这里很多人会混：因子里保存的是测量，优化目标里写的是误差。
以 LiDAR BetweenFactor 为例。
假设：
X_i：第 i 个关键帧位姿
X_j：第 j 个关键帧位姿
Z_ij：传感器测出来的 i 到 j 的相对位姿
Z_ij 是测量值。它表示 LiDAR 认为：
从 X_i 到 X_j 应该是这个相对运动。
但优化器当前也有一组估计值，根据估计值可以算：
Z_ij_hat = X_i^{-1} * X_j
误差就是测量相对位姿和当前估计相对位姿之间的差：
e_ij = Log( Z_ij^{-1} * Z_ij_hat )
完整解释：
在 LIO-SAM 的 mapOptimization 因子图优化里，X_i、X_j 表示的是关键帧在 map 地图坐标系下的全局位姿变量。它不是一个普通点，而是一个完整的 6 自由度位姿，包含 x、y、z、roll、pitch、yaw。如果用坐标变换表示，可以理解成：
X_i = T_map_lidar_i
含义：
X_i 表示第 i 个关键帧的 LiDAR 坐标系相对于 map 坐标系的位姿。
X_i = [x_i, y_i, z_i, roll_i, pitch_i, yaw_i]
X_j = [x_j, y_j, z_j, roll_j, pitch_j, yaw_j]
这些 X_i、X_j 一开始不是优化器凭空生成的。第一帧通常会作为地图坐标系的起点，并添加 PriorFactor 先验因子，用来固定整张地图的参考系。后续关键帧的初始值主要来自 LiDAR 前端的 scan-to-map 匹配结果。也就是当前帧点云进来后，系统先用 IMU、odom 或上一帧位姿给一个粗略预测，然后用当前点云和局部地图匹配，得到当前帧在 map 下的位姿，也就是 LIO-SAM 里的 transformTobeMapped。如果当前帧满足关键帧条件，就把这个位姿作为新变量 X_j 的初值插入因子图。
当前帧点云
↓
IMU / odom / 上一帧位姿给当前帧一个粗初值
↓
scan-to-map 点云匹配
↓
得到 transformTobeMapped
↓
如果当前帧满足关键帧条件
↓
X_j_initial = transformTobeMapped
↓
把 X_j_initial 插入因子图，作为 X_j 的初始估计
这里要分清三个东西：X_j_initial、Z_ij、X_j_optimized。X_j_initial 是当前关键帧的初始全局位姿，通常来自 scan-to-map；Z_ij 是传感器因子保存的相对位姿测量，表示从第 i 帧到第 j 帧，传感器认为机器人移动了多少、旋转了多少；X_j_optimized 是优化器综合所有因子后得到的最终位姿。它们可能都和 LiDAR 前端结果有关，但角色不同：X_j_initial 是变量初值，优化器可以改；Z_ij 是固定测量约束，加入因子图后一般不变；X_j_optimized 是最终优化结果。
LiDAR 相邻关键帧因子，也就是 BetweenFactor，比较的是传感器测到的相对位姿和优化器当前估计出来的相对位姿。传感器给的是 Z_ij，优化器当前有 X_i、X_j，所以优化器自己也能算出一个相对位姿：
传感器测量的相对位姿：
Z_ij
优化器根据当前变量算出来的相对位姿：
Z_hat_ij = X_i^{-1} * X_j
LiDAR 因子误差：
e_lidar_ij = Log( Z_ij^{-1} * Z_hat_ij )
代入 Z_hat_ij：
e_lidar_ij = Log( Z_ij^{-1} * ( X_i^{-1} * X_j ) )
这里的 X_i^{-1} * X_j 可以理解成：站在第 i 帧坐标系下，看第 j 帧相对第 i 帧移动了多少、旋转了多少。Z_ij 是 LiDAR 或其他传感器测出来的相对运动。二者越接近，说明优化器当前的 X_i、X_j 越符合这个传感器约束；二者差得越远，残差越大。因为位姿属于 SE(3)，不能直接像普通数字一样相减，所以用 Log() 把位姿差转换成普通的 6 维误差向量：
e_lidar_ij = [dx, dy, dz, droll, dpitch, dyaw]
dx, dy, dz：
平移误差。
droll, dpitch, dyaw：
旋转误差。
你前面问的那个问题非常关键：既然 Z_ij 可能就是由 X_i 和 X_j_initial 算出来的，那刚开始它们不是相等的吗？答案是：对，刚构造 LiDAR BetweenFactor 的那一刻，这条因子的残差往往就是 0 或者很小。
已有历史关键帧：
X_i
当前 scan-to-map 得到：
X_j_initial
构造 LiDAR 相对测量：
Z_ij = X_i^{-1} * X_j_initial
同时插入新变量初值：
X_j = X_j_initial
所以刚加入时：
X_i^{-1} * X_j
= X_i^{-1} * X_j_initial
= Z_ij
因此：
e_lidar_ij = Log( Z_ij^{-1} * Z_ij )
≈ 0
所以，如果图里只有这一条 LiDAR 因子，确实没什么好优化的，因为它一开始就满足了。但实际因子图里不只有 LiDAR 相邻帧约束，还可能有先验、GPS、回环、IMU、bias 等约束。这些因子会从不同角度“拉扯”关键帧位姿，而且它们之间可能会冲突。优化器最后不是让某一条因子完全正确，而是让所有因子的加权总误差最小。
因子图优化的总目标可以写成：
X* = argmin_X J(X)
J(X) = Σ_k [ e_k(X)^T * Ω_k * e_k(X) ]
合起来写：
X* = argmin_X Σ_k [ e_k(X)^T * Ω_k * e_k(X) ]
这里 argmin_X 不是和后面相乘，它是一个操作符，意思是：在所有可能的 X 里面，找到一组 X，让后面的总代价函数最小。 X* 里的 * 也不是乘号，而是“最优”的意思。所以这句话翻译成人话就是：找到一组最优关键帧位姿 X*，让所有因子的加权误差总和最小。
X：
所有待优化变量，例如 X_0, X_1, X_2, …, X_j。
X*：
优化后的最优变量。
e_k(X)：
第 k 个因子在当前 X 下的误差。
e_k(X)^T：
误差向量 e_k(X) 的转置。
如果 e_k(X) 是 6 × 1 列向量，
那么 e_k(X)^T 是 1 × 6 行向量。
Ω_k：
第 k 个因子的信息矩阵，也就是权重矩阵。
Ω_k = Σ_k^{-1}
Σ_k：
第 k 个因子的协方差矩阵。
e_k(X)^T * Ω_k * e_k(X) 是一个完整的加权误差项。假设 e_k(X) 是 6 × 1 的误差向量，Ω_k 是 6 × 6 的信息矩阵，那么：
e_k(X)^T * Ω_k * e_k(X)
维度变化：
(1 × 6) * (6 × 6) * (6 × 1) = 1 × 1
最终结果是一个普通数字，表示第 k 个因子的加权误差大小。
如果不加 Ω_k，那就是普通平方误差；加了 Ω_k 以后，就可以表达“哪个传感器更可信”。协方差 Σ_k 越小，说明测量越准，信息矩阵 Ω_k = Σ_k^{-1} 越大，权重越高；协方差越大，说明测量越不准，权重越低。
一维情况下：
cost = e^2 / σ^2
如果 σ 很小：
说明测量很准，cost 会被放大，优化器更重视这个因子。
如果 σ 很大：
说明测量不准，cost 会变小，优化器允许它有更大误差。
把 LIO-SAM 中常见因子的总误差写完整一些，可以是：
J =
Σ || e_prior ||^2_Σprior
+ Σ || e_lidar ||^2_Σlidar
+ Σ || e_gps ||^2_Σgps
+ Σ || e_loop ||^2_Σloop
+ Σ || e_imu ||^2_Σimu
+ Σ || e_bias ||^2_Σbias
其中：
|| e ||^2_Σ = e^T * Σ^{-1} * e
也就是：
|| e ||^2_Σ = e^T * Ω * e
其中 PriorFactor 用来固定地图参考系，一般约束第一帧位姿：
PriorFactor 误差：
e_prior = Log( Z_0^{-1} * X_0 )
Z_0：
第一帧的先验位姿。
X_0：
优化器当前估计的第一帧位姿。
LiDAR BetweenFactor 约束相邻关键帧的局部运动关系：
LiDAR BetweenFactor 误差：
e_lidar_ij = Log( (Z_ij_lidar)^{-1} * ( X_i^{-1} * X_j ) )
Z_ij_lidar：
LiDAR 前端得到的第 i 帧到第 j 帧的相对位姿测量。
X_i^{-1} * X_j：
优化器当前估计出来的第 i 帧到第 j 帧的相对位姿。
GPSFactor 主要约束某个关键帧的全局位置，通常不直接约束姿态：
GPSFactor 误差：
e_gps_i = p( X_i ) – z_i_gps
p( X_i )：
从位姿 X_i 中取出位置部分，也就是 [x_i, y_i, z_i]。
z_i_gps：
GPS 测量的位置。
LoopFactor 回环因子本质上也是 BetweenFactor，只不过它连接的不是相邻帧，而是两个相隔很远、但实际回到同一地点附近的关键帧：
LoopFactor 误差：
e_loop_ij = Log( (Z_ij_loop)^{-1} * ( X_i^{-1} * X_j ) )
Z_ij_loop：
回环检测得到的第 i 帧到第 j 帧的相对位姿测量。
X_i^{-1} * X_j：
优化器当前估计的第 i 帧到第 j 帧的相对位姿。
IMUFactor 用来约束两个关键时刻之间的旋转、速度和位置变化。IMU 预积分会把两个关键帧之间的高频 IMU 数据积分成旋转增量、速度增量和位置增量：
IMU 预积分测量：
ΔR_ij：
第 i 帧到第 j 帧的旋转增量。
Δv_ij：
第 i 帧到第 j 帧的速度增量。
Δp_ij：
第 i 帧到第 j 帧的位置增量。
IMU 残差可以理解成三部分：
IMUFactor 残差：
e_imu =
[
e_R
e_p
e_v
]
旋转误差：
e_R = Log( (ΔR_ij)^{-1} * ( R_i^{-1} * R_j ) )
位置误差：
e_p = R_i^{-1} * ( p_j – p_i – v_i * Δt – 0.5 * g * Δt^2 ) – Δp_ij
速度误差：
e_v = R_i^{-1} * ( v_j – v_i – g * Δt ) – Δv_ij
其中 R_i、R_j 是第 i、j 时刻的旋转，p_i、p_j 是位置，v_i、v_j 是速度，g 是重力加速度，Δt 是时间间隔。IMU 的作用是提供短时间、高频、连续的运动约束，帮助点云去畸变、姿态预测和短时间外推。
BiasFactor 约束 IMU 零偏不要乱跳：
BiasFactor 误差：
e_bias = b_j – b_i
b_i：
第 i 时刻的 IMU bias。
b_j：
第 j 时刻的 IMU bias。
IMU bias 包括陀螺仪 bias 和加速度计 bias。它们会慢慢变化，但不应该在相邻关键帧之间突然剧烈跳变，所以 bias 因子保证 bias 估计更平滑。
这些因子可以理解成很多根橡皮筋：Prior 把第一帧固定住；LiDAR BetweenFactor 维持相邻关键帧之间的局部运动关系；GPS 把某些关键帧拉向全局位置；LoopFactor 把回到同一地点的两帧拉近；IMUFactor 约束短时间内的旋转、速度和位置变化；BiasFactor 约束 IMU 零偏平滑变化。如果所有传感器都完全准确，所有约束可以同时满足；但实际每个传感器都有噪声，所以优化器要根据权重折中。
求解时，因为这些误差函数是非线性的，优化器会在当前估计附近线性化：
线性化：
e( X ⊕ δ ) ≈ e( X ) + J * δ
e( X )：
当前状态下的误差。
J：
雅可比矩阵，表示状态变化一点点时误差怎么变。
δ：
要求解的状态增量。
⊕：
位姿流形上的增量更新，不是普通加法。
线性化后，优化器要求解：
min_δ || e + J * δ ||^2_Ω
展开：
min_δ ( e + J * δ )^T * Ω * ( e + J * δ )
对应的正规方程是：
( J^T * Ω * J ) * δ = – J^T * Ω * e
如果有很多因子，每个因子都会贡献自己的 J_k、Ω_k、e_k，最后累加成整体方程：
H = Σ_k J_k^T * Ω_k * J_k
g = Σ_k J_k^T * Ω_k * e_k
H * δ = -g
求出 δ 后，优化器更新状态：
X_new = X_old ⊕ δ
LIO-SAM 后端使用的是 GTSAM 的 iSAM2。iSAM2 不是每次从头优化整张图，而是新关键帧和新因子进来后做增量式更新，重点更新受影响的变量，所以适合实时 SLAM。
最后总结一下：LIO-SAM 中 X_i、X_j 是关键帧全局位姿变量，初值主要来自 LiDAR scan-to-map；Z_ij 是传感器因子保存的相对位姿测量，加入因子图后一般固定不变。刚构造 LiDAR BetweenFactor 时，因为 Z_ij 往往就是由 X_i 和 X_j_initial 计算出来的，所以这条因子的初始误差可能很小，甚至接近 0。但随着 GPS、回环、IMU、先验等其他因子共同作用，X_i、X_j 会被不断调整，此时 X_i^{-1} * X_j 不一定完全等于 Z_ij，而是尽量接近它。优化器最终做的事情，就是最小化所有因子的加权残差总和，让 LiDAR 局部运动约束、GPS 全局位置约束、回环闭合约束、IMU 连续运动约束、bias 平滑约束整体达到最合理的平衡，从而得到一条全局更一致、更稳定的轨迹。
解释框：
┌──────────────────────────────────────────────────────────────┐
│ 因子图里“测量”和“误差”的关系 │
│ │
│ X_i：第 i 个关键帧的位姿变量。 │
│ X_j：第 j 个关键帧的位姿变量。 │
│ Z_ij：传感器测得的 i 到 j 的相对位姿。 │
│ │
│ 当前估计产生的相对位姿： │
│ Z_ij_hat = X_i^{-1} * X_j │
│ │
│ 残差： │
│ e_ij = Log( Z_ij^{-1} * Z_ij_hat ) │
│ │
│ X_i^{-1}：位姿 X_i 的逆，表示从地图系变回第 i 帧坐标系。 │
│ X_i^{-1} * X_j：从第 i 帧到第 j 帧的相对位姿。 │
│ Z_ij^{-1}：测量相对位姿的逆。 │
│ Log()：把 SE(3) 位姿误差矩阵转换成 6 维误差向量。 │
└──────────────────────────────────────────────────────────────┘
所以“相对位姿”和“误差”不是矛盾。相对位姿是因子测量，误差是测量和当前估计之间的差。
5.1 大因子图有哪些因子？
mapOptimization.cpp 的大图优化关键帧位姿：
X0, X1, X2, …, Xn
每个 X_i 是一个 Pose3：
X_i = [R_i, t_i]
其中 R_i 是姿态，t_i 是位置。
大图因子如下：
┌──────────────────────────────────────────────────────────────┐
│ mapOptimization 大因子图 │
│ │
│ 优化变量： │
│ X0, X1, X2, …, Xn │
│ 每个 Xi 是一个关键帧 Pose3。 │
│ │
│ 因子： │
│ 1. PriorFactor<Pose3> │
│ 第一帧先验，固定坐标系。 │
│ │
│ 2. LiDAR BetweenFactor<Pose3> │
│ 相邻关键帧之间的 LiDAR 相对位姿约束。 │
│ │
│ 3. GPSFactor │
│ GPS 给出的绝对位置约束。 │
│ │
│ 4. Loop BetweenFactor<Pose3> │
│ 回环检测得到的历史帧和当前帧之间的相对位姿约束。 │
│ │
│ 优化目标： │
│ minimize { │
│ prior_error² │
│ + lidar_between_error² │
│ + gps_error² │
│ + loop_error² │
│ } │
└──────────────────────────────────────────────────────────────┘
PriorFactor<Pose3>：
e_prior = Log( X_prior^{-1} * X0 )
作用是固定第一帧，防止整张图没有坐标基准。
LiDAR BetweenFactor<Pose3>：
e_lidar = Log( Z_lidar^{-1} * (X_i^{-1} * X_j) )
作用是约束相邻关键帧之间的相对运动。
GPSFactor：
e_gps = translation(X_i) – gps_i
作用是提供绝对位置锚点。
Loop BetweenFactor<Pose3>：
e_loop = Log( Z_loop^{-1} * (X_old^{-1} * X_cur) )
作用是机器人回到历史位置时，把累计漂移拉回来。
6. imuPreintegration：IMU 预积分小因子图
imuPreintegration.cpp 是另一个核心模块。它维护的是高频小因子图。它和 mapOptimization.cpp 的大图不同：
┌──────────────────────────────────────────────────────────────┐
│ 大图 vs 小图 │
│ │
│ mapOptimization 大图： │
│ 低频，主要优化关键帧 pose。 │
│ 因子包括 LiDAR、GPS、Loop。 │
│ │
│ imuPreintegration 小图： │
│ 高频，优化 pose、velocity、bias。 │
│ 因子包括 IMU 预积分因子、bias 连续性因子、LiDAR 校正因子。 │
└──────────────────────────────────────────────────────────────┘
小图优化变量是：
X(k)：第 k 个校正时刻的 pose 位姿
V(k)：第 k 个校正时刻的 velocity 速度
B(k)：第 k 个校正时刻的 IMU bias 零偏
其中：
B(k) = [ba, bg]
ba 是加速度计 bias，bg 是陀螺仪 bias。
6.1 小图里面的因子逐个解释
你截图里提到的小图因子是：
PriorFactor<Pose3>
PriorFactor<Vector3> velocity
PriorFactor<imuBias> bias
ImuFactor
Bias BetweenFactor
LiDAR pose correction PriorFactor
完整解释如下：
┌──────────────────────────────────────────────────────────────┐
│ imuPreintegration 小因子图 │
│ │
│ 优化变量： │
│ X(k)：pose，位姿。 │
│ V(k)：velocity，速度。 │
│ B(k)：bias，IMU 零偏。 │
│ │
│ 1. PriorFactor<Pose3> │
│ 给初始位姿先验，约束 X(0)。 │
│ e_pose_prior = Log( X_prior^{-1} * X(0) ) │
│ │
│ 2. PriorFactor<Vector3> velocity │
│ 给初始速度先验，约束 V(0)。 │
│ e_vel_prior = V(0) – V_prior │
│ │
│ 3. PriorFactor<imuBias> bias │
│ 给初始 IMU bias 先验，约束 B(0)。 │
│ e_bias_prior = B(0) – B_prior │
│ │
│ 4. ImuFactor │
│ IMU 预积分因子，连接 X(k-1), V(k-1), B(k-1), X(k), V(k)。 │
│ 约束两次 LiDAR 校正之间的运动变化。 │
│ │
│ 5. Bias BetweenFactor │
│ 约束 B(k-1) 和 B(k) 不要突变。 │
│ e_bias_between = B(k) – B(k-1) │
│ │
│ 6. LiDAR pose correction PriorFactor │
│ 用 LiDAR 优化位姿 lidarPose 校正 X(k)。 │
│ e_lidar = Log( lidarPose^{-1} * X(k) ) │
└──────────────────────────────────────────────────────────────┘
6.2 ImuFactor 到底是什么？
两个 LiDAR 校正时刻之间，可能来了很多条 IMU 数据：
imu_1, imu_2, imu_3, …, imu_m
如果每条 IMU 都加进因子图，图会很大。IMU 预积分就是把这一段 IMU 数据压缩成一个测量：
ΔR：这段时间 IMU 积分出来的相对旋转
ΔV：这段时间 IMU 积分出来的速度变化
ΔP：这段时间 IMU 积分出来的位置变化
ImuFactor 连接：
X(k-1), V(k-1), B(k-1), X(k), V(k)
代码形态：
gtSAMgraph.add(gtsam::ImuFactor(
X(key – 1), V(key – 1),
X(key), V(key),
B(key – 1),
*imuPreintegrated));
解释：
┌──────────────────────────────────────────────────────────────┐
│ ImuFactor 解释 │
│ │
│ X(key-1)：上一校正时刻位姿。 │
│ V(key-1)：上一校正时刻速度。 │
│ B(key-1)：上一校正时刻 IMU bias。 │
│ X(key)：当前校正时刻位姿。 │
│ V(key)：当前校正时刻速度。 │
│ imuPreintegrated：两次校正之间 IMU 预积分结果，包含 ΔR/ΔV/ΔP。│
│ │
│ 约束含义： │
│ 从 X(k-1), V(k-1) 运动到 X(k), V(k) 的变化， │
│ 应该和 IMU 预积分得到的 ΔR, ΔV, ΔP 一致。 │
└──────────────────────────────────────────────────────────────┘
6.3 Bias BetweenFactor 到底是什么？
IMU bias 不应该突然跳变。bias 一般是缓慢变化的，所以加一个连续性约束：
gtSAMgraph.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(
B(key – 1),
B(key),
gtsam::imuBias::ConstantBias(),
biasNoise));
解释：
┌──────────────────────────────────────────────────────────────┐
│ Bias BetweenFactor 解释 │
│ │
│ B(key-1)：上一时刻 bias。 │
│ B(key)：当前时刻 bias。 │
│ gtsam::imuBias::ConstantBias()：测量的 bias 变化为 0。 │
│ biasNoise：允许 bias 变化的程度。 │
│ │
│ 约束含义： │
│ B(key) 应该接近 B(key-1)，bias 不应该突然跳变。 │
│ │
│ 误差： │
│ e_bias = B(key) – B(key-1) │
└──────────────────────────────────────────────────────────────┘
如果 biasNoise 设置很小，优化器会强烈认为 bias 不应该变；如果设置大一些，优化器允许 bias 有更大变化。
6.4 LiDAR pose correction PriorFactor 是什么？
mapOptimization.cpp 通过 scan-to-map 得到一个 LiDAR 优化位姿：
lidarPose
IMU 自己积分会漂，所以 imuPreintegration.cpp 会把这个 LiDAR 位姿作为校正因子加入小图：
gtSAMgraph.add(gtsam::PriorFactor<gtsam::Pose3>(
X(key),
lidarPose,
correctionNoise));
解释：
┌──────────────────────────────────────────────────────────────┐
│ LiDAR pose correction PriorFactor 解释 │
│ │
│ X(key)：IMU 小图里的当前位姿变量。 │
│ lidarPose：LiDAR scan-to-map 后端优化出来的位姿。 │
│ correctionNoise：LiDAR 位姿校正噪声，表示信任 LiDAR 的程度。 │
│ │
│ 约束含义： │
│ IMU 积分会漂，LiDAR 几何匹配更稳定， │
│ 所以让 X(key) 接近 lidarPose。 │
│ │
│ 误差： │
│ e_lidar = Log( lidarPose^{-1} * X(key) ) │
└──────────────────────────────────────────────────────────────┘
IMU 小图优化后得到：
prevPose = result.at<Pose3>(X(key));
prevVel = result.at<Vector3>(V(key));
prevBias = result.at<imuBias::ConstantBias>(B(key));
然后用新 bias 重置预积分器：
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias);
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias);
形成闭环：
IMU 高频积分 -> 给 LiDAR 去畸变和初值
LiDAR scan-to-map -> 得到较稳定位姿
LiDAR 位姿进入 IMU 小图 -> 校正 pose、velocity、bias
新 bias -> 下一段 IMU 积分更准
7. GPS 和回环：怎么进入后端因子图？
在 LIO-SAM 里，LiDAR 和 IMU 主要解决的是局部连续定位问题。也就是说，它能比较好地估计机器人从上一帧到当前帧移动了多少、转了多少。但是只靠 LiDAR-IMU 相对运动，时间长了还是会累计漂移。比如机器人绕一个大圈回到起点，纯里程计可能认为自己没有完全回到原来的地方，而是偏了几十厘米甚至几米。
所以后端需要两类“全局校正”信息：
┌──────────────────────────────────────────────────────────────┐
│ 后端全局校正信息 │
│ │
│ 1. GPS 因子 │
│ 提供绝对位置约束，告诉系统“当前关键帧大概在全局哪里”。 │
│ │
│ 2. 回环因子 Loop Factor │
│ 当机器人回到以前走过的地方时，告诉系统“当前帧和历史帧其实 │
│ 是同一个地方附近”，从而把累计漂移拉回来。 │
└──────────────────────────────────────────────────────────────┘
GPS 和回环最终都会进入 mapOptimization.cpp 里的大因子图。大因子图优化的变量是关键帧位姿：
X0, X1, X2, …, Xn
每个 Xi 是一个关键帧的 6 自由度位姿：
Xi = Pose3(Ri, ti)
其中：
Ri：第 i 个关键帧的旋转姿态
ti：第 i 个关键帧的位置
7.1 GPS 因子：GPS 怎么进入因子图？
GPS 提供的是绝对位置，一般可以表示成：
gps_i = [gps_x, gps_y, gps_z]
这里：
gps_x：GPS 测得的 x 方向位置
gps_y：GPS 测得的 y 方向位置
gps_z：GPS 测得的 z 方向位置
GPS 不直接告诉系统机器人朝向是多少，所以 GPS 因子一般只约束位置，不约束 roll / pitch / yaw。
在因子图中，当前关键帧位姿是：
X_i
从 X_i 里面可以取出平移部分：
translation(X_i)
也就是：
translation(X_i) = [x_i, y_i, z_i]
GPS 因子的残差是：
e_gps = translation(X_i) – gps_i
放在一个框里：
┌──────────────────────────────────────────────────────────────┐
│ GPSFactor 公式解释 │
│ │
│ gps_i = [gps_x, gps_y, gps_z] │
│ translation(X_i) = [x_i, y_i, z_i] │
│ │
│ GPS 残差： │
│ e_gps = translation(X_i) – gps_i │
│ │
│ X_i：第 i 个关键帧的位姿变量 │
│ translation(X_i)：从 X_i 中取出的位移部分 │
│ gps_i：GPS 测得的绝对位置 │
│ e_gps：当前优化位姿位置和 GPS 位置之间的误差 │
└──────────────────────────────────────────────────────────────┘
代码形式一般类似
gtSAMgraph.add(gtsam::GPSFactor(
key,
gtsam::Point3(gps_x, gps_y, gps_z),
gpsNoise));
解释：
key
当前关键帧编号。
gtsam::Point3(gps_x, gps_y, gps_z)
GPS 测得的位置。
gpsNoise
GPS 噪声模型，也就是告诉优化器“这个 GPS 可信程度有多高”。GPS 协方差小，说明 GPS 可信；GPS 协方差大，说明 GPS 不稳定，优化器就不应该太相信它。
GPS 通常不会每帧都加，会先判断：
┌──────────────────────────────────────────────────────────────┐
│ GPSFactor 添加前的常见判断 │
│ │
│ 1. GPS 队列里是否有数据。 │
│ 2. GPS 时间戳是否和当前 LiDAR 关键帧时间接近。 │
│ 3. GPS 协方差是否小于阈值，太差的 GPS 不加。 │
│ 4. 当前距离上一次加入 GPS 因子是否移动了足够距离。 │
│ 5. 当前位姿不确定性是否需要 GPS 修正。 │
└──────────────────────────────────────────────────────────────┘
所以 GPS 的作用可以总结成一句话：GPS 给关键帧一个绝对位置锚点，用来压制 LiDAR-IMU 长时间运行后的全局漂移。
7.2 回环检测为什么需要？
回环检测解决的问题是：机器人走了一圈，回到了以前走过的地方，但是纯 LiDAR-IMU 里程计因为累计误差，轨迹没有闭合。
比如机器人真实路线是一个正方形：
起点 -> 向前 -> 右转 -> 再右转 -> 再右转 -> 回到起点
真实情况下最后应该回到起点附近。但是由于每一步 scan-to-map 都有一点点误差，累计起来后，系统可能认为最后位置离起点还有 0.5m、1m，甚至更多。这时候就需要回环。
回环的核心逻辑是：
┌──────────────────────────────────────────────────────────────┐
│ 回环的核心含义 │
│ │
│ 如果当前关键帧和某个很久以前的历史关键帧表示同一个地方， │
│ 那么它们之间应该存在一个可靠的相对位姿约束。 │
│ │
│ 把这个约束加入因子图后，优化器会重新调整整条轨迹， │
│ 把累计漂移分摊到历史轨迹中，使地图闭合。 │
└──────────────────────────────────────────────────────────────┘
7.3 回环检测是不是一直运行？
回环通常是一个后台线程周期性运行，不是每来一帧都做完整 ICP。原因是 ICP 点云配准比较耗时，如果每帧都做，会影响实时性。
逻辑可以理解成：
void loopClosureThread()
{
while (ros::ok())
{
performLoopClosure(); // 执行回环检测和 ICP 验证
visualizeLoopClosure(); // 可视化回环边
rate.sleep(); // 按固定频率运行，比如 1Hz
}
}
这里：
performLoopClosure()
是真正执行回环检测的函数。
visualizeLoopClosure()
用于在 RViz 或调试中显示回环约束边。
rate.sleep()
控制回环线程频率，避免一直占用 CPU。
完整流程可以这样看：
┌──────────────────────────────────────────────────────────────┐
│ 回环检测完整流程 │
│ │
│ 1. 取当前最新关键帧 latestID。 │
│ │
│ 2. 在历史关键帧位置 cloudKeyPoses3D 里做 KD-tree 半径搜索。 │
│ │
│ 3. 找到空间距离近的历史关键帧候选。 │
│ │
│ 4. 检查时间间隔，过滤掉刚刚经过的近邻帧。 │
│ │
│ 5. 得到一个历史回环候选帧 oldID。 │
│ │
│ 6. 构建当前局部点云 C_cur。 │
│ │
│ 7. 构建历史局部点云 C_old。 │
│ │
│ 8. 用 ICP 把 C_cur 对齐到 C_old。 │
│ │
│ 9. 检查 ICP 是否收敛、fitness score 是否合格。 │
│ │
│ 10. 如果合格，计算回环相对位姿 Z_loop。 │
│ │
│ 11. 把 oldID、curID、Z_loop、loopNoise 放入回环队列。 │
│ │
│ 12. 在后端因子图中添加 Loop BetweenFactor。 │
│ │
│ 13. iSAM2 重新优化关键帧轨迹，更新历史位姿和地图。 │
└──────────────────────────────────────────────────────────────┘
7.4 第一步：怎么找回环候选帧？
LIO-SAM 会保存所有历史关键帧的位置：
cloudKeyPoses3D
里面每个点就是一个关键帧的位置：
x, y, z
当前最新关键帧编号是：
latestID = cloudKeyPoses3D->size() – 1
当前关键帧位置是：
currentPoint = cloudKeyPoses3D->points[latestID]
然后用 KD-tree 在历史关键帧位置里做半径搜索：
kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
kdtreeHistoryKeyPoses->radiusSearch(
currentPoint,
historyKeyframeSearchRadius,
pointSearchIndLoop,
pointSearchSqDisLoop);
变量解释：
┌──────────────────────────────────────────────────────────────┐
│ 回环候选搜索变量解释 │
│ │
│ cloudKeyPoses3D： │
│ 历史关键帧位置点云，只包含 x/y/z。 │
│ │
│ latestID： │
│ 当前最新关键帧编号。 │
│ │
│ currentPoint： │
│ 当前最新关键帧的位置。 │
│ │
│ kdtreeHistoryKeyPoses： │
│ 历史关键帧位置构成的 KD-tree，用于快速半径搜索。 │
│ │
│ historyKeyframeSearchRadius： │
│ 回环候选搜索半径，比如 10m、15m、20m。 │
│ │
│ pointSearchIndLoop： │
│ 半径搜索找到的历史关键帧编号集合。 │
│ │
│ pointSearchSqDisLoop： │
│ 搜到的历史关键帧与当前帧之间的距离平方。 │
└──────────────────────────────────────────────────────────────┘
但是光空间距离近还不够，因为当前帧和刚刚过去的前几帧肯定也很近，但那不是回环。所以还要加时间间隔判断：
double timeDiff = fabs(cloudKeyPoses6D->points[id].time
– cloudKeyPoses6D->points[latestID].time);
if (timeDiff > historyKeyframeSearchTimeDiff)
{
closestID = id;
return true;
}
这里：
historyKeyframeSearchTimeDiff
表示最小时间间隔。只有当前帧和历史帧时间相隔足够久，才认为它可能是回环。
所以候选帧必须同时满足：
┌──────────────────────────────────────────────────────────────┐
│ 回环候选帧判断条件 │
│ │
│ 1. 空间距离近： │
│ 当前关键帧位置和历史关键帧位置小于搜索半径。 │
│ │
│ 2. 时间间隔大： │
│ 当前帧和历史帧不是刚刚连续经过的相邻帧。 │
│ │
│ 3. 还没有被加入过重复回环： │
│ 避免同一对关键帧反复添加回环因子。 │
└──────────────────────────────────────────────────────────────┘
举个例子：
当前关键帧 latestID = 500
当前位置 = [0.3, 0.2, 0]
历史关键帧 id = 10
历史位置 = [0.0, 0.0, 0]
两者距离很近，而且时间间隔很大，就可能是回环候选。
7.5 第二步：怎么构建当前局部点云和历史局部点云？
找到候选帧后，不能直接加回环因子，因为“位置近”不代表一定是同一个地方。比如两个楼层上下重合、两条相似走廊、定位漂移导致的假近邻，都可能误判。
所以要构建两团局部点云，用 ICP 做几何验证。
当前局部点云：
C_cur
历史局部点云：
C_old
它们通常不是单独一帧点云，而是某个关键帧附近多帧拼起来的局部子地图。这样点云更完整，ICP 更稳定。
构建逻辑可以理解成：
┌──────────────────────────────────────────────────────────────┐
│ 当前局部点云 C_cur │
│ │
│ 以当前关键帧 curID 为中心，取它附近若干关键帧， │
│ 把这些关键帧点云按照各自关键帧位姿变换到地图系， │
│ 然后拼接成当前局部点云。 │
│ │
│ 历史局部点云 C_old │
│ │
│ 以历史候选关键帧 oldID 为中心，取它附近若干关键帧， │
│ 同样变换到地图系并拼接成历史局部点云。 │
└──────────────────────────────────────────────────────────────┘
关键帧点云原本保存在：
cornerCloudKeyFrames[id]
surfCloudKeyFrames[id]
关键帧位姿保存在：
cloudKeyPoses6D->points[id]
把某个关键帧点云变换到地图系的公式是：
p_map = R_key * p_local + t_key
其中：
p_local：关键帧局部坐标系下的点
R_key：该关键帧的旋转
t_key：该关键帧的位置
p_map：变换到地图系后的点
也就是说，当前和历史两团局部点云都已经是在同一个地图坐标系下表达的点云。这样 ICP 才能直接判断它们是否能对齐。
7.6 第三步：ICP 是怎么验证回环的？
ICP 全称是 Iterative Closest Point，迭代最近点算法。它的目标是求一个刚体变换：
T_icp
让当前局部点云 C_cur 尽量对齐历史局部点云 C_old。
公式是：
T_icp = argmin Σ || T * p_cur – p_old ||²
解释框：
┌──────────────────────────────────────────────────────────────┐
│ ICP 优化目标 │
│ │
│ T_icp = argmin Σ || T * p_cur – p_old ||² │
│ │
│ p_cur：当前局部点云 C_cur 中的点。 │
│ p_old：历史局部点云 C_old 中的对应点。 │
│ T：待求的刚体变换，包括旋转 R 和平移 t。 │
│ T_icp：ICP 求出来的最优变换。 │
│ │
│ 含义： │
│ 找一个 T，让当前局部点云变换后尽量贴合历史局部点云。 │
└──────────────────────────────────────────────────────────────┘
ICP 迭代过程可以理解成：
┌──────────────────────────────────────────────────────────────┐
│ ICP 迭代过程 │
│ │
│ 1. 对 C_cur 中每个点，在 C_old 中找最近点。 │
│ 2. 得到一批点对：p_cur <-> p_old。 │
│ 3. 根据这些点对求一个刚体变换 T。 │
│ 4. 用 T 更新 C_cur 的位置。 │
│ 5. 重复 1~4，直到变换变化很小或误差不再下降。 │
└──────────────────────────────────────────────────────────────┘
代码：
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius * 2);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);
if (icp.hasConverged() == false)
return;
if (icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
Eigen::Matrix4f correctionLidarFrame = icp.getFinalTransformation();
逐个解释：
┌──────────────────────────────────────────────────────────────┐
│ ICP 代码解释 │
│ │
│ pcl::IterativeClosestPoint<PointType, PointType> icp： │
│ 创建 ICP 配准对象。 │
│ │
│ setMaxCorrespondenceDistance： │
│ 设置最大对应点距离。超过这个距离的最近点不算有效匹配。 │
│ │
│ setMaximumIterations(100)： │
│ 最多迭代 100 次。 │
│ │
│ setTransformationEpsilon(1e-6)： │
│ 两次迭代之间位姿变换变化很小时，认为收敛。 │
│ │
│ setEuclideanFitnessEpsilon(1e-6)： │
│ 匹配误差变化很小时，认为收敛。 │
│ │
│ setRANSACIterations(0)： │
│ 不额外使用 RANSAC 剔除外点。 │
│ │
│ setInputSource(cureKeyframeCloud)： │
│ 设置 source，即当前局部点云 C_cur，它会被移动。 │
│ │
│ setInputTarget(prevKeyframeCloud)： │
│ 设置 target，即历史局部点云 C_old，作为参考。 │
│ │
│ icp.align(*unused_result)： │
│ 执行 ICP 配准。unused_result 是配准后的 source 点云。 │
│ │
│ icp.hasConverged()： │
│ 判断 ICP 是否收敛。没收敛就说明回环不可靠。 │
│ │
│ icp.getFitnessScore()： │
│ ICP 匹配分数，越小越好。超过阈值就拒绝回环。 │
│ │
│ icp.getFinalTransformation()： │
│ 返回最终配准变换 correctionLidarFrame。 │
└──────────────────────────────────────────────────────────────┘
这里的：
correctionLidarFrame
就是 ICP 求出来的校正变换。它表示：
把当前局部点云对齐到历史局部点云，需要施加的旋转和平移。
7.7 第四步：回环相对位姿 Z_loop 是怎么得到的？
这一步很关键。回环因子不是直接把 correctionLidarFrame 塞进去，而是要把它变成两个关键帧之间的相对位姿约束。
先定义：
X_old：历史关键帧位姿
X_cur：当前关键帧位姿
其中：
X_old = 历史关键帧 oldID 在地图系下的位姿
X_cur = 当前关键帧 curID 在地图系下的位姿
如果没有回环，当前估计的相对位姿是：
X_old^{-1} * X_cur
解释：
X_old：old frame -> map
X_old^{-1}：map -> old frame
X_cur：cur frame -> map
所以：
X_old^{-1} * X_cur
表示：
cur frame 相对于 old frame 的位姿
也就是“从历史帧 old 看，当前帧 cur 在哪里”。
ICP 得到的是一个校正变换：
T_icp
它表示当前局部点云如何对齐到历史局部点云。通过这个校正变换，可以得到一个更可信的当前帧校正位姿：
X_cur_corrected = T_icp * X_cur
然后回环测量可以理解为：
Z_loop = X_old^{-1} * X_cur_corrected
也就是：
ICP 认为 old 到 cur 的相对位姿应该是 Z_loop。
放在一个框里：
┌──────────────────────────────────────────────────────────────┐
│ 回环相对位姿 Z_loop 的来源 │
│ │
│ X_old：历史关键帧位姿。 │
│ X_cur：当前关键帧原始估计位姿。 │
│ T_icp：ICP 得到的当前局部点云到历史局部点云的校正变换。 │
│ │
│ 当前帧校正位姿： │
│ X_cur_corrected = T_icp * X_cur │
│ │
│ 回环相对位姿测量： │
│ Z_loop = X_old^{-1} * X_cur_corrected │
│ │
│ 含义： │
│ 回环检测认为，当前帧相对于历史帧的相对位姿应该是 Z_loop。 │
└──────────────────────────────────────────────────────────────┘
注意：不同代码实现里，因子添加顺序可能是：
BetweenFactor(oldID, curID, Z_loop)
也可能是：
BetweenFactor(curID, oldID, Z_loop_inverse)
顺序不同，测量值可能取逆，但本质完全一样：给历史帧和当前帧之间加一条相对位姿约束。
7.8 第五步：回环因子怎么加入因子图？
回环验证通过后，通常不会立刻直接改所有位姿，而是把回环信息放入队列：
loopIndexQueue.push_back(make_pair(oldID, curID));
loopPoseQueue.push_back(Z_loop);
loopNoiseQueue.push_back(loopNoise);
解释：
loopIndexQueue
保存哪两个关键帧之间有回环，比如 (oldID, curID)。
loopPoseQueue
保存回环相对位姿测量 Z_loop。
loopNoiseQueue
保存回环噪声模型，表示这个回环约束的可信程度。
随后在后端图优化中添加：
gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(
oldID,
curID,
Z_loop,
loopNoise));
含义是：
X_old^{-1} * X_cur 应该接近 Z_loop
也就是：
当前优化轨迹中 old 到 cur 的相对位姿，应该和 ICP 测出来的回环相对位姿一致。
7.9 回环误差公式完整解释
回环误差写成：
e_loop = Log( Z_loop^{-1} * (X_old^{-1} * X_cur) )
这个公式你之前问过，这里完整解释。
┌──────────────────────────────────────────────────────────────┐
│ 回环误差公式完整解释 │
│ │
│ e_loop = Log( Z_loop^{-1} * (X_old^{-1} * X_cur) ) │
│ │
│ X_old： │
│ 历史关键帧位姿，表示 old frame -> map。 │
│ │
│ X_old^{-1}： │
│ 历史关键帧位姿的逆，表示 map -> old frame。 │
│ │
│ X_cur： │
│ 当前关键帧位姿，表示 cur frame -> map。 │
│ │
│ X_old^{-1} * X_cur： │
│ 当前优化变量推导出的 old -> cur 相对位姿。 │
│ │
│ Z_loop： │
│ ICP 测出来的 old -> cur 回环相对位姿。 │
│ │
│ Z_loop^{-1}： │
│ 回环测量相对位姿的逆。 │
│ │
│ Z_loop^{-1} * (X_old^{-1} * X_cur)： │
│ 当前估计的相对位姿 和 ICP 测量相对位姿 之间的差。 │
│ │
│ Log(…)： │
│ 李群 SE(3) 上的对数映射，把 4×4 位姿差转换成 6 维误差向量。 │
│ │
│ e_loop： │
│ 回环残差。越接近 0，说明当前轨迹越满足回环约束。 │
└──────────────────────────────────────────────────────────────┘
这里的 Log() 不是普通数学里的标量对数，而是 SE(3) 李群上的对数映射。因为位姿误差本身是一个刚体变换矩阵，优化器需要一个向量形式的误差，所以要用 Log() 把它变成 6 维向量：
[旋转误差 3 维, 平移误差 3 维]
7.10 回环加入后，地图为什么会被拉回来？
假设没有回环时，系统估计：
X_old = [0, 0, 0]
X_cur = [1.0, 0.2, 0]
也就是说，系统认为当前帧比历史帧偏了 1 米。但是 ICP 发现当前局部点云和历史局部点云其实高度重合，测得：
Z_loop ≈ [0.1, 0.0, 0]
说明当前帧应该非常接近历史帧。
这时因子图里会同时存在两类约束：
┌──────────────────────────────────────────────────────────────┐
│ 回环前已有约束 │
│ │
│ X0 -> X1 -> X2 -> … -> X_cur │
│ 每两个相邻关键帧之间有 LiDAR odometry 相对位姿约束。 │
│ │
│ 新增回环约束 │
│ │
│ X_old <————————–> X_cur │
│ ICP 告诉系统：old 和 cur 实际上应该接近。 │
└──────────────────────────────────────────────────────────────┘
优化器不会简单粗暴地只移动当前帧，而是会在整条历史轨迹上分摊误差，让所有约束尽可能同时满足。结果就是：
当前帧被拉近历史帧；
中间轨迹也被平滑调整；
地图闭合效果变好；
累计漂移被压缩。
这就是回环因子的作用。
8. 最终总结
按照模块来看，LIO-SAM 第一层是 imageProjection.cpp。这一层处理的是原始传感器数据，主要目标是把原始 LiDAR 点云变成一帧“时间统一、结构清晰”的点云。由于机械式 LiDAR 一帧扫描不是瞬时完成的，每个点都有自己的采集时间，所以系统要根据 point.time 得到 pointTime。然后从 IMU 队列中找到对应时刻的旋转变化，从 odom 队列中估计对应时刻的平移变化，构造当前点时刻的变换 T_point。再用 p_start = T_start^{-1} T_point p_point 把这个点从采集时刻坐标系补偿到本帧起始坐标系。完成 deskew 后，系统根据 ring 和水平角把点云投影到 range image 里，为后续曲率计算和特征提取做准备。
第二层是 featureExtraction.cpp。这一层的核心是从去畸变后的点云中提取稳定的几何特征。系统用同一根激光线前后各 5 个邻居的 range 计算曲率。曲率大，说明这个点相对周围变化明显，可能是墙角、柱子边缘、门框边缘等角点；曲率小，说明这个点附近比较平滑，可能来自地面、墙面、桌面等平面区域。为了避免把遮挡边界误选成角点，系统会用 columnDiff 和相邻深度差判断遮挡，并用 cloudNeighborPicked 标记不稳定点。为了避免特征集中在局部区域，每根激光线会分成 6 段，每段分别选角点和平面点，使特征空间分布更均匀。
第三层是 mapOptimization.cpp 的 scan-to-map 匹配。系统不会直接拿当前帧和上一帧做简单配准，而是根据当前估计位姿，在历史关键帧中搜索附近关键帧，把这些关键帧的角点和平面点变换到地图坐标系，构建局部角点地图和局部平面地图。当前帧角点变到地图系后，在局部角点地图里找 5 个近邻，通过 PCA 判断它们是否形成线结构，如果是，就建立点到线残差。当前帧平面点变到地图系后，在局部平面地图里找 5 个近邻，拟合平面方程，建立点到平面残差。优化器调整当前帧 6 自由度位姿，让所有点到线、点到面的残差平方和最小，从而得到当前帧 LiDAR odometry。
第四层是 mapOptimization.cpp 的大因子图优化。当前帧如果满足关键帧条件，就会保存为关键帧。第一帧通过 PriorFactor<Pose3> 固定坐标系，相邻关键帧之间通过 LiDAR BetweenFactor<Pose3> 建立相对位姿约束，GPS 可用时通过 GPSFactor 添加绝对位置约束，回环成功时通过 Loop BetweenFactor<Pose3> 添加历史帧和当前帧之间的相对位姿约束。因子图里保存的是测量，比如 LiDAR 相对位姿 Z_ij 或回环相对位姿 Z_loop；优化目标里计算的是误差，比如 Log(Z_ij^{-1}(X_i^{-1}X_j))。所以“相对位姿”和“误差”不是矛盾，前者是传感器或 ICP 给出的测量，后者是当前优化变量与测量之间的差。
第五层是 imuPreintegration.cpp 的小因子图。这个图是为了高频 IMU odometry 和 IMU bias 估计服务的。它优化的变量不只是位姿 X(k)，还包括速度 V(k) 和 IMU 零偏 B(k)。ImuFactor 把两次 LiDAR 校正之间的大量 IMU 数据压缩成预积分约束，约束前后两个时刻的 pose 和 velocity；Bias BetweenFactor 约束 bias 不要突然跳变；LiDAR pose correction PriorFactor 用 LiDAR scan-to-map 优化出的位姿来校正 IMU 小图中的 pose。优化后得到新的 pose、velocity 和 bias，再用新的 bias 重置预积分器，使下一段 IMU 积分更准确。
第六层是 GPS 和回环。GPS 给的是绝对位置，主要用于室外压长期漂移；它进入因子图后只约束关键帧位置，不直接约束姿态。回环检测则用于机器人回到历史位置时消除累计漂移。回环线程通常后台周期运行，先用关键帧位置做半径搜索，找空间距离近、时间间隔大的历史候选帧；然后构建当前局部点云 C_cur 和历史局部点云 C_old，用 ICP 验证它们是否能对齐。ICP 收敛且 fitness score 合格后，系统根据 ICP 校正变换得到回环相对位姿 Z_loop，再把它作为 Loop BetweenFactor 加进大因子图。回环误差 e_loop = Log(Z_loop^{-1}(X_old^{-1}X_cur)) 的含义是：当前轨迹估计出来的 old 到 cur 的相对位姿，应该和 ICP 测出来的相对位姿一致。如果不一致，优化器就会调整历史关键帧轨迹，把累计漂移分摊掉，使地图闭合。
在 LIO-SAM 中，回环检测线程按照 loopClosureFrequency 周期在后台运行，默认频率为 1 Hz；每次检测时，程序先将最新关键帧设为当前待检测帧 loopKeyCur，再以该关键帧的三维位置为查询点，在保存全部历史关键帧位置的 kdtreeHistoryKeyPoses 中调用 radiusSearch() 做半径搜索，默认搜索半径 historyKeyframeSearchRadius 为 15 m，并且 radiusSearch() 的最大返回数量参数设置为 0，因此会返回半径内的全部空间邻近关键帧，而不是固定选择几个候选帧。程序随后按搜索结果的返回顺序逐个检查这些关键帧的时间戳，只有与当前帧时间间隔大于 historyKeyframeSearchTimeDiff，默认 30 s 的关键帧，才可能作为回环历史帧；一旦找到第一个满足时间条件的历史关键帧 loopKeyPre，程序就停止继续查找，因此一次回环检测最终只选择一对“当前帧—历史帧”进行 ICP 验证，并不会对半径内所有候选帧逐个执行 ICP。确定候选帧后，程序调用 loopFindNearKeyframes() 构建两组用于匹配的点云：当前点云 C_cur 只提取当前关键帧本身的角点云和平面点云，因为传入的 searchNum 为 0；历史点云 C_old 则以候选历史帧为中心，向前和向后各提取 historyKeyframeSearchNum 个关键帧，默认值为 25，因此在索引范围充足时最多会融合 51 个历史关键帧的角点云和平面点云。提取出的各帧点云会按照各自保存的关键帧位姿统一变换到同一个公共参考坐标系中，再进行体素降采样；如果当前点云少于 300 个点，或者历史局部点云少于 1000 个点，程序会直接放弃本次回环检测。之后 ICP 将 C_cur 设置为源点云，将 C_old 设置为目标点云，利用当前关键帧位姿提供的初始空间接近关系进行配准，默认最大对应点距离为搜索半径的 2 倍，即 30 m，最大迭代次数为 100 次；只有 ICP 成功收敛（迭代优化变换矩阵），并且 fitness score 小于 historyKeyframeFitnessScore，默认 0.3，才认为该回环候选有效。ICP 输出的最终校正变换会左乘到当前关键帧原本存在漂移的全局位姿上，得到校正后的当前关键帧位姿，再与历史候选帧位姿计算相对变换 poseFrom.between(poseTo)（用纯矩阵算出），该相对变换就是回环测量值 Z_loop；系统同时使用 ICP 的 fitness score 构造回环因子的噪声，最后将当前关键帧索引、历史关键帧索引、Z_loop 和噪声模型加入回环约束队列，随后作为 Loop BetweenFactor 加入因子图，使优化器能够调整整条历史轨迹并消除累计漂移。
最终可以用一句话概括：LIO-SAM 的前端用 IMU/Odom 对 LiDAR 点云去畸变，再从点云中提取角点和平面点；中间的 scan-to-map 用这些特征和局部地图匹配，估计当前帧位姿；后端的大因子图把 LiDAR、GPS、回环约束统一优化，保证长期轨迹一致；IMU 小因子图负责高频预测和 bias 校正；GPS 与回环负责把长期漂移拉回正确位置。
版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！
欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。
