1. 文件整体作用LeGO-LOAM 中前端激光里程计和后端地图优化承担的职责不同。前端激光里程计通常每一帧都运行能够快速估计机器人短时间内发生的运动因此输出频率较高、轨迹较连续。但前端主要依据相邻帧或短时间局部约束计算位姿累计运行后容易产生位置和姿态漂移。后端地图优化则会将当前特征点与局部地图中的边缘特征、平面特征进行匹配通过 scan-to-map 优化修正前端漂移。它输出的位姿通常更接近真实地图坐标但计算量较大不会像前端一样每帧都立即得到完整优化结果。TransformFusion就位于两者之间。它不会让当前帧重新跑一次 scan-to-map而是以“最近一次地图优化后的位姿”为新的可信起点再叠加当前帧相对该优化帧的前端运动增量。这样当前帧既保留了前端高频更新的连续性也继承了后端地图优化的校正效果。整体数据关系可以概括为前端激光里程计 高频、连续但存在累计漂移 ↓ 后端地图优化 低频、精度高可修正漂移 ↓ TransformFusion 将当前前端相对运动接到最近优化后的位姿上 ↓ 输出当前帧地图校正位姿 /integrated_to_init对应订阅和发布代码如下pubLaserOdometry2 nh.advertisenav_msgs::Odometry( /integrated_to_init, 5); subLaserOdometry nh.subscribenav_msgs::Odometry( /laser_odom_to_init, 5, TransformFusion::laserOdometryHandler, this); subOdomAftMapped nh.subscribenav_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字段中读取transformBefMappedtransformBefMapped[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_linkmap_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_linkTF 链已经由这个文件发布。若 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详细解读
发布时间：2026/6/21 10:29:54
1. 文件整体作用LeGO-LOAM 中前端激光里程计和后端地图优化承担的职责不同。前端激光里程计通常每一帧都运行能够快速估计机器人短时间内发生的运动因此输出频率较高、轨迹较连续。但前端主要依据相邻帧或短时间局部约束计算位姿累计运行后容易产生位置和姿态漂移。后端地图优化则会将当前特征点与局部地图中的边缘特征、平面特征进行匹配通过 scan-to-map 优化修正前端漂移。它输出的位姿通常更接近真实地图坐标但计算量较大不会像前端一样每帧都立即得到完整优化结果。TransformFusion就位于两者之间。它不会让当前帧重新跑一次 scan-to-map而是以“最近一次地图优化后的位姿”为新的可信起点再叠加当前帧相对该优化帧的前端运动增量。这样当前帧既保留了前端高频更新的连续性也继承了后端地图优化的校正效果。整体数据关系可以概括为前端激光里程计 高频、连续但存在累计漂移 ↓ 后端地图优化 低频、精度高可修正漂移 ↓ TransformFusion 将当前前端相对运动接到最近优化后的位姿上 ↓ 输出当前帧地图校正位姿 /integrated_to_init对应订阅和发布代码如下pubLaserOdometry2 nh.advertisenav_msgs::Odometry( /integrated_to_init, 5); subLaserOdometry nh.subscribenav_msgs::Odometry( /laser_odom_to_init, 5, TransformFusion::laserOdometryHandler, this); subOdomAftMapped nh.subscribenav_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字段中读取transformBefMappedtransformBefMapped[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_linkmap_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_linkTF 链已经由这个文件发布。若 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则将两者连接起来输出当前时刻更可靠的机器人位姿。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。
