看完回环检测的代码，我们重新回到主线程：

correctPoses()

void correctPoses(){if (aLoopIsClosed == true){recentCornerCloudKeyFrames. clear();recentSurfCloudKeyFrames.   clear();recentOutlierCloudKeyFrames.clear();// update key posesint numPoses = isamCurrentEstimate.size();for (int i = 0; i < numPoses; ++i){cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().y();cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().z();cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().x();cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().roll();}aLoopIsClosed = false;}}

correctPoses()主要用来判断是否发生了回环检测，如果有回环就更新当前关键帧的位姿和位置。

publishTF()

void publishTF(){geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);odomAftMapped.pose.pose.orientation.x = -geoQuat.y;odomAftMapped.pose.pose.orientation.y = -geoQuat.z;odomAftMapped.pose.pose.orientation.z = geoQuat.x;odomAftMapped.pose.pose.orientation.w = geoQuat.w;odomAftMapped.pose.pose.position.x = transformAftMapped[3];odomAftMapped.pose.pose.position.y = transformAftMapped[4];odomAftMapped.pose.pose.position.z = transformAftMapped[5];将生成的四元数信息填充到 odomAftMapped.pose.pose.orientation。
将位置 transformAftMapped[3], [4], [5] 赋值给 odomAftMapped.pose.pose.position，代表的是更新后位姿的平移部分。odomAftMapped.twist.twist.angular.x = transformBefMapped[0];odomAftMapped.twist.twist.angular.y = transformBefMapped[1];odomAftMapped.twist.twist.angular.z = transformBefMapped[2];odomAftMapped.twist.twist.linear.x = transformBefMapped[3];odomAftMapped.twist.twist.linear.y = transformBefMapped[4];odomAftMapped.twist.twist.linear.z = transformBefMapped[5];将 transformBefMapped 中的角速度和线速度信息发布到 odomAftMapped.twist.twist 中。
angular 表示旋转速度，linear 表示平移速度，这里直接从 transformBefMapped 获取数据。
transformBefMapped 是上一帧的transformSum!!!pubOdomAftMapped.publish(odomAftMapped);aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry);aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], transformAftMapped[4], transformAftMapped[5]));tfBroadcaster.sendTransform(aftMappedTrans);}

 publishKeyPosesAndFrames()

void publishKeyPosesAndFrames(){if (pubKeyPoses.getNumSubscribers() != 0){sensor_msgs::PointCloud2 cloudMsgTemp;pcl::toROSMsg(*cloudKeyPoses3D, cloudMsgTemp);cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);cloudMsgTemp.header.frame_id = "camera_init";pubKeyPoses.publish(cloudMsgTemp);}if (pubRecentKeyFrames.getNumSubscribers() != 0){sensor_msgs::PointCloud2 cloudMsgTemp;pcl::toROSMsg(*laserCloudSurfFromMapDS, cloudMsgTemp);cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);cloudMsgTemp.header.frame_id = "camera_init";pubRecentKeyFrames.publish(cloudMsgTemp);}if (pubRegisteredCloud.getNumSubscribers() != 0){pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);*cloudOut += *transformPointCloud(laserCloudCornerLastDS,  &thisPose6D);*cloudOut += *transformPointCloud(laserCloudSurfTotalLast, &thisPose6D);sensor_msgs::PointCloud2 cloudMsgTemp;pcl::toROSMsg(*cloudOut, cloudMsgTemp);cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);cloudMsgTemp.header.frame_id = "camera_init";pubRegisteredCloud.publish(cloudMsgTemp);} }

发布的数据有历史关键帧位置数据cloudKeyPoses3D，这就是rviz里面轨迹的那条线。

laserCloudSurfFromMapDS是子地图数据。

laserCloudCornerLastDS等是当前帧的角点经位姿变化到地图坐标系下的点云数据。

clearCloud()

void clearCloud(){laserCloudCornerFromMap->clear();laserCloudSurfFromMap->clear();  laserCloudCornerFromMapDS->clear();laserCloudSurfFromMapDS->clear();   }

至此，lego-loam主要的代码就都注释完啦！！！完结撒花，咱们lio-sam再见！！

ヾ(￣▽￣)Bye~Bye~