LeGO-LOAM中transformFusion详细解读

1. 文件整体作用

LeGO-LOAM 中，前端激光里程计和后端地图优化承担的职责不同。前端激光里程计通常每一帧都运行，能够快速估计机器人短时间内发生的运动，因此输出频率较高、轨迹较连续。但前端主要依据相邻帧或短时间局部约束计算位姿，累计运行后容易产生位置和姿态漂移。

后端地图优化则会将当前特征点与局部地图中的边缘特征、平面特征进行匹配，通过 scan-to-map 优化修正前端漂移。它输出的位姿通常更接近真实地图坐标，但计算量较大，不会像前端一样每帧都立即得到完整优化结果。

TransformFusion 就位于两者之间。它不会让当前帧重新跑一次 scan-to-map，而是以"最近一次地图优化后的位姿"为新的可信起点，再叠加当前帧相对该优化帧的前端运动增量。这样，当前帧既保留了前端高频更新的连续性，也继承了后端地图优化的校正效果。

整体数据关系可以概括为：

复制代码

前端激光里程计：
高频、连续，但存在累计漂移
        ↓
后端地图优化：
低频、精度高，可修正漂移
        ↓
TransformFusion：
将当前前端相对运动接到最近优化后的位姿上
        ↓
输出当前帧地图校正位姿 /integrated_to_init

对应订阅和发布代码如下：

复制代码

pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry>(
    "/integrated_to_init", 5);

subLaserOdometry = nh.subscribe<nav_msgs::Odometry>(
    "/laser_odom_to_init", 5,
    &TransformFusion::laserOdometryHandler, this);

subOdomAftMapped = nh.subscribe<nav_msgs::Odometry>(
    "/aft_mapped_to_init", 5,
    &TransformFusion::odomAftMappedHandler, this);

2. 核心思想：不是"当前帧重新优化"，而是"优化结果继续往前传播"

理解这个文件时，最关键的是区分"当前帧"和"最近一次完成地图优化的关键帧"。

假设最近一次地图优化发生在时刻 k。在该时刻，前端里程计先给出了一个原始位姿，随后后端地图优化根据局部地图匹配结果对其进行校正。之后，机器人继续运动到当前时刻 t，前端里程计又输出当前最新位姿。

此时系统拥有三份关键状态：最近优化关键帧的原始前端位姿、该关键帧优化后的地图位姿，以及当前帧的原始前端位姿。TransformFusion 的任务就是利用这三份状态，构造当前帧的地图校正位姿。

复制代码

时刻 k：最近一次地图优化关键帧

前端原始位姿：T_bef
后端优化位姿：T_aft


时刻 t：当前最新 LiDAR 帧

当前前端位姿：T_sum

最终输出：

复制代码

当前地图校正位姿：T_mapped

核心关系如下：

复制代码

T_mapped = T_aft · (T_bef⁻¹ · T_sum)

这里括号中的部分表示：从最近一次优化关键帧，到当前帧之间，前端里程计认为机器人发生了多少相对运动；最前面的 T_aft 则表示：最近一次优化后，机器人在地图中的正确参考位姿。

因此可以用一句更直观的话概括：

复制代码

最近一次优化后的位姿
+
当前帧相对最近优化帧的前端运动增量
=
当前帧最终地图校正位姿

不过这里的"加"不是普通的坐标相加。机器人位姿包含位置和姿态，机器人可能在这一段时间内发生转弯、俯仰或侧倾，所以前端得到的相对位移必须先随着姿态旋转到正确方向，再叠加到地图优化后的位姿上。

3. 五组核心变量之间的关系

源码中最重要的变量是下面五组长度为 6 的数组：

复制代码

float transformSum[6];
float transformIncre[6];
float transformMapped[6];
float transformBefMapped[6];
float transformAftMapped[6];

它们不要拆开孤立理解，而要看成一条完整的位姿传播链。

transformBefMapped 和 transformAftMapped 对应同一个最近优化关键帧。前者表示该关键帧在进入后端地图优化之前，前端激光里程计给出的原始估计；后者表示后端完成 scan-to-map 匹配后，对该帧做出的校正结果。两者的差异，就是后端认为前端累计漂移了多少。

transformSum 表示当前时刻前端激光里程计给出的最新原始位姿。当前帧可能已经位于最近优化关键帧之后很多帧，因此不能直接把 transformAftMapped 当作当前输出，否则机器人位姿会停在最后一次优化的位置，无法反映当前运动。

transformIncre 是计算过程中的中间变量，主要保存当前帧相对于最近优化帧的平移增量。transformMapped 则是最终结果，也就是发布到 /integrated_to_init 的地图校正位姿。

可以将这五组变量连起来理解：

复制代码

transformBefMapped：
最近优化关键帧的前端原始位姿

transformAftMapped：
同一关键帧经过地图优化后的校正位姿

transformSum：
当前时刻前端里程计位姿

transformIncre：
当前帧相对最近优化帧的运动增量

transformMapped：
当前时刻最终发布的地图校正位姿

数组前三项用于描述旋转，后三项用于描述平移：

复制代码

transform[0~2]：内部姿态角

transform[3]：x 方向位置
transform[4]：y 方向位置
transform[5]：z 方向位置

需要注意的是，前三项不能直接理解成标准 ROS 下的 roll、pitch、yaw。旧版 LOAM 使用了历史遗留的 camera 坐标系约定，因此源码中对四元数进行了轴交换和符号转换：

复制代码

tf::Matrix3x3(
    tf::Quaternion(
        geoQuat.z,
        -geoQuat.x,
        -geoQuat.y,
        geoQuat.w
    )
).getRPY(roll, pitch, yaw);

transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;

所以不能只改某一个负号或交换某个角度下标。输入、内部计算、输出四元数恢复必须保持完全一致，否则容易出现地图翻转、轨迹偏轴、姿态错误或点云匹配不稳定。

4. `odomAftMappedHandler()`：保存最近一次优化前后的参考位姿

odomAftMappedHandler() 接收的话题是：

复制代码

/aft_mapped_to_init

它的职责不是立即输出结果，而是保存后续高频位姿计算所需的参考状态。首先，代码从 pose.pose 中读取最近一次地图优化后的位姿：

复制代码

transformAftMapped[0] = -pitch;
transformAftMapped[1] = -yaw;
transformAftMapped[2] = roll;

transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
transformAftMapped[5] = odomAftMapped->pose.pose.position.z;

这里的 transformAftMapped 就是后端 scan-to-map 优化后的结果。它表示：地图优化模块认为机器人在该关键帧时刻，真正应该位于哪里、朝向哪里。

然后，代码从 twist 字段中读取 transformBefMapped：

复制代码

transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;
transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;
transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;

transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;
transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;
transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;

这里有一个非常重要的工程细节：在标准 ROS 语义中，twist.linear 应当表示线速度，twist.angular 应当表示角速度；但 LeGO-LOAM 在这个话题中复用了 twist 字段，将其当作"优化前的原始位姿缓存"。

也就是说，这个话题中的含义是：

复制代码

pose.pose：
最近一次优化后的位姿 T_aft

twist.angular：
最近一次优化前的姿态 T_bef

twist.linear：
最近一次优化前的位置 T_bef

因此，/aft_mapped_to_init 不能被简单当作普通的标准里程计消息使用。其他模块若直接把 twist 当成速度，就会读到错误数据。对于 TransformFusion 而言，这种设计是必须的，因为它需要同时知道"优化前从哪里开始"和"优化后被校正到哪里"。

5. `laserOdometryHandler()`：接收当前高频前端位姿

laserOdometryHandler() 接收：

复制代码

/laser_odom_to_init

该话题代表当前时刻前端激光里程计给出的位姿。它更新快，能够连续反映机器人运动，但仍可能带有前端累计漂移。

回调函数首先读取当前四元数，并转换到 LOAM 内部使用的姿态表示：

复制代码

geometry_msgs::Quaternion geoQuat =
    laserOdometry->pose.pose.orientation;

tf::Matrix3x3(
    tf::Quaternion(
        geoQuat.z,
        -geoQuat.x,
        -geoQuat.y,
        geoQuat.w
    )
).getRPY(roll, pitch, yaw);

transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;

随后读取当前位置：

复制代码

transformSum[3] = laserOdometry->pose.pose.position.x;
transformSum[4] = laserOdometry->pose.pose.position.y;
transformSum[5] = laserOdometry->pose.pose.position.z;

到这里，当前前端里程计结果已经保存在 transformSum 中。它表示机器人从初始帧累计到当前时刻的原始位姿，但这个结果并没有直接发布为最终地图位姿。

真正的校正发生在下面这一句：

复制代码

transformAssociateToMap();

该函数会读取此前保存的 transformBefMapped、transformAftMapped，以及当前的 transformSum，计算最终的 transformMapped。

之后，程序将内部姿态重新转成 ROS 四元数：

复制代码

geoQuat = tf::createQuaternionMsgFromRollPitchYaw(
    transformMapped[2],
    -transformMapped[0],
    -transformMapped[1]
);

并发布最终结果：

复制代码

laserOdometry2.pose.pose.position.x = transformMapped[3];
laserOdometry2.pose.pose.position.y = transformMapped[4];
laserOdometry2.pose.pose.position.z = transformMapped[5];

pubLaserOdometry2.publish(laserOdometry2);

因此，/integrated_to_init 的含义是：当前帧保持前端运动连续性，但坐标基准已经切换到最近一次地图优化后的校正参考系中。

6. `transformAssociateToMap()`：先计算当前相对运动

transformAssociateToMap() 是整个文件的核心。它不是直接用当前前端位姿覆盖后端优化位姿，而是先求当前帧相对于最近优化帧到底走了多少。

从理论上看，三维位姿通常由旋转矩阵与平移向量组成：

复制代码

T =
[ R  t ]
[ 0  1 ]

其中，R 表示姿态旋转，t 表示三维位置。

当前帧相对最近优化关键帧的前端运动可以写为：

复制代码

ΔT_odom = T_bef⁻¹ · T_sum

其中，T_bef 表示最近一次优化时前端的原始位姿，T_sum 表示当前时刻前端的原始位姿。通过先乘 T_bef 的逆，可以去除旧参考帧的影响，只保留从最近优化帧到当前帧之间发生的相对运动。

源码没有直接构造 4×4 齐次变换矩阵，而是手工通过三角函数完成坐标变换。函数开头的平移部分如下：

复制代码

float x1 = cos(transformSum[1]) *
           (transformBefMapped[3] - transformSum[3])
         - sin(transformSum[1]) *
           (transformBefMapped[5] - transformSum[5]);

float y1 = transformBefMapped[4] - transformSum[4];

float z1 = sin(transformSum[1]) *
           (transformBefMapped[3] - transformSum[3])
         + cos(transformSum[1]) *
           (transformBefMapped[5] - transformSum[5]);

这里表面上是"优化前减当前"，并不像最直观的"当前减优化前"。但不能只根据这一小段判断整体方向，因为 LOAM 内部存在特定的坐标轴定义、旋转顺序、符号约定，后面还会继续经过多轮旋转，并在最终叠加时出现相应的符号变化。

后续代码继续将差值转换为相对增量：

复制代码

transformIncre[3] =
    cos(transformSum[2]) * x2
  + sin(transformSum[2]) * y2;

transformIncre[4] =
   -sin(transformSum[2]) * x2
  + cos(transformSum[2]) * y2;

transformIncre[5] = z2;

最终，transformIncre[3~5] 表示当前帧相对于最近优化关键帧的前端平移变化。它不是最终地图坐标，而是待叠加到优化后位姿上的局部运动增量。

7. 姿态计算：长串三角函数本质是旋转复合

函数中最难读的部分，是大量 sin()、cos()、asin() 和 atan2()：

复制代码

float sbcx = sin(transformSum[0]);
float cbcx = cos(transformSum[0]);

float sblx = sin(transformBefMapped[0]);
float cblx = cos(transformBefMapped[0]);

float salx = sin(transformAftMapped[0]);
float calx = cos(transformAftMapped[0]);

这些变量并不是新的传感器数据，也不是新的优化状态，只是将三个姿态中的正弦、余弦结果提前缓存下来，避免在后续复杂表达式中不断重复计算。

它们对应三套姿态：

复制代码

transformSum：
当前时刻前端原始姿态

transformBefMapped：
最近优化帧的前端原始姿态

transformAftMapped：
最近优化帧的后端优化姿态

从标准位姿变换角度看，当前校正姿态的关系可以写为：

复制代码

R_mapped = R_aft · R_befᵀ · R_sum

其中，R_befᵀ 的作用是消除"最近优化时刻原始前端姿态"的影响；再乘以 R_aft，将当前前端运动放到地图优化后的正确姿态基准中。

源码没有使用 Eigen 直接写矩阵乘法，而是将旋转矩阵相乘后的元素全部展开。例如：

复制代码

float srx = ...;
transformMapped[0] = -asin(srx);

随后使用：

复制代码

transformMapped[1] = atan2(
    srycrx / cos(transformMapped[0]),
    crycrx / cos(transformMapped[0])
);

transformMapped[2] = atan2(
    srzcrx / cos(transformMapped[0]),
    crzcrx / cos(transformMapped[0])
);

这里的 asin() 和 atan2() 用于从复合后的旋转矩阵中恢复内部欧拉角。atan2(y, x) 不只是计算 y/x 的反正切，而是会结合 x、y 的符号判断角度所在象限，因此在机器人发生大角度转向时更可靠。

这部分的本质可以理解成：

复制代码

当前前端姿态
先去除最近优化帧时的原始前端姿态
再叠加最近优化帧的地图校正姿态
最终得到当前帧校正后的姿态

8. 最终位置计算：局部位移必须先旋转再叠加

完成姿态计算后，程序还不能直接把 transformIncre 加到 transformAftMapped 上。因为 transformIncre 描述的是相对运动，而相对运动所在的方向与当前地图坐标方向有关。

例如机器人已经左转 90°，那么机器人自身坐标系中的"向前走 1 米"，在地图坐标系中可能是 y 增加 1 米，而不是 x 增加 1 米。如果直接把局部前进量加到地图 x 上，轨迹方向会错误。

因此，源码会先旋转平移增量：

复制代码

x1 = cos(transformMapped[2]) * transformIncre[3]
   - sin(transformMapped[2]) * transformIncre[4];

y1 = sin(transformMapped[2]) * transformIncre[3]
   + cos(transformMapped[2]) * transformIncre[4];

z1 = transformIncre[5];

接着继续依据其余姿态角进行变换：

复制代码

x2 = x1;

y2 = cos(transformMapped[0]) * y1
   - sin(transformMapped[0]) * z1;

z2 = sin(transformMapped[0]) * y1
   + cos(transformMapped[0]) * z1;

最后才把已经转换方向后的位移叠加到优化后位置：

复制代码

transformMapped[3] = transformAftMapped[3]
                   - (cos(transformMapped[1]) * x2
                   + sin(transformMapped[1]) * z2);

transformMapped[4] = transformAftMapped[4] - y2;

transformMapped[5] = transformAftMapped[5]
                   - (-sin(transformMapped[1]) * x2
                   + cos(transformMapped[1]) * z2);

从标准概念上，可以将这一过程理解为：

复制代码

t_mapped =
t_aft +
R_mapped · Δt_odom

源码中的负号、轴交换和旋转顺序，来自 LOAM 自己的内部坐标约定；但整体语义没有变化：先将当前相对位移旋转到地图方向，再接到后端优化后的地图位置上。

9. 输出 Odometry、TF 与工程注意点

计算完成后，transformMapped 会被发布为 /integrated_to_init。该消息使用：

复制代码

父坐标系：/camera_init
子坐标系：/camera

对应代码：

复制代码

laserOdometry2.header.frame_id = "/camera_init";
laserOdometry2.child_frame_id = "/camera";

TF 发布部分如下：

复制代码

laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;

laserOdometryTrans2.setRotation(
    tf::Quaternion(
        -geoQuat.y,
        -geoQuat.z,
        geoQuat.x,
        geoQuat.w
    )
);

laserOdometryTrans2.setOrigin(
    tf::Vector3(
        transformMapped[3],
        transformMapped[4],
        transformMapped[5]
    )
);

tfBroadcaster2.sendTransform(laserOdometryTrans2);

因此，这个文件真正广播的是：

复制代码

/camera_init → /camera

源码中虽然还声明了 /map → /camera_init 和 /camera → /base_link：

复制代码

map_2_camera_init_Trans.frame_id_ = "/map";
map_2_camera_init_Trans.child_frame_id_ = "/camera_init";

camera_2_base_link_Trans.frame_id_ = "/camera";
camera_2_base_link_Trans.child_frame_id_ = "/base_link";

但在这份代码中没有看到对应的 sendTransform()。因此，不能认为完整的 /map → /camera_init → /camera → /base_link TF 链已经由这个文件发布。若 RViz 报 TF 不连通，或需要将 LeGO-LOAM 接到导航系统中，还需要检查其他节点、静态 TF 发布器或 launch 文件。

此外，这个节点没有使用 message_filters 做时间同步，也没有为"是否已经收到第一帧地图优化结果"设置单独标志位。它默认认为 transformBefMapped 与 transformAftMapped 已经代表有效的最近优化状态。因此在系统刚启动时，应确保地图优化相关话题能够正常发布，否则初期位姿输出可能不符合预期。

10. 总结

TransformFusion 是 LeGO-LOAM 中连接前端激光里程计与后端地图优化的关键模块。它的名字里虽然带有 Fusion，但它并不是 EKF、UKF、因子图优化或多传感器概率融合器。代码中没有协方差矩阵、状态转移矩阵、卡尔曼增益、观测模型或权重分配，它做的是确定性的三维位姿复合。其核心作用是：后端地图优化每次得到更准确的关键帧位姿后，TransformFusion 将该位姿作为新的全局校正基准，并把当前前端里程计产生的高频相对运动接到这个基准上。

理解这一文件，必须先明确"最近一次优化帧"与"当前帧"不是同一个概念。最近一次优化帧通常是某个较早的关键帧，后端在这个帧上完成了 scan-to-map 匹配和非线性优化。此时系统会保存两套位姿：transformBefMapped 表示优化前的前端原始位姿，transformAftMapped 表示优化后的地图校正位姿。它们对应同一个时间点，因此两者的差异可以视为后端对前端累计漂移的修正量。

当前帧到来时，前端激光里程计会输出 transformSum。该位姿反映当前机器人相对于初始帧的原始运动状态，但前端持续运行后不可避免会积累误差。系统不会直接把 transformSum 当作最终输出，也不会强制把当前帧锁死在 transformAftMapped 的位置。正确做法是先计算当前帧相对于最近优化帧的运动增量，然后让"优化后的关键帧位姿"继续沿着这一增量前进，最终得到当前帧的 transformMapped。

可以把整个过程理解成：

复制代码

先求：
当前帧相对最近优化帧又走了多少

再做：
把这段相对运动接到最近一次优化后的正确位姿上

最终得到：
当前帧高频、连续、带地图校正的位姿

从数学语义上看，核心关系是当前校正位姿等于最近优化后位姿，乘以当前帧相对于最近优化帧的相对运动。之所以不能简单对 x、y、z 直接相加，是因为机器人运动发生在三维空间中。机器人可能在这一段时间内转弯、侧倾、爬坡或俯仰，其局部坐标中的前进方向并不一定与地图坐标系中的 x 轴一致。因此，当前相对平移必须随着姿态变化进行旋转，最终才能正确映射到地图坐标中。

源码中的 transformAssociateToMap() 正是在完成这一过程。函数前半部分计算当前帧与最近优化帧之间的相对平移；中间部分通过长串三角函数计算姿态复合；后半部分将平移增量根据校正后的姿态旋转，再叠加到优化后的地图位置。虽然实现形式看起来复杂，包含大量 sin()、cos()、asin()、atan2() 和不同轴之间的变换，但本质上就是对三维刚体变换进行组合。

源码中还有一个重要细节：/aft_mapped_to_init 的 twist 字段并没有按照 ROS 标准语义存储速度，而是被复用来存储"优化前的原始位姿"。其中 twist.angular 保存优化前姿态，twist.linear 保存优化前位置；真正的优化后位姿则放在 pose.pose 中。这种实现便于在一个 nav_msgs::Odometry 消息中同时传递两套位姿，但会破坏 twist 的常规语义。因此，后续若将该话题接入其他系统，不能直接把它的 twist 当作线速度和角速度使用。

从 TF 角度看，这份文件实际发布的是 /camera_init → /camera。代码中虽然定义了 /map → /camera_init 和 /camera → /base_link，但没有看到对应的广播调用。因此，若后续要接 RViz、Nav2、robot_localization 或底盘控制，还需要检查完整 TF 树是否由其他节点补齐。对于现代导航系统，更推荐逐步整理成 map → odom → base_link 的标准结构：地图优化造成的全局校正放在 map → odom，局部连续运动放在 odom → base_link，这样全局校正发生跳变时，不会直接影响局部控制器对机器人连续运动的判断。

总体而言，TransformFusion 不是负责"产生优化"的模块，而是负责"传播优化结果"的模块。它让后端地图优化不只停留在少量关键帧上，而是能够持续影响之后每一个高频前端里程计帧。前端提供实时性和连续性，后端提供地图一致性和漂移校正，TransformFusion 则将两者连接起来，输出当前时刻更可靠的机器人位姿。

欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。

LeGO-LOAM中transformFusion详细解读

1. 文件整体作用

2. 核心思想：不是"当前帧重新优化"，而是"优化结果继续往前传播"

3. 五组核心变量之间的关系

4. odomAftMappedHandler()：保存最近一次优化前后的参考位姿

5. laserOdometryHandler()：接收当前高频前端位姿

6. transformAssociateToMap()：先计算当前相对运动

7. 姿态计算：长串三角函数本质是旋转复合

8. 最终位置计算：局部位移必须先旋转再叠加

9. 输出 Odometry、TF 与工程注意点

10. 总结

4. `odomAftMappedHandler()`：保存最近一次优化前后的参考位姿

5. `laserOdometryHandler()`：接收当前高频前端位姿

6. `transformAssociateToMap()`：先计算当前相对运动