三维SLAM算法LeGO-LOAM源码阅读（四）

最后一个部分是对位姿信息的融合计算，难得代码不长，先看看构造函数：

//综合后发送的里程计信息
pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5);
//特征匹配时粗配准的里程计信息
subLaserOdometry = nh.subscribe("/laser_odom_to_init", 5, &TransformFusion::laserOdometryHandler, this);
//建图精配准之后的里程计信息
subOdomAftMapped = nh.subscribe("/aft_mapped_to_init", 5, &TransformFusion::odomAftMappedHandler, this);

因此该节点是由两个回调函数所驱动的。我们先看到odomAftMappedHandler这个回调函数：

void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
    {
        double roll, pitch, yaw;
        geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
        tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
        //位姿作为计算的基础
        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;
        //速度作为下一次计算的先验
        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;
    }

因此是通过odomAftMappedHandler函数获取精配准后的位姿作为transformAftMapped，而获取配准后的速度信息作为transformBefMapped准备下一次计算。

而laserOdometryHandler是将粗配准的里程计信息与精配准的里程计信息融合计算，并在回调函数中便发送了最终外发的里程计话题。在该回调函数中的TF与里程计话题才是最终决定的。融合计算的过程实在头晕，下次再仔细看看。。。

void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
    {
        currentHeader = laserOdometry->header;

        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;
        //位姿与速度的融合计算
        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];
        pubLaserOdometry2.publish(laserOdometry2);

        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);
    }

那么我们回溯一下这两个话题，粗配准的里程计信息是FeatureAssociation发出的，精配准的信息是mapOptimization发出的，均以200Hz的频率，当odomAftMappedHandler收到精配准信息后更新位姿，这个位姿将在laserOdometryHandler收到下一条粗配准信息后综合计算再发出。

最后来一张搓搓的图解释整个过程：