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

详情请见。。。
https://www.guyuehome.com/46822

  • 5
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

桦树无泪

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值