从零入门激光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);}