loam的学习记录4（TransformMaintenance节点）

TransformMaintenance节点

这是最后一个节点了，该节点的main函数很短。
main函数

ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> 
                                   ("/laser_odom_to_init", 5, laserOdometryHandler);

ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry> 
                                   ("/aft_mapped_to_init", 5, odomAftMappedHandler);

订阅laserOdometry节点发布的"/laser_odom_to_init"消息 和 laserMapping节点发布的"/aft_mapped_to_init"消息。

void transformAssociateToMap();

该函数更改了transformIncre（平移增量）和transformMapped（经过mapping矫正过后的最终的世界坐标系下的位姿），用来融合odometry节点mapping节点后的位姿。

回调函数laserOdometryHandler

double roll, pitch, yaw;
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;

通过laserOdometry构造四元数与位移

transformAssociateToMap();

调用transformAssociateToMap函数，更新位姿。

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

四元数由旋转角得到

laserOdometry2.header.stamp = laserOdometry->header.stamp;
laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
laserOdometry2.pose.pose.orientation.z = geoQuat.x;
laserOdometry2.pose.pose.orientation.w = geoQuat.w;
laserOdometry2.pose.pose.position.x = transformMapped[3];
laserOdometry2.pose.pose.position.y = transformMapped[4];
laserOdometry2.pose.pose.position.z = transformMapped[5];

设置发布的laserOdometry2的参数（四元数+位移）。

回调函数odomAftMappedHandler
在接收到了laserMapping的消息后，更新位姿。

当接收到laserMapping的消息后更新位姿，而这个优化结果会被回调函数laserOdometryHandler​​​​​​​里的transformAssociateToMap这一个函数一直利用来建图，一直到下一次接收到laserMapping​​​​​​​的消息。

参考：
LOAM:3D激光里程计及环境建图的方法和实现（四）
LOAM_velodyne学习（四）