先明确一个前提：原始 LOAM 的点云畸变来自“同一帧中的点并非同一时刻采到”。LiDAR 在扫描这一圈期间持续运动，早采到的点和晚采到的点对应的传感器姿态不同，因此墙面、地面或柱子在原始点云中可能被拉弯。LOAM 的前端一边估计帧间运动，一边利用该运动近似补偿帧内点的不同采样时刻。
但要注意：官方 A-LOAM 的 laserOdometry.cpp 默认配置是：
#define DISTORTION 0
此时不会真正按点时间做逐点去畸变；所有点都会直接使用完整的帧间位姿。下面先按“开启 DISTORTION=1 后的完整理论流程”解释，再说明默认代码实际做了什么。
1. 当前帧、上一帧与待优化位姿
设：
F0：第一帧点云，也是当前保存的上一帧参考点云
F1：第二帧点云，也是当前需要处理的点云
对于第二帧，A-LOAM 需要求的是：
T₀←₁ = { R₀←₁ , t₀←₁ }
含义是：
把第二帧 F1 中的点，变换到第一帧 F0 的参考坐标系。
代码变量对应为：
q_last_curr：当前帧 → 上一帧 的旋转四元数
t_last_curr：当前帧 → 上一帧 的平移向量
完整变换为：
P_last = R_last←curr · P_curr + t_last←curr
写成齐次变换矩阵：
┌ ┐
T_last←curr = │ R_last←curr t_last←curr │
│ 0 1 │
└ ┘
这里的 q_last_curr、t_last_curr 是整帧的 6DoF 相对运动，不是每个点单独估计一份位姿。它表达的是：从当前扫描参考时刻到上一帧参考时刻，LiDAR 总共发生了多少旋转和平移。A-LOAM 后续才根据每个点自己的采样时间，把这份整帧运动拆成该点对应的部分运动。
2. 第一帧 F0：只保存参考特征，不优化位姿
第一帧到来时，系统没有更早的点云，因此不存在“上一帧特征”，也无法建立点到线、点到面约束。
所以第一帧不会进入 Ceres 优化，而是完成初始化：
当前帧 less sharp 角点
↓
保存到 laserCloudCornerLast
当前帧 less flat 面点
↓
保存到 laserCloudSurfLast
laserCloudCornerLast
↓
建立角点 KD-Tree
laserCloudSurfLast
↓
建立面点 KD-Tree
程序启动时，相对位姿初值是：
para_q = {0, 0, 0, 1}; // 单位四元数
para_t = {0, 0, 0}; // 零平移
等价于：
q_last_curr = I
t_last_curr = 0
也就是：
初始假设：LiDAR 没有旋转、没有平移。
第一帧结束后，系统中已经有了第一帧的角点库、面点库和 KD-Tree。第二帧到来后，就可以把自己的特征点拿去第一帧中找对应结构。
3. 第二帧 F1：一开始其实并没有真正“变换到第一帧”
这是最容易混淆的地方。
第二帧刚进入时，系统还不知道真实的：
T₀←₁ = {R₀←₁, t₀←₁}
因此，它第一次不能使用正确位姿把 F1 精确变到 F0。
它只能先使用初始猜测：
q₀←₁⁽⁰⁾ = I
t₀←₁⁽⁰⁾ = 0
即：
先假设第二帧和第一帧没有相对运动。
此时，对于第二帧任意原始点：
P₁
第一次临时变换结果是：
P₀_guess = I · P₁ + 0 = P₁
所以第二帧第一次寻找对应关系时，本质上是：
不做真实坐标变换
↓
直接拿第二帧点的位置
↓
去第一帧 KD-Tree 找最近的角点或面点
↓
得到一批粗略对应关系
这并不代表系统认为机器人真的没动，而是把“零运动”仅作为第一次配准的初值。
只要 LiDAR 频率较高、机器人单帧移动不大、前后两帧仍有足够重叠区域，系统通常可以从这个粗初值中找到足以启动优化的对应关系。若机器人移动过快、转弯过猛、环境中重复结构过多或特征太少，第二帧第一次匹配就可能不稳定。
4. 开启去畸变后：每个点如何获得自己的“部分位姿”
假设 LiDAR 扫描周期为：
SCAN_PERIOD = 0.1 s
一帧中的点不是同时采集的。
s = 0：扫描刚开始采到的点
s = 0.5：扫描进行到一半采到的点
s = 1：扫描结束附近采到的点
A-LOAM 会从 intensity 的小数部分恢复点的相对时间比例：
s = (intensity – floor(intensity)) / SCAN_PERIOD
在 A-LOAM 自己的特征提取流程中，intensity 不再只表示反射强度；整数部分通常保存线束编号，小数部分被复用于保存扫描内相对时间。
对于当前帧第 iii 个点，其时间比例为 sis_isi。A-LOAM 使用整帧相对运动进行插值：
q_i = Slerp(I, q_last_curr, s_i)
t_i = s_i · t_last_curr
其中：
I：单位旋转，即不旋转
Slerp：四元数球面线性插值
q_i：该点在自身采样时刻对应的部分旋转
t_i：该点在自身采样时刻对应的部分平移
于是，第二帧中第 iii 个点会临时变换为：
P_i,last =
Slerp(I, q_last_curr, s_i) · P_i,curr
+
s_i · t_last_curr
举例：假设这一帧扫描期间，LiDAR 总共前进了 10 cm、左转了 2∘2^\\circ2∘。
扫描开始点，s = 0：
补偿 0 cm，旋转 0°
扫描中间点，s = 0.5：
补偿 5 cm，旋转 1°
扫描结束点，s = 1：
补偿 10 cm，旋转 2°
因此，去畸变不是“每个点都使用完整 10 cm、2∘2^\\circ2∘”，而是每个点按自身采样时间使用不同大小的部分运动。
5. TransformToStart() 到底做了什么
TransformToStart() 的作用不是提前把整个第二帧永久转换成第一帧点云，而是：
对当前正在处理的一个点，
使用“当前估计的 q_last_curr、t_last_curr”，
临时计算它在上一帧参考坐标中的位置，
用于 KD-Tree 查找对应结构。
等价伪代码如下：
double s = point_time_ratio;
Quaternion q_part =
Slerp(Identity, q_last_curr, s);
Vector3d t_part =
s * t_last_curr;
point_for_search =
q_part * point_raw + t_part;
这里的 point_for_search 对应代码中的 pointSel。
它只用于：
当前点大概应该去上一帧的什么位置找线、找平面。
它不是真正最终输出的位姿结果，也不是一开始就准确的去畸变点坐标。
第二帧第一轮中：
q_last_curr = I
t_last_curr = 0
所以即使 DISTORTION=1：
q_part = Slerp(I, I, s) = I
t_part = s · 0 = 0
即第二帧第一次数据关联阶段，所有点仍近似保持原始位置；真正的运动估计需要依靠后面的 Ceres 优化完成。
6. 第二帧如何寻找角点和面点对应关系
6.1 角点：当前点对应上一帧的一条边缘线
对于第二帧当前的一个 sharp 角点：
P_curr
处理步骤为：
P_curr
↓
TransformToStart()
↓
得到 pointSel
↓
在第一帧 laserCloudCornerLast 的 KD-Tree 中找最近点 A
↓
再从附近不同 scan line 中找第二个点 B
↓
A、B 构成上一帧中的一条边缘线
最终建立的几何约束是：
当前角点经过位姿变换后，
应该尽量落在上一帧 A、B 构成的直线上。
点到线残差可写为：
P(q,t,s) =
Slerp(I, q_last_curr, s) · P_curr
+
s · t_last_curr
r_edge =
[(P(q,t,s) – A) × (P(q,t,s) – B)]
/
||A – B||
其中：
A、B：上一帧中构成边缘线的两个角点
×：叉乘
||A – B||：该边缘线的长度
r_edge：当前点到边缘线的三维残差向量
残差向量的模长，就是当前点到该直线的距离。
6.2 面点：当前点对应上一帧的局部平面
对于第二帧当前的一个 flat 面点：
P_curr
处理流程为：
P_curr
↓
TransformToStart()
↓
得到 pointSel
↓
KD-Tree 找到上一帧最近面点 A
↓
在附近 scan line 中继续寻找 B、C
↓
A、B、C 构成上一帧局部平面
平面法向量为：
n =
[(A – B) × (A – C)]
/
||(A – B) × (A – C)||
点到面残差为：
r_plane =
nᵀ · [P(q,t,s) – A]
含义是：
当前平面点经过变换后，
距离上一帧局部平面还有多远。
角点主要约束边缘方向，面点主要约束墙面、地面、桌面等平面结构。多个点到线、点到面约束共同约束 LiDAR 的旋转和平移。
7. 真正优化 q_last_curr、t_last_curr 的是 Ceres
必须区分两个阶段：
TransformToStart()
作用：根据当前位姿猜测，临时变换点，用于找对应关系。
Ceres + LidarEdgeFactor / LidarPlaneFactor
作用：把 q_last_curr、t_last_curr 当作未知变量，
真正通过最小化残差求出相对位姿。
Ceres 优化变量是：
para_q：四元数存储的旋转变量
para_t：三维平移变量
优化目标可以概括为：
min
Σ ρ( ||r_edge||² )
+
Σ ρ( r_plane² )
其中：
ρ：Huber 鲁棒核函数
r_edge：点到线残差
r_plane：点到面残差
HuberLoss 的作用是降低错误匹配、动态物体、遮挡点或重复结构造成的大残差影响。四元数使用局部参数化，避免优化过程中四元数不再满足单位长度约束。A-LOAM 的实现中把旋转和平移作为两个参数块加入 Ceres，并为旋转使用四元数参数化。
8. Ceres 内部优化时，去畸变如何一起变化
当 Ceres 尝试更新一组新的：
q_last_curr
t_last_curr
时，残差因子会重新计算：
q_part = Slerp(I, q_last_curr, s)
t_part = s · t_last_curr
P_compensated =
q_part · P_raw + t_part
因此，Ceres 调整整帧位姿时，点的部分旋转、部分平移和补偿后坐标也会同步变化。
完整关系是：
Ceres 更新 q_last_curr、t_last_curr
↓
当前点的部分位姿 q_part、t_part 自动变化
↓
当前点补偿后的位置自动变化
↓
点到线、点到面残差重新计算
↓
Ceres 继续调整 q_last_curr、t_last_curr
所以更准确的说法不是：
先完全去畸变
再优化位姿
而是：
位姿估计和逐点去畸变共用同一组 q_last_curr、t_last_curr，
并在优化残差计算过程中耦合更新。
但有一个关键限制：
Ceres 内部迭代时不会重新 KD-Tree 搜索对应点。
在一次 Ceres 求解中，点到线、点到面的对应关系已经固定；Ceres 只在固定对应关系下优化位姿。只有 Ceres 完成一次求解、进入下一轮外层循环时，系统才会用新的位姿重新变换点，并重新寻找对应关系。
9. 外层优化几次，Ceres 内层优化几次
A-LOAM 的 laserOdometry.cpp 中，外层循环固定执行两轮：
for (int outer = 0; outer < 2; ++outer)
{
// 1. 使用当前 q、t 做数据关联
// 2. 构造点到线、点到面残差
// 3. 调用 Ceres 优化 q、t
}
每一轮外层循环都包含：
重新找对应关系
+
重新建立 Ceres Problem
+
重新构造残差块
+
执行一次 Ceres Solve
而每次 Ceres Solve 的最大迭代次数配置为：
max_num_iterations = 4
因此，对于任意非第一帧：
外层数据关联与优化轮数：固定 2 轮
每轮 Ceres 最大迭代次数：4 次
理论最大内部迭代总数：2 × 4 = 8 次
注意是“最多 8 次”，不是保证一定优化满 8 次。若残差已经收敛、步长过小或满足 Ceres 停止条件，Ceres 可能提前结束。代码中也使用了 DENSE_QR 作为线性求解器。
10. 第二帧 F1 的完整两轮优化过程
第一轮外层优化：零运动初值，找粗对应
初值：
q₀←₁ = I
t₀←₁ = 0
流程：
第二帧 sharp / flat 特征点
↓
用 q = I、t = 0 调用 TransformToStart()
↓
得到近似未经补偿的 pointSel
↓
去第一帧 KD-Tree 中寻找粗略线、面对应
↓
建立 LidarEdgeFactor / LidarPlaneFactor
↓
Ceres 最多迭代 4 次
↓
得到第一轮位姿结果 T₀←₁^(1)
此时 Ceres 已经把“默认不动”的初值修正为一个粗略的真实相对运动。
第二轮外层优化：使用第一轮结果，重新找更准确对应
第一轮结束后：
q_last_curr = q₀←₁^(1)
t_last_curr = t₀←₁^(1)
第二轮不会把它们重置为零，而是继续使用第一轮结果。
流程变成：
使用第一轮优化后的 q、t
↓
按每个点时间比例 s 做更合理的部分运动补偿
↓
TransformToStart() 得到更准确 pointSel
↓
重新去第一帧 KD-Tree 中寻找线、面对应
↓
重新构造残差
↓
Ceres 再最多迭代 4 次
↓
得到最终相对位姿 T₀←₁*
用时序表示：
F1，第 1 轮：
q = I，t = 0
↓
粗对应
↓
Ceres ≤ 4 次
↓
T₀←₁^(1)
F1，第 2 轮：
q = T₀←₁^(1)
↓
重新对应
↓
Ceres ≤ 4 次
↓
T₀←₁*
这就是典型的：
初值预测
↓
数据关联
↓
固定对应关系下优化位姿
↓
根据新位姿重新建立数据关联
↓
再次优化
在 A-LOAM 的 laserOdometry 中，外层循环和 Ceres 内层迭代是两个不同层次的过程。外层第 1 轮开始时，系统先拿当前的位姿初值 R0,t0R^0,t^0R0,t0 把当前帧点临时变换到上一帧坐标系附近，并在上一帧 KD-Tree 中查找对应关系，例如给当前角点选出上一帧中的边缘线 ABABAB，给当前面点选出局部平面 ABCABCABC。这些“当前点对应哪条线、哪个平面”的关系一旦建立，就会在本轮 Ceres 优化期间固定下来。随后 Ceres 内层最多迭代 4 次：第 1 次根据 R0,t0R^0,t^0R0,t0 计算点到线、点到面的残差并求出位姿增量，更新得到 R1,t1R^1,t^1R1,t1；第 2 次并不会重新从零开始，而是直接以刚得到的 R1,t1R^1,t^1R1,t1 为基础继续计算残差、继续求增量，得到 R2,t2R^2,t^2R2,t2；后续第 3、4 次同样不断继承上一次内层迭代的结果，使位姿逐步收敛。因此，内层本质上是在同一批固定对应关系下连续微调 R,tR,tR,t，而不是每优化一次就重新选对应点。若开启 DISTORTION=1，每次内层更新 R,tR,tR,t 后，残差因子内部会依据点的时间比例 sss 重新计算该点的部分旋转、部分平移及去畸变后坐标，但它仍然是相对于固定的线 ABABAB 或平面 ABCABCABC 计算残差；不会重新执行 KD-Tree 搜索。等 Ceres 完成这一整轮最多 4 次的优化后，才进入外层第 2 轮：此时系统拿外层第 1 轮最终得到的 R,tR,tR,t，重新调用 TransformToStart()，再去上一帧 KD-Tree 中重新寻找更准确的线、面对应关系，然后在这批新的对应关系下再次调用 Ceres 进行最多 4 次迭代优化。简言之：内层是“固定对应、连续更新位姿”；外层是“使用更新后的位姿，重新找对应，再继续优化”。
11. 第三帧开始：初值不再是零，而是沿用上一帧运动
第二帧优化结束后，得到：
T₀←₁*
即：
第二帧相对于第一帧的最终相对运动。
第三帧 F2 到来时，程序不会重新执行：
q_last_curr = I
t_last_curr = 0
而是直接保留第二帧最后优化出的结果，作为第三帧初值：
T₁←₂^(0) ≈ T₀←₁*
也就是：
上一帧前进约 10 cm、左转约 2°
↓
下一帧初始先猜测
也大概前进 10 cm、左转 2°
↓
再通过 F2 与 F1 的特征匹配进行修正
这是一种非常简单的短时运动连续性假设。
上一帧的相对运动
≈
下一帧的相对运动初值
它只是预测，不是最终答案。每一帧仍然要经过两轮“重新找对应 + Ceres 优化”，才能得到当前帧真正的相对位姿。
12. 相对位姿优化完成后，如何累积为全局里程计
当第二帧得到最终相对位姿：
q_last_curr
t_last_curr
后，才会更新全局位姿：
t_w_curr =
t_w_last + q_w_last · t_last_curr
q_w_curr =
q_w_last · q_last_curr
代码中对应逻辑是：
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;
含义是：
先用上一帧的全局姿态，
把当前帧相对平移转到世界坐标系；
再把当前帧相对旋转累乘到全局旋转上。
对于第二帧，因为第一帧全局位姿为单位旋转、零平移，所以第二帧全局位姿基本就是：
第二帧相对于第一帧的优化结果。
对于后续帧，则不断执行：
帧间位姿增量
↓
连续累积
↓
得到 /laser_odom_to_init
这就是前端激光里程计轨迹。
13. 官方默认代码下，实际上发生了什么
官方 A-LOAM 默认：
DISTORTION = 0
于是代码中：
s = 1.0
对每个点都成立。
因此：
q_part = Slerp(I, q_last_curr, 1) = q_last_curr
t_part = 1 · t_last_curr = t_last_curr
即所有点都使用同一份完整帧间变换：
P_last =
q_last_curr · P_curr
+
t_last_curr
这表示：
默认代码没有逐点时间插值；
默认代码没有真正的帧内运动畸变补偿；
默认代码只是把整帧近似看作同一时刻采集。
此外，源码中将全部特征点和全分辨率点云统一变换到扫描末尾的 TransformToEnd() 代码块被：
if (0)
包住，默认不会执行。
所以即使把：
DISTORTION = 1
打开，laserOdometry 中的对应搜索和残差计算会开始按点时间插值，但程序仍不会默认生成一份完整、统一到扫描末尾时刻的去畸变点云并发布给后端。
最终总结
从第二帧开始，A-LOAM 并不是先知道第二帧到第一帧的真实位姿，再把点云转换过去；真实相对位姿本来就是未知量。它首先使用单位旋转和零平移作为初值，暂时认为前后两帧重合，用这个粗初值把当前点临时投到上一帧参考坐标附近，并在上一帧 KD-Tree 中寻找可能对应的边缘线和局部平面。随后，Ceres 将整帧的 q_last_curr、t_last_curr 作为待优化变量，通过最小化点到线、点到面残差，估计出当前帧相对于上一帧的旋转和平移。
开启去畸变后，A-LOAM 不为每一个点单独求位姿，而是先优化整帧相对运动，再根据每个点在扫描周期中的时间比例 sss，对整帧旋转做 SLERP 插值、对整帧平移做线性插值，从而得到该点的部分运动补偿。Ceres 每次更新整帧位姿时，点的部分位姿和残差会自动随之变化；但一次 Ceres Solve 内部不会重新查找 KD-Tree 对应关系，只有下一轮外层优化时才重新匹配。
官方 A-LOAM 对每一帧固定执行两轮外层“找对应 + Ceres 优化”，每轮 Ceres 最大迭代 4 次，因此每个非初始帧最多进行 8 次内部非线性优化。第二帧从零运动开始；第三帧及以后，则沿用上一帧最终相对运动作为当前帧初值，再经过两轮匹配和优化修正。最终得到的帧间位姿增量再累积到 q_w_curr、t_w_curr，形成高频的前端激光里程计轨迹。
