LeGo-LOAM中的transformFusion节点+相关数学公式推导

LeGo-LOAM中的transformFusion节点

LeGo-LOAM是在LOAM基础上改进的激光雷达定位与建图方法，它对地面点进行了优化、提出了两步LM优化算法，加入了闭环检测和全局因子图优化等。
本系列博客只说明了关键代码的功能实现，以及一些关键地方的数学推导，详细的解释都写在了代码注释中，注释后的代码在融合相机后会传到github。同时也列出来一些问题提醒自己有条件时实际去验证。

这一节先说明transformFusion节点的作用，这不会花费很多篇章，之后将featureAssociation节点与mapOptmization节点用到的Jacobian的推导放在这一节。

这个模块接收2种消息

// 从featureAssociation节点中接收的当前帧位姿，是当前帧点云结束点时刻Lidar坐标系到world系的位姿变换（高频）
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &TransformFusion::laserOdometryHandler, this);
// 从mapOptmization节点接收的优化后的位姿（低频）
subOdomAftMapped = nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 5, &TransformFusion::odomAftMappedHandler, this);

这个模块的作用就是将低频的位姿与高频的位姿融合，对应LOAM中的

这个节点最重要的函数就是transformAssociateToMap()他与mapOptmization节点中的同名函数功能一致，数学表达如下：

1. transformSum[]是Odometry模块计算出来的当前帧结束点时刻Lidar坐标系到世界坐标系的变换$T_{cur-end}^{world}$;
2. transformBefMapped[]是mapping之前的Odometry计算的世界坐标系下的转换矩阵$T_{bef-map}^{world}$;
3. 这里要计算transformTobeMapped[]

$$
T_{tobe}^{world} = T_{cur-end}^{world} * (T_{bef-map}^{world})^{-1} * T_{aft-map}^{world}
$$

位姿transformTobeMapped[]与Odometry模块同频率被发布出去。

Gauss-Newton优化算法需要的Jacobian推导