gmapping算法源码实现分析（一）

gmapping算法源码实现分析一—— slam-gmapping功能包主干流程分析1.slam_gmapping.cpp初始化流程:SlamGmapping() 构造函数 ├─ init() - 创建 GridSlamProcessor 实例读取参数 └─ startLiveSlam() - 设置订阅和回调 ├─ 创建 LaserScan 订阅 (通过 MessageFilter) ├─ 注册 laserCallback 回调 └─ 启动 publishLoop 线程 (定期发布 map-odom 变换)激光雷达数据处理流程:laserCallback(scan) ├─ 节流控制 (throttle_scans_) ├─ 首次扫描: initMapper(scan) │ ├─ 计算激光坐标系姿态 │ ├─ 创建 RangeSensor 和 OdometrySensor │ ├─ 设置 GMapping 参数 │ └─ 调用 gsp_-init() 初始化粒子滤波器 │ └─ addScan(scan, odom_pose) ├─ 获取里程计姿态 getOdomPose() ├─ 准备激光数据 (处理角度反转) ├─ 创建 RangeReading └─ 调用 gsp_-processScan(reading) ← 核心算法入口 如果 processScan 成功: ├─ 获取最佳粒子的位姿 ├─ 计算 map_to_odom_ 变换 ├─ 更新地图 updateMap(scan) (按时间间隔) └─ 发布地图和元数据2.核心算法层 - openslam_gmapping功能包(gridslamprocessor.cpp)processScan 的实际执行流程:boolGridSlamProcessor::processScan(constRangeReadingreading,intadaptParticles){// 步骤1: 获取相对位姿并初始化OrientedPoint relPosereading.getPose();if(!m_count){m_lastPartPosem_odoPoserelPose;// 第一次扫描初始化}// 步骤2: 运动模型更新 - 为每个粒子绘制新位姿for(ParticleVector::iterator itm_particles.begin();it!m_particles.end();it){OrientedPointpose(it-pose);// 根据运动模型从上一帧位姿采样新位姿posem_motionModel.drawFromMotion(it-pose,relPose,m_odoPose);}// 步骤3: 累积位移并检查是否达到更新阈值OrientedPoint moverelPose-m_odoPose;m_linearDistancesqrt(move*move);m_angularDistancefabs(move.theta);m_odoPoserelPose;boolprocessedfalse;// 步骤4: 判断是否需要处理 (距离/角度/时间阈值)if(!m_count||m_linearDistancem_linearThresholdDistance||m_angularDistancem_angularThresholdDistance||(period_0.0(reading.getTime()-last_update_time_)period_)){last_update_time_reading.getTime();double*plainReadingnewdouble[m_beams];// 复制激光数据到数组RangeReading*reading_copynewRangeReading(...);if(m_count0){// 步骤5a: 非首帧 - 执行 scan matching 重采样scanMatch(plainReading);// ← 对每个粒子进行扫描匹配优化onScanmatchUpdate();updateTreeWeights(false);// 更新轨迹树权重resample(plainReading,adaptParticles,reading_copy);// ← 重采样}else{// 步骤5b: 首帧 - 仅注册扫描到地图for(ParticleVector::iterator itm_particles.begin();it!m_particles.end();it){m_matcher.invalidateActiveArea();m_matcher.computeActiveArea(it-map,it-pose,plainReading);m_matcher.registerScan(it-map,it-pose,plainReading);// 创建轨迹树节点TNode*nodenewTNode(it-pose,0.,it-node,0);node-readingreading_copy;it-nodenode;}}updateTreeWeights(false);delete[]plainReading;// 步骤6: 重置状态供下一帧使用m_lastPartPosem_odoPose;m_linearDistance0;m_angularDistance0;m_count;processedtrue;// 更新粒子的 previousPosefor(ParticleVector::iterator itm_particles.begin();it!m_particles.end();it){it-previousPoseit-pose;}}m_readingCount;returnprocessed;}3.地图更新流程 - updateMap (slam_gmapping.cpp)voidSlamGmapping::updateMap(constsensor_msgs::msg::LaserScan::ConstSharedPtr scan){// 创建临时的 ScanMatcherMapScanMatcherMapsmap(center,xmin_,ymin_,xmax_,ymax_,delta_);// 遍历最佳粒子的轨迹树重建地图for(TNode*nbest.node;n;nn-parent){matcher.invalidateActiveArea();matcher.computeActiveArea(smap,n-pose,((*n-reading)[0]));matcher.registerScan(smap,n-pose,((*n-reading)[0]));}// 调整 ROS 地图消息大小if(map size changed){重新分配 map_.data}// 将栅格概率转换为 OccupancyGrid 格式for(x,y){doubleoccsmap.cell(p);if(occ0)→-1(未知)elseif(occocc_thresh_)→100(障碍)else→0(空闲)}// 发布地图sst_-publish(map_);sstm_-publish(map_.info);}4.坐标变换发布 - publishTransformvoidSlamGmapping::publishTransform(){// 定期发布 map - odom 的静态变换// map_to_odom_ 在 laserCallback 中计算:// map_to_odom_ (odom_to_laser * laser_to_map).inverse()transform.transformtf2::toMsg(map_to_odom_);tfB_-sendTransform(transform);}总结主要流程是:ROS2 Wrapper 层: 接收 LaserScan → 转换格式 → 调用底层算法GridSlamProcessor 层:运动模型预测粒子位姿扫描匹配优化位姿粒子重采样维护轨迹树地图生成层: 从最佳粒子轨迹重建 OccupancyGrid 并发布