slam_toolbox Karto 内核

05 Karto 内核

源码：lib/karto_sdk/include/karto_sdk/Mapper.h、lib/karto_sdk/src/Mapper.cpp。
这是 slam_toolbox 的算法本体。ROS 层只负责把 LocalizedRangeScan 喂进来，再把 CorrectedPose 变成 TF。

5.1 主要对象

类型	角色
LocalizedRangeScan	一帧扫描 + 里程计位姿 + 修正位姿
LaserRangeFinder	角分辨率、量程、相对 base 的 OffsetPose、是否 360°
Mapper	处理入口、参数、持有 Graph / SensorManager / Matcher
MapperGraph	顶点=扫描，边=相对位姿约束
ScanMatcher	相关匹配：粗搜 + 细搜
MapperSensorManager	LastScan、RunningScans 滑动窗口
ScanSolver	抽象优化器，实现是 Ceres

SMapper（slam_mapper.cpp）只是包一层：configure() 把 YAML 写进 Mapper 的 setter，getOccupancyGrid() 调 OccupancyGrid::CreateFromScans。

5.2 Mapper::Process（建图主路径，约 L2731）

校验 LaserRangeFinder::Validate
未初始化 → Initialize(rangeThreshold)
取该传感器 LastScan
若有上一帧：
    T = (odom_last → corrected_last)
    本帧 corrected = T * odom_curr      // 用上一帧的修正量预测
HasMovedEnough？不够则 false
若 use_scan_matching 且有上一帧：
    SequentialScanMatcher::MatchScan(本帧, RunningScans, bestPose, cov)
    SetSensorPose(bestPose)             // 同时更新 corrected
AddScan（赋 id）
AddVertex + AddEdges(cov)               // 与近邻连边
AddRunningScan
若 do_loop_closing：
    对每个传感器 TryCloseLoop
SetLastScan
return true

MatchScan 是前端：在局部 running scans 上做相关网格搜索，得到相对里程计更好的 R,t 和协方差。
AddEdges 把相邻约束交给 ScanSolver::AddConstraint（先积累，不算）。
TryCloseLoop 成功才会 CorrectPoses() → ScanSolver::Compute()。

5.3 回环：MapperGraph::TryCloseLoop（约 L1515）

FindPossibleLoopClosure          // 距离 loop_search_maximum_distance 内的候选链
                                 // 链长 ≥ loop_match_minimum_chain_size
while 还有候选：
    粗匹配 LoopScanMatcher::MatchScan(..., coarse=false 细搜关)
    粗响应 > loop_match_minimum_response_coarse
    且 cov xx/yy < loop_match_maximum_variance_coarse
        细匹配 SequentialScanMatcher
        细响应 ≥ loop_match_minimum_response_fine
            SetSensorPose(bestPose)
            LinkChainToScan      // 加回环边
            CorrectPoses()       // 调 solver Compute，写回各节点 pose
            FireEndLoopClosure   // ROS 层 LoopClosureListener 发 event

误回环会把整图拧断。阈值宁高勿低。关掉 do_loop_closing 可先验证里程计+前端。

ROS 层 LoopClosureListener 挂在 FireEndLoopClosure：发 /slam_toolbox/loop_closure_event，并 requestPoseGraphPublish()。整图只在回环后发，平时用 new_node_event 做增量。

5.4 定位：ProcessLocalization（约 L2883）

前半与 Process 几乎一样（预测、HasMovedEnough、MatchScan、加顶点加边、可回环），最后：

AddScanToLocalizationBuffer(scan, vertex)

缓冲区是队列，长度 = scan_buffer_size。超出则：

• RemoveNodeFromGraph（拆邻边，并从 Ceres RemoveNode / RemoveConstraint）
• delete 旧扫描

所以定位时图上只有“旧地图节点 + 最近 N 个窗口节点”。窗口过期不影响序列化进来的底图。

ClearLocalizationBuffer 在 /initialpose 时调用，窗口清空，LastScan / RunningScans 也清。

5.5 附近重定位：ProcessAgainstNodesNearBy

FindNearByScan 按当前（指定的）位姿找最近顶点，把 RunningScans 重置成该邻域，再 MatchScan。用于：

• 续建：START_AT_GIVEN_POSE
• 定位：/initialpose 后的第一帧

5.6 匹配在搜什么

Karto 的 scan matching 是 相关网格搜索，不是 ICP 迭代最近点：

• 把参考扫描 smearing 成查找表
• 在 correlation_search_space_dimension × 角度范围内离散搜
• 粗角度步长 coarse_angle_resolution，细角度 fine_search_angle_offset
• 响应低于阈值或 use_response_expansion 时扩大搜索窗
• distance_variance_penalty / angle_variance_penalty：偏离里程计越多惩罚越大

走廊等退化场景响应会变平，匹配可能沿走廊滑。这是前端固有问题，不是 YAML 能彻底消灭的。

5.7 和 ROS 门槛的关系

shouldProcessScan 先滤一遍，HasMovedEnough 再用 minimum_travel_distance / minimum_travel_heading 滤一遍。
默认 shouldProcessScan 用 0.8 * dist²，所以多数时候 ROS 层先挡住。把 check_min_dist_and_heading_precisely 打开后，原地旋转也能过 ROS 层，再交给 Karto 的航向门槛。