从零入门激光SLAM（十三）——LeGo-LOAM源码超详细解析4

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群****Q：772356582，欢迎大家加入讨论。

一、整体框架

1.1 目的

主要根据里程计获得的先验位姿进行后端优化，闭环检测和图优化

1.2 输入

//接收相机坐标系下的点和里程计
//上一帧角点
subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this); 
//上一帧面点
subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
//上一帧无效点
subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this); 
//里程计位姿
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);  
//IMU数据
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 50, &mapOptimization::imuHandler, this);

1.3 输出

//机器人关键帧在全局坐标系下的位置信息，轨迹
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("/key_pose_origin", 2);
//机器人周围激光雷达点云数据
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 2);
//经过位姿图优化和点云配准后的里程计信息
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);
//机器人历史轨迹的点云数据
pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/history_cloud", 2);
//经过ICP配准后的机器人激光雷达点云数据
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/corrected_cloud", 2);
//机器人最近获取的点云数据
pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/recent_cloud", 2);
//经过位姿图优化和点云配准后的机器人点云数据用于建图
pubRegisteredCloud = nh.advertise<sensor_msgs::PointCloud2>("/registered_cloud", 2);

主函数

主要的功能是在run函数里面

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lego_loam");
    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
    mapOptimization MO;
    // 1.进行闭环检测与闭环的功能
    std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
    // 2.将数据发布到ros中，可视化
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
    ros::Rate rate(200);
    while (ros::ok())
    {
        ros::spinOnce();
        MO.run(); //进入执行run函数
        rate.sleep();
    }
    loopthread.join();
    visualizeMapThread.join();
    return 0;
}
// 3.run函数
void run(){
	if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
		timeLastProcessing = timeLaserOdometry;
		transformAssociateToMap(); //转换到map坐标系下
		extractSurroundingKeyFrames(); //提取周围的关键帧
		downsampleCurrentScan(); //下采样当前帧
		// 当前扫描进行边缘优化，图优化以及进行LM优化的过程
		scan2MapOptimization();
		saveKeyFramesAndFactor(); //保存关键帧和因子
		correctPoses(); //校正位姿
		publishTF(); //发布坐标变换
		publishKeyPosesAndFrames(); //发布关键帧和因子
		clearCloud();}}} //清除点云

二、函数解析

2.1 transformAssociateToMap

作用：将坐标转移到世界坐标系下,得到可用于建图的Lidar坐标
输入：transformBefMapped[] 前一帧在世界坐标系的位姿
```
 transformSum  当前帧的位姿
```
输出：transformTobeMapped当前帧在世界坐标系的位置
代码：

 void transformAssociateToMap()
    {
	 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]);
	 float x2 = x1;
	 float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
	 float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
	 // 计算平移增量
	 transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
	 transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
	 transformIncre[5] = z2;
	 ……
		 x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) 
		 * transformIncre[4];
	 y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2])
		 * transformIncre[4];
	 z1 = transformIncre[5];

	 x2 = x1;
	 y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
	 z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

	 transformTobeMapped[3] = transformAftMapped[3] 
		 - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
	 transformTobeMapped[4] = transformAftMapped[4] - y2;
	 transformTobeMapped[5] = transformAftMapped[5] 
		 - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);}