【3D SLAM源码解读系列】(五) OpenKarto 源码解析：SLAM Toolbox 的二维 SLAM 核心

原创已于 2026-10-21 20:36:28 修改 · 327 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

收录于

3D SLAM源码解读系列

于 2026-10-21 20:35:39 首次发布

本文深入剖析slam_toolbox中集成的OpenKarto二维SLAM核心实现，聚焦扫描匹配（含相关栅格建模、粗/细搜索与协方差估计）、回环检测（候选链筛选、粗精匹配与约束构建）、位姿图优化（误差函数、雅可比推导、Ceres与SPA求解机制）及地图生成（射线追踪、占据判定与持久化）。内容严格基于slam_toolbox维护的Karto修改版源码，涵盖ROS 2集成、参数化设计与工程适配细节。

前言

原本这个系列都暂时搁置了，但是最近项目需要用到slam_toolbox的功能包用于2d在线slam，于是就有了本文
往期内容：
与前面介绍的三维激光 SLAM（Simultaneous Localization and Mapping，同时定位与建图）不同，Karto SLAM 面向二维激光雷达建图
- 但扫描匹配、回环检测与位姿图优化这些环节并没有因为维度降低而消失，它们反而被压缩进一个更紧凑的实现里，成为理解完整 SLAM 系统的合适切口
- 这里需要说明：本文讨论的 Karto 不是上游 OpenKarto 的原样副本，而是 slam_toolbox 随包维护并持续修改的这一份实现，后文出现的定位模式、节点裁剪、近邻查询、序列化与占据栅格参数，都以本地代码为准
本文同时参考两篇文献
- Konolige 等人的《Efficient Sparse Pose Adjustment for 2D Mapping》，用于解释位姿图优化的误差函数与稀疏求解
- 《Multi-Objective Optimization of Loop Closure Detection Parameters for Indoor 2D Simultaneous Localization and Mapping》，用于解释回环搜索半径与候选链长度这两个参数的影响
本文我们将先回顾二维 SLAM 的基本组成，再看 slam_toolbox 的工程结构，最后沿扫描匹配、回环检测、位姿图优化与地图生成四条链路逐段对照公式与源码

文章目录

1 回顾

1-1 完整的SLAM组成

请添加图片描述

一个完整的二维 SLAM 系统通常由三部分组成
- 前端：传感器数据处理、扫描匹配、位姿估计
- 后端：回环检测、位姿图优化
- 地图构建：根据优化后的位姿融合激光观测
前端回答的是"当前这一帧相对于上一帧在哪里"，它追求的是局部一致与实时性
后端回答的是"整条轨迹是否自洽"，它关心的是全局一致性，允许延迟，也允许一次性修正很多帧
地图构建则把每一帧的端点按修正后的位姿写进栅格，形成可供导航使用的占据地图
在 slam_toolbox 里，这三部分由一次 addScan 调用串起来的
下面这段代码就是整条链路的骨架

// 前端：把 ROS 消息变成带位姿的扫描，并交给 Mapper 处理
LocalizedRangeScan * range_scan = getLocalizedRangeScan(laser, scan, odom_pose);

// 前端与后端：Process 内部完成扫描匹配、图更新与回环检测
processed = smapper_->getMapper()->Process(range_scan, &covariance);

if (processed) {
  // 位姿修正结果发布为 map 到 odom 的变换
  setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose,
    scan->header.stamp, update_reprocessing_transform);

  // 地图数据：扫描进入数据集，后续用于重建占据栅格
  dataset_->Add(range_scan);

  publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
}

说人话：前端负责"这一步往哪走"，后端负责"回头看整条路走得对不对"，地图负责"把看到的东西按修正后的位置贴上去"

1-2 常见的2D SLAM

二维激光 SLAM 有几条常见路线，它们的差别主要在前端如何估计位姿、后端如何维护全局一致性

系统	前端	后端	地图表示	主要特点
`GMapping`	粒子滤波	重采样	占据栅格	每个粒子携带一张地图，计算量随粒子数增长
`Hector SLAM`	高斯牛顿扫描匹配	无显式回环	多分辨率栅格	依赖扫描匹配精度，对剧烈运动较敏感
`Cartographer`	子图内扫描匹配	子图间位姿图优化	子图与栅格	分层结构，支持二维与三维
`Karto SLAM`	相关栅格扫描匹配	位姿图优化	占据栅格	节点与约束显式建模，回环与优化边界清晰

Karto 的核心选择是位姿图，而不是粒子滤波
- 每一帧激光扫描对应图中的一个节点，节点之间用相对位姿与协方差连成边
- 前端只负责给出节点的初始位姿，后端在检测到回环时把新的边加入图中，再统一求解所有节点的修正量
- 这条路线的好处是地图不参与优化，优化规模只与节点数和边数有关
在 Mapper::Process 里，前端与后端的分界看得很清楚

// 前端：用运行扫描窗口做一次扫描匹配
if (m_pUseScanMatching->GetValue() && pLastScan != NULL) {
  response = m_pSequentialScanMatcher->MatchScan(pScan,
    m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
    bestPose, cov);
  pScan->SetSensorPose(bestPose);
}

// 后端：新节点入图、建立约束，并尝试回环
m_pGraph->AddVertex(pScan);
m_pGraph->AddEdges(pScan, cov);

if (m_pDoLoopClosing->GetValue()) {
  std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
  const_forEach(std::vector<Name>, &deviceNames)
  {
    m_pGraph->TryCloseLoop(pScan, *iter);
  }
}

从这里可以看到，MatchScan 只影响当前帧的位姿，AddEdges 与 TryCloseLoop 才会把当前帧与历史轨迹连接起来
后续几节就沿着这条分界线展开

2 slam_toolbox

2-1 介绍

请添加图片描述

slam_toolbox 是 ROS 2（Robot Operating System 2，第二代机器人操作系统）生态中常用的二维激光建图功能包
- 它提供在线异步建图、在线同步建图、lifelong 建图与纯定位四种主要运行模式，离线建图则由同步节点配合另一份参数文件完成
- 它的核心并不是从零实现的 SLAM 算法，而是随包维护的一份 Karto 实现，再加上 ROS 2 的消息接入、参数管理、坐标变换、地图发布与序列化
- 这一点很重要：我们读到的 Mapper、ScanMatcher、MapperGraph 都带着 karto 命名空间，但它们已经与上游 OpenKarto 产生了明显差异
slam_toolbox 维护的 Karto 实现不是原版 OpenKarto 的简单封装，而是在长期维护中做过适配与修改
- 因此本文分析的对象是 slam_toolbox 仓库中的 Karto 实现，类与函数的行为应以该仓库的实际源码为准
- 后文出现的定位缓冲区、节点评分、近邻查询与占据栅格参数，都属于这一份实现，而不是上游 OpenKarto 的原始行为

2-2 数据流程

四种主要模式并不是四套独立的 SLAM 实现
- 它们共享同一套 ROS 2 接口、扫描转换、坐标变换、求解器与地图发布机制
- 真正的差异集中在三处：扫描从哪个回调进入、回调与工作线程如何协作、最终调用哪个 Karto 入口

这条链路可以拆成五段
- 入口：setROSInterfaces 建立订阅与 TF 过滤器，回调统一绑定到虚函数 laserCallback
- 调度：异步、定位与 lifelong 在回调里直接处理，同步模式先入队再由独立线程取出
- 处理：addScan 把 ROS 消息转换成 LocalizedRangeScan，再按 processor_type_ 分派到不同的 Karto 入口
- Karto 内部：扫描匹配、节点入图、建边、回环检测与位姿图优化
- 输出：缓存 map 到 odom 的变换、发布位姿、更新数据集，并由后台线程周期性重建地图
四种主要模式的差异可以概括如下

模式	节点类	扫描调度	图的生命周期
在线异步	`AsynchronousSlamToolbox`	回调中直接处理	持续增长
在线同步	`SynchronousSlamToolbox`	入队后由独立线程处理	持续增长
lifelong	`LifelongSlamToolbox`	回调中直接处理，处理后再评估节点	增长并删除低分节点
纯定位	`LocalizationSlamToolbox`	回调中直接处理	只保留加载图与局部缓冲

四种模式共享同一条 Karto 主链路，差异集中在扫描调度与图的生命周期，下面直接进入它们共用的 OpenKarto 核心

3 OpenKarto

3-1 介绍

OpenKarto 是一个面向二维激光雷达 SLAM（Simultaneous Localization and Mapping，同时定位与建图）的开源算法框架，其核心思想是利用激光扫描匹配估计相对位姿，并通过位姿图优化融合多帧扫描之间的约束，从而获得全局一致的机器人轨迹与环境地图。
理解 OpenKarto 可以围绕四个问题展开：
- 前端位姿估计：如何根据激光扫描数据估计机器人位姿？
- 约束构建：如何将不同时间的扫描组织为位姿图？
- 回环检测：如何识别机器人重新到达已访问区域，并引入跨时刻约束？
- 后端优化：如何联合优化所有扫描位姿，减小累计漂移并提高全局一致性？

3-1-1 slam_toolbox与上游 OpenKarto 的差异

slam_toolbox 在 Karto SLAM 基础上发展而来，核心代码是经过修改和扩展的 Karto SDK，并增加了面向机器人应用的系统功能
与上游 OpenKarto 相比，主要扩展在五处
- ROS 2 集成与运行机制：节点、话题、服务、TF 与生命周期管理，支持同步与异步建图
- 位姿图持久化与地图管理：保存与恢复位姿图，支持续建与后续优化
- 定位与长期建图：基于已有位姿图的定位模式，以及 lifelong mapping
- 后端优化器扩展：可插拔求解器架构，支持 Ceres 等后端
- 可视化与图操作工具：RViz 插件，用于查看位姿图、调整图结构与辅助地图管理
这些差异集中在系统架构与工程能力，扫描匹配、回环约束与位姿图优化的基本原理没有改变

3-2 Mapper::Process()

理解整个 OpenKarto 的算法流程，我们可以从 Mapper::Process() 函数入手。作为 Mapper 类处理激光扫描的核心入口，该函数串联了从输入扫描校验、运动阈值筛选，到扫描匹配、位姿图构建与回环检测等一系列操作，能够帮助我们从整体上把握 OpenKarto 的工作流程。
整个函数的执行过程可以概括为：
- 扫描校验与初始化：检查输入扫描的有效性，并在首次处理时初始化 Mapper。
- 位姿预测与运动筛选：根据上一帧扫描的修正结果更新当前帧的初始位姿，并通过 HasMovedEnough() 判断机器人是否发生了足够的运动。
- 连续扫描匹配：调用 MatchScan()，根据当前扫描与历史扫描估计当前帧的位姿，并获取匹配相关的协方差。
- 位姿图构建：将当前扫描加入管理器，并通过 AddVertex() 和 AddEdges() 将其纳入位姿图。
- 回环检测：在启用回环检测时，调用 TryCloseLoop()，尝试建立当前扫描与历史扫描之间的回环约束。
- 更新扫描状态：将当前扫描设置为该传感器的最新扫描，完成本次处理。

// 处理一帧激光扫描，串联扫描匹配、位姿图构建与回环检测等流程
kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance)
{
  // 1. 检查输入扫描是否有效
  if (pScan != NULL) {
    // 获取当前扫描对应的激光雷达设备
    karto::LaserRangeFinder * pLaserRangeFinder =
      pScan->GetLaserRangeFinder();

    // 检查雷达对象及扫描数据是否合法
    if (pLaserRangeFinder == NULL || pScan == NULL ||
        pLaserRangeFinder->Validate(pScan) == false) {
      return false;
    }

    // 首次处理扫描时，根据雷达量程阈值初始化 Mapper
    if (m_Initialized == false) {
      Initialize(pLaserRangeFinder->GetRangeThreshold());
    }

    // 获取同一传感器上一帧被接受的扫描
    LocalizedRangeScan * pLastScan =
      m_pMapperSensorManager->GetLastScan(pScan->GetSensorName());

    // 2. 根据上一帧的位姿修正结果，更新当前帧的初始修正位姿
    if (pLastScan != NULL) {
      // 构造从上一帧里程计位姿到其修正位姿的坐标变换
      Transform lastTransform(
        pLastScan->GetOdometricPose(),
        pLastScan->GetCorrectedPose());

      // 将上一帧的修正关系应用到当前帧的里程计位姿
      pScan->SetCorrectedPose(
        lastTransform.TransformPose(pScan->GetOdometricPose()));
    }

    // 3. 运动阈值筛选：如果当前帧相对于上一帧的运动不足，则跳过
    if (!HasMovedEnough(pScan, pLastScan)) {
      return false;
    }

    // 初始化扫描匹配的协方差矩阵
    // 如果后续执行扫描匹配，匹配器可以进一步更新该矩阵
    Matrix3 cov;
    cov.SetToIdentity();

    // 保存扫描匹配结果及其响应分数
    Pose2 bestPose;
    kt_double response = 0.0;

    // 记录匹配前的里程计位姿，用于调试时分析匹配修正量
    Pose2 odometricPoseBeforeMatch = pScan->GetOdometricPose();

    // 4. 连续扫描匹配
    // 仅当启用了扫描匹配且存在上一帧扫描时执行
    if (m_pUseScanMatching->GetValue() && pLastScan != NULL) {
      // 将当前扫描与传感器管理器维护的运行扫描集合进行匹配
      // bestPose：匹配得到的最佳位姿
      // cov：匹配相关的协方差矩阵
      // response：匹配响应分数
      response = m_pSequentialScanMatcher->MatchScan(
        pScan,
        m_pMapperSensorManager->GetRunningScans(
          pScan->GetSensorName()),
        bestPose,
        cov);

      // 将匹配得到的位姿写入当前扫描对象的 SensorPose
      pScan->SetSensorPose(bestPose);

      // 如果调用者提供了协方差输出指针，则返回本次计算得到的矩阵
      if (covariance) {
        *covariance = cov;
      }
    }

    // 5. 将当前扫描加入传感器管理器，并分配对应的状态 ID
    m_pMapperSensorManager->AddScan(pScan);

    // 启用扫描匹配后，进一步构建位姿图
    if (m_pUseScanMatching->GetValue()) {
      // 将当前扫描添加为位姿图节点
      m_pGraph->AddVertex(pScan);

      // 根据当前扫描及协方差信息建立相关约束边
      m_pGraph->AddEdges(pScan, cov);

      // 可选调试日志：输出匹配响应及匹配位姿相对原始里程计位姿的修正量
      if (g_KartoDebugLogging) {
        kt_double dx =
          bestPose.GetX() - odometricPoseBeforeMatch.GetX();
        kt_double dy =
          bestPose.GetY() - odometricPoseBeforeMatch.GetY();
        kt_double dtheta = math::NormalizeAngle(
          bestPose.GetHeading() -
          odometricPoseBeforeMatch.GetHeading());

        std::cout << "[ScanMatcher] New Node Added | ScanID = "
          << pScan->GetStateId()
          << " | Response = " << response
          << " | Correction = (" << dx << ", "
          << dy << ", " << dtheta << ")"
          << std::endl;
      }

      // 将当前扫描加入运行扫描集合，供后续扫描匹配使用
      m_pMapperSensorManager->AddRunningScan(pScan);

      // 6. 回环检测：尝试建立当前扫描与历史扫描之间的回环约束
      if (m_pDoLoopClosing->GetValue()) {
        // 获取传感器管理器中登记的传感器名称
        std::vector<Name> deviceNames =
          m_pMapperSensorManager->GetSensorNames();

        // 遍历传感器，尝试对当前扫描执行回环检测
        const_forEach(std::vector<Name>, &deviceNames)
        {
          m_pGraph->TryCloseLoop(pScan, *iter);
        }
      }
    }

    // 7. 更新当前传感器的最后一帧扫描记录
    // 即使关闭扫描匹配，也会更新该记录
    m_pMapperSensorManager->SetLastScan(pScan);

    // 当前扫描处理成功
    return true;
  }

  // 输入扫描为空，处理失败
  return false;
}

需要注意，Mapper::Process() 主要负责组织和调度整个处理流程，并不直接实现所有算法细节。例如，扫描匹配的具体过程由 MatchScan() 完成，回环检测由 TryCloseLoop() 负责，而位姿图的全局优化还需要进一步追踪相关优化函数。

3-2-1 初始位姿

Karto 的初始位姿来自两部分
- 当前帧的里程计位姿
- 上一帧的修正量传播过来的偏移
设上一帧的里程计位姿为 $o_{i-1}$ ，修正位姿为 $c_{i-1}$ ，当前帧的里程计位姿为 $o_i$
初始修正位姿可以写成

$c_i^{-} = \left(o_{i-1} \ominus c_{i-1}\right) \oplus o_i$

其中：
- $\oplus$ ：位姿复合， $\oplus b = ab$
- $\ominus$ ：相对位姿， $\ominus b = a^{-1} \oplus b$ ，即把 $b$ 表示在 $a$ 的坐标系下

说人话：里程计提供当前帧的运动估计，上一帧的修正结果则用于补偿里程计的累计偏差。将两者结合，可以为当前帧提供更合理的初始位姿，减少扫描匹配的搜索负担。

   // 2. 根据上一帧的位姿修正结果，更新当前帧的初始修正位姿
    if (pLastScan != NULL) {
      // 构造从上一帧里程计位姿到其修正位姿的坐标变换
      Transform lastTransform(
        pLastScan->GetOdometricPose(),
        pLastScan->GetCorrectedPose());

      // 将上一帧的修正关系应用到当前帧的里程计位姿
      pScan->SetCorrectedPose(
        lastTransform.TransformPose(pScan->GetOdometricPose()));
    }

3-2-2 运动阈值检测

HasMovedEnough 决定当前帧是否值得进入后续的扫描匹配

kt_bool Mapper::HasMovedEnough(
  LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const
{
  // 首帧没有参考，直接放行
  if (pLastScan == NULL) {
    return true;
  }

  // 条件一：距离上一帧的时间足够长
  kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime();
  if (timeInterval >= m_pMinimumTimeInterval->GetValue()) {
    return true;
  }

  // 把位姿从基座坐标系换算到雷达坐标系
  Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
  Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());

  // 条件二：朝向变化足够大
  kt_double deltaHeading = math::NormalizeAngle(
    scannerPose.GetHeading() - lastScannerPose.GetHeading());
  if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) {
    return true;
  }

  // 条件三：位移足够大
  kt_double squaredTravelDistance =
    lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
  if (squaredTravelDistance >=
    math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
  {
    return true;
  }

  return false;
}

三个条件之间是“或”的关系，任一满足就会处理当前帧
- 时间条件对应 MinimumTimeInterval
- 朝向条件对应 MinimumTravelHeading
- 位移条件对应 MinimumTravelDistance
设上一帧的雷达位姿为 $p_{i-1}$ ，当前帧的雷达位姿为 $p_i$ ，则位移条件可以写成

$\|p_i-p_{i-1}\|^2 \ge d_{\min}^2-\epsilon$

其中 $d_{\min}$ 是 MinimumTravelDistance， $\epsilon$ 是 KT_TOLERANCE，用于吸收浮点误差
朝向条件为

$|\Delta\theta| \ge \theta_{\min}$

这里有两个容易混淆的参数边界
- slam_mapper.cpp 只把 ROS 参数 minimum_travel_distance 与 minimum_travel_heading 写入 Karto
- 它没有把 ROS 参数 minimum_time_interval 写入 Karto 的 MinimumTimeInterval
- 因此新建 mapper 时，MinimumTimeInterval 保持 Karto 的默认值 $3600$ 秒，时间条件通常不会触发
- 实际运行时，当前帧能否通过主要由朝向或位移条件决定
外层 shouldProcessScan 与内层 HasMovedEnough 是两道独立的门槛
- 默认配置下，外层要求时间达到 $0.5$ 秒，且位移平方达到 $0.8$ 倍的最小位移平方，约合 $0.447$ 米
- 前 5 帧还会被 scan_ctr < 5 拦住，throttle_scans 也会按抽帧间隔丢弃一部分扫描
- 内层则要求位移达到 $0.5$ 米，或朝向变化达到 $0.5$ 弧度
- 两者不是简单的包含关系，外层通过并不代表内层一定通过

3-2-3 初始化与反序列化

Process 第一次处理扫描时，m_Initialized 还是假，于是调用 Initialize
Initialize 先按当前参数创建顺序扫描匹配器，再根据 m_Deserialized 分成两条路
- 普通新建：创建 MapperSensorManager 与 MapperGraph
- 反序列化恢复：不重建传感器管理器与图，只更新运行扫描缓冲区与回环匹配器的参数

void Mapper::Initialize(kt_double rangeThreshold)
{
  if (m_Initialized) {
    return;
  }

  // 顺序扫描匹配器按当前参数创建
  if (m_pSequentialScanMatcher) {
    delete m_pSequentialScanMatcher;
  }
  m_pSequentialScanMatcher = ScanMatcher::Create(this,
    m_pCorrelationSearchSpaceDimension->GetValue(),
    m_pCorrelationSearchSpaceResolution->GetValue(),
    m_pCorrelationSearchSpaceSmearDeviation->GetValue(),
    rangeThreshold);

  if (m_Deserialized) {
    // 恢复已有图，只同步运行扫描缓冲区与回环匹配器
    m_pMapperSensorManager->SetRunningScanBufferSize(
      m_pScanBufferSize->GetValue());
    m_pMapperSensorManager->SetRunningScanBufferMaximumDistance(
      m_pScanBufferMaximumScanDistance->GetValue());
    m_pGraph->UpdateLoopScanMatcher(rangeThreshold);
  } else {
    // 新建会话，从头构造传感器管理器与位姿图
    m_pMapperSensorManager = new MapperSensorManager(
      m_pScanBufferSize->GetValue(),
      m_pScanBufferMaximumScanDistance->GetValue());
    m_pGraph = new MapperGraph(this, rangeThreshold);
  }

  m_Initialized = true;
}

LoadFromFile 读取归档后会把 m_Deserialized 置真，同时把 m_Initialized 置假
因此下一次 Initialize 会走恢复分支，不会重新构造并覆盖已经加载的位姿图
串行化与反序列化的完整流程放在 3-6-4 展开

3-3 扫描匹配

 // 4. 连续扫描匹配
    // 仅当启用了扫描匹配且存在上一帧扫描时执行
    if (m_pUseScanMatching->GetValue() && pLastScan != NULL) {
      // 将当前扫描与传感器管理器维护的运行扫描集合进行匹配
      // bestPose：匹配得到的最佳位姿
      // cov：匹配相关的协方差矩阵
      // response：匹配响应分数
      response = m_pSequentialScanMatcher->MatchScan(
        pScan,
        m_pMapperSensorManager->GetRunningScans(
          pScan->GetSensorName()),
        bestPose,
        cov);

      // 将匹配得到的位姿写入当前扫描对象的 SensorPose
      pScan->SetSensorPose(bestPose);

      // 如果调用者提供了协方差输出指针，则返回本次计算得到的矩阵
      if (covariance) {
        *covariance = cov;
      }
    }

这一节讲的是扫描匹配本身：
- 给定一段参考扫描与一个初始位姿，搜索当前帧的最优位姿
同一套 MatchScan 会在三处被复用：顺序匹配、近邻链连接与 3-4 的回环粗匹配
MatchScan 的完整实现如下

template<class T>
kt_double ScanMatcher::MatchScan(
  LocalizedRangeScan * pScan, const T & rBaseScans, Pose2 & rMean,
  Matrix3 & rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
{
  // 1. 取当前扫描的初始位姿，相关栅格将以它为中心
  Pose2 scanPose = pScan->GetSensorPose();

  // 没有有效读数时无法匹配，返回初始位姿与最大协方差
  if (pScan->GetNumberOfRangeReadings() == 0) {
    rMean = scanPose;
    rCovariance(0, 0) = MAX_VARIANCE;
    rCovariance(1, 1) = MAX_VARIANCE;
    rCovariance(2, 2) =
      4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue());
    return 0.0;
  }

  // 2. 让相关栅格覆盖以 scanPose 为中心的区域
  Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();
  Vector2<kt_double> offset;
  offset.SetX(scanPose.GetX() -
    (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution()));
  offset.SetY(scanPose.GetY() -
    (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution()));
  m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);

  // 3. 把参考扫描写进相关栅格
  AddScans(rBaseScans, scanPose.GetPosition());

  // 4. 以栅格分辨率的 2 倍作为粗搜索步长
  Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(),
    m_pSearchSpaceProbs->GetHeight());
  Vector2<kt_double> coarseSearchOffset(
    0.5 * (searchDimensions.GetX() - 1) *
    m_pCorrelationGrid->GetResolution(),
    0.5 * (searchDimensions.GetY() - 1) *
    m_pCorrelationGrid->GetResolution());
  Vector2<kt_double> coarseSearchResolution(
    2 * m_pCorrelationGrid->GetResolution(),
    2 * m_pCorrelationGrid->GetResolution());

  // 5. 粗搜索
  kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset,
    coarseSearchResolution,
    m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),
    m_pMapper->m_pCoarseAngleResolution->GetValue(),
    doPenalize, rMean, rCovariance, false);

  // 响应为 0 时逐步扩大角度搜索范围，最多扩大三次
  if (m_pMapper->m_pUseResponseExpansion->GetValue() == true) {
    if (math::DoubleEqual(bestResponse, 0.0)) {
      kt_double newSearchAngleOffset =
        m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
      for (kt_int32u i = 0; i < 3; i++) {
        newSearchAngleOffset += math::DegreesToRadians(20);

        bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset,
          coarseSearchResolution, newSearchAngleOffset,
          m_pMapper->m_pCoarseAngleResolution->GetValue(),
          doPenalize, rMean, rCovariance, false);

        if (math::DoubleEqual(bestResponse, 0.0) == false) {
          break;
        }
      }
    }
  }

  // 6. 细搜索：范围与步长都在粗结果的基础上缩小
  if (doRefineMatch) {
    Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
    Vector2<kt_double> fineSearchResolution(
      m_pCorrelationGrid->GetResolution(),
      m_pCorrelationGrid->GetResolution());
    bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset,
      fineSearchResolution,
      0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
      m_pMapper->m_pFineSearchAngleOffset->GetValue(),
      doPenalize, rMean, rCovariance, true);
  }

  return bestResponse;
}

这段代码可以分成四个阶段
- 准备阶段：读取初始位姿，处理空扫描，设置相关栅格的覆盖范围
- 粗搜索：以两倍栅格分辨率的步长枚举候选位姿，调用 CorrelateScan 评分
- 响应扩张：粗搜索响应为 $0$ 时，把角度搜索偏移增加 $20^\circ$ 后重试，最多三次
- 细搜索：doRefineMatch 为真时，在粗结果附近用更小的范围与步长再搜一次
rMean 与 rCovariance 都是输出参数
- rMean 保存搜索得到的最优位姿
- rCovariance 保存由响应分布估计出的协方差
- 返回值是本次匹配的响应分数
同一次匹配可以用不同的参数组合调用
- 顺序匹配与近邻链连接通常启用惩罚与细搜索
- 回环粗匹配只做粗搜索，把细搜索留给精匹配阶段
因此这里只讲通用的搜索、评分与协方差，回环的判定条件与全局修正留到 3-4
完整的流程如下：

3-3-1 相关栅格与可见点筛选

有了初始位姿后，OpenKarto 会利用参考扫描构建相关栅格，为后续扫描匹配提供评分依据
- 如果仅根据激光端点是否落在占据栅格中进行评分，微小的位姿偏差就可能导致端点落入相邻的空闲栅格，使匹配得分发生明显变化
为了减小这种栅格离散性带来的影响，相关栅格通过空间响应机制，将离散的激光端点扩展为周围具有不同响应值的评分区域
- 端点附近的响应值较高，距离越远通常越低
这种响应分布可以用高斯核直观表示：

$\cdot \exp\left(-\frac{\|p - p_o\|^{2}}{2\sigma_s^{2}}\right)$

其中：
- $p$ ：待评价的位置
- $p_o$ ：端点的位置或其对应栅格的中心
- $\sigma_s$ ：响应的空间扩散范围
这样，即使候选位姿存在少量偏差，扫描端点仍可能获得一定的响应值，使匹配评分对微小的位置变化不那么敏感
两种评分方式的差异可以用下图直观对比

请添加图片描述

图中以单个端点为例，抹开标准差取 slam_toolbox 默认的 0.1 米，按 0.05 米一格显示（顺序匹配器的相关栅格真实分辨率为 0.01 米），对比占据栅格的阶梯式评分与相关栅格的高斯响应：后者在端点偏移 $0.05$ 米（图中一格）时仍有约 88 的响应，并随偏移逐渐衰减，在 $±2σs \pm 2\sigma_s$ 大约 $±0.2 \pm 0.2$ 米处截断，边界上的核值仍有约 $14$

说人话：相比直接用只有“命中”和“未命中”两种结果的栅格进行匹配，OpenKarto 通过平滑响应，让激光点即使有轻微偏移也能获得一定分数，避免微小的位置变化导致匹配得分突然下降。

virtual void CalculateKernel()
{
  kt_double resolution = GetResolution();

  // 抹开范围限制在两个标准差以内
  m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
  m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];

  kt_int32s halfKernel = m_KernelSize / 2;
  for (kt_int32s i = -halfKernel; i <= halfKernel; i++) {
    for (kt_int32s j = -halfKernel; j <= halfKernel; j++) {
      kt_double distanceFromMean = hypot(i * resolution, j * resolution);

      // 高斯核，归一化到 0 到 100
      kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
      kt_int32u kernelValue =
        static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));

      int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
      m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
    }
  }
}

端点写入栅格后立即调用 SmearPoint
SmearPoint 只在端点确实占据该栅格时才扩散，并且逐格取最大值
这样相关栅格的峰值仍然对应端点本身，邻域只是提供梯度，不会把峰值搬走

inline void SmearPoint(const Vector2<kt_int32s> & rGridPoint)
{
  int gridIndex = GridIndex(rGridPoint);
  if (GetDataPointer()[gridIndex] != GridStates_Occupied) {
    return;
  }

  kt_int32s halfKernel = m_KernelSize / 2;
  for (kt_int32s j = -halfKernel; j <= halfKernel; j++) {
    kt_int8u * pGridAdr =
      GetDataPointer(Vector2<kt_int32s>(rGridPoint.GetX(), rGridPoint.GetY() + j));

    kt_int32s kernelConstant = halfKernel + m_KernelSize * (j + halfKernel);
    for (kt_int32s i = -halfKernel; i <= halfKernel; i++) {
      kt_int8u kernelValue = m_pKernel[i + kernelConstant];

      // 取最大值，避免多个端点重复累加
      if (kernelValue > pGridAdr[i]) {
        pGridAdr[i] = kernelValue;
      }
    }
  }
}

相关栅格建好之后，并不是所有端点都参与评分
- 对于不是全向的雷达，扫描端点里有一部分落在机器人背后或视野之外，这些点在当前视角下不可见
- Karto 用相邻端点的叉积方向判断可见性
设视点为 $v$ ，参考端点为 $f$ ，当前端点为 $p$ ，则

$s = (v_y - f_y)\,p_x + (f_x - v_x)\,p_y + (f_y v_x - f_x v_y)$

当 $s < 0$ 时，当前端点位于视点背侧，跳过；否则保留
代码用一个滞后迭代器实现：只有连续落在可见一侧的点才写入有效点集

PointVectorDouble ScanMatcher::FindValidPoints(
  LocalizedRangeScan * pScan,
  const Vector2<kt_double> & rViewPoint) const
{
  const PointVectorDouble & rPointReadings = pScan->GetPointReadings();

  // 两个端点距离太近时方向判断不可靠
  const kt_double minSquareDistance = math::Square(0.1);

  PointVectorDouble::const_iterator trailingPointIter = rPointReadings.begin();
  PointVectorDouble validPoints;

  Vector2<kt_double> firstPoint;
  kt_bool firstTime = true;

  const_forEach(PointVectorDouble, &rPointReadings)
  {
    Vector2<kt_double> currentPoint = *iter;

    if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY())) {
      firstPoint = currentPoint;
      firstTime = false;
    }

    Vector2<kt_double> delta = firstPoint - currentPoint;
    if (delta.SquaredLength() > minSquareDistance) {
      // 叉积的符号决定端点位于视点的哪一侧
      double a = rViewPoint.GetY() - firstPoint.GetY();
      double b = firstPoint.GetX() - rViewPoint.GetX();
      double c = firstPoint.GetY() * rViewPoint.GetX() -
        firstPoint.GetX() * rViewPoint.GetY();
      double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;

      firstPoint = currentPoint;

      if (ss < 0.0) {
        // 背侧点，跳过
        trailingPointIter = iter;
      } else {
        for (; trailingPointIter != iter; ++trailingPointIter) {
          validPoints.push_back(*trailingPointIter);
        }
      }
    }
  }

  return validPoints;
}

这个筛选对二维雷达尤其重要
- 如果雷达视角小于 $180^\circ$ ，背面没有读数，筛选不会改变结果
- 如果雷达是 $270^\circ$ 或接近全向，背面读数可能来自机身穿透或镜面反射，筛掉它们能减少错误匹配

3-3-2 搜索空间、粗搜索与响应评分

在获得初始位姿、构建相关栅格并筛选有效端点之后，扫描匹配进入搜索阶段
- Karto 不会在整个空间中寻找最优位姿，而是围绕初始位姿，在一个有限的三维搜索空间内枚举候选位姿
- 搜索分两级：粗搜索用较大的步长遍历候选位姿，快速定位响应较高的区域；细搜索在粗搜索结果的邻域内缩小范围、减小步长，进一步修正位姿
- 每个候选位姿都根据相关栅格计算响应，叠加距离与角度惩罚后取得分最高者
本节展开粗搜索的空间、候选枚举、响应评分与惩罚，最优候选的选取、细搜索与协方差放在 3-3-3
搜索空间是一个三维栅格： $x$ 、 $y$ 、 $\theta$
给定搜索中心 $c_0$ 与搜索半径，候选位姿集合为

$\mathcal{S} = \left\{ c_0 \oplus (\delta x, \delta y, \delta\theta) \;\middle|\; \delta x \in [-o_x, o_x],\ \delta y \in [-o_y, o_y],\ \delta\theta \in [-\Delta\theta, \Delta\theta] \right\}$

离散化之后，每个方向的候选数为

$n_x = \mathrm{round}\left(\frac{2o_x}{r_x}\right) + 1,\quad n_y = \mathrm{round}\left(\frac{2o_y}{r_y}\right) + 1,\quad n_\theta = \mathrm{round}\left(\frac{2\Delta\theta}{r_\theta}\right) + 1$

总候选数为 $n_x n_y n_\theta$

下图把三维搜索空间拆成平移与朝向两个视图，并给出粗搜索的候选数量

请添加图片描述

默认配置下，粗搜索的位置范围是 $±0.25 \pm 0.25$ 米、步长 $0.02$ 米，朝向范围是 $±20∘ \pm 20^\circ$ 、步长 $2^\circ$
- 平移方向各有 $26$ 个候选，朝向有 $21$ 个，粗搜索共 $26 \times 26 \times 21 = 14196$ 个候选
- 这个数量看起来不小，但评分按行并行执行，而且只在顺序匹配与回环匹配需要时触发
对每个候选位姿，把扫描端点变换到栅格坐标系，累加对应栅格值，再除以偏移数组长度与占据值 $100$

$\frac{1}{N \cdot 100}\sum_{k=1}^{N} M\left(T(c)\,q_k\right)$

其中：
- $q_k$ ：第 $k$ 个端点在扫描坐标系下的坐标，落在栅格外时该项记为 $0$
- $N$ ：该角度下偏移数组的长度，等于扫描端点数
- $T (c)$ ：由候选位姿确定的变换
如果直接取最大值，搜索会偏向离初始位姿较远的候选，因为远处偶尔会碰到高分栅格
Karto 在评分上再乘两个惩罚项，把搜索拉回里程计附近

$\eta_d = \max\left(1 - \frac{0.2\,\|\delta p\|^{2}}{\sigma_d^{2}},\ \eta_d^{\min}\right)$

$\eta_\theta = \max\left(1 - \frac{0.2\,\delta\theta^{2}}{\sigma_\theta^{2}},\ \eta_\theta^{\min}\right)$

最终得分为

$S(c)\,\eta_d\,\eta_\theta$

Karto 的默认值是 $\sigma_d^{2} = 0.09$ 平方米与 $\sigma_\theta^{2} \approx 0.122$ 弧度平方（ $20^\circ$ 换算成弧度后平方），两个下限分别是 $0.5$ 与 $0.9$
slam_toolbox 把 $\sigma_d^{2}$ 改成 $0.5$ 平方米、 $\sigma_\theta^{2}$ 改成 $1.0$ 弧度平方，代码里的距离下限是 $0.05$
随包提供的 YAML 又把距离下限覆盖回 $0.5$ ；即便如此，距离方差从 $0.09$ 放大到 $0.5$ 后惩罚衰减更慢，默认配置下的搜索仍比原版 Karto 更宽松
如果粗搜索的最高响应为零，说明所有候选位姿下都没有端点落在有效响应区域
- use_response_expansion 为真时，Karto 会把角度搜索范围依次扩大 $20^\circ$ ，最多尝试三次
- 这个机制只在初始化或里程计严重跳变时才会触发，正常运行时不增加开销

kt_double ScanMatcher::CorrelateScan(
  LocalizedRangeScan * pScan, const Pose2 & rSearchCenter,
  const Vector2<kt_double> & rSearchSpaceOffset,
  const Vector2<kt_double> & rSearchSpaceResolution,
  kt_double searchAngleOffset, kt_double searchAngleResolution,
  kt_bool doPenalize, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doingFineMatch)
{
  // 预计算每个角度下所有端点的栅格偏移
  m_pGridLookup->ComputeOffsets(pScan,
    rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);

  // 位置搜索数组
  m_xPoses.clear();
  kt_int32u nX = static_cast<kt_int32u>(
    math::Round(rSearchSpaceOffset.GetX() * 2.0 / rSearchSpaceResolution.GetX()) + 1);
  kt_double startX = -rSearchSpaceOffset.GetX();
  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) {
    m_xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX());
  }

  m_yPoses.clear();
  kt_int32u nY = static_cast<kt_int32u>(
    math::Round(rSearchSpaceOffset.GetY() * 2.0 / rSearchSpaceResolution.GetY()) + 1);
  kt_double startY = -rSearchSpaceOffset.GetY();
  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) {
    m_yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY());
  }

  // 角度搜索数组
  kt_int32u nAngles = static_cast<kt_int32u>(
    math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);

  kt_int32u poseResponseSize =
    static_cast<kt_int32u>(m_xPoses.size() * m_yPoses.size() * nAngles);
  m_pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];

  m_rSearchCenter = rSearchCenter;
  m_searchAngleOffset = searchAngleOffset;
  m_nAngles = nAngles;
  m_searchAngleResolution = searchAngleResolution;
  m_doPenalize = doPenalize;

  // 按 y 行并行评分
  tbb::parallel_for_each(m_yPoses, (*this));
}

并行粒度选在 $y$ 方向，而不是每个候选位姿一个任务
- 这样每个任务内部可以连续访问同一行栅格，缓存命中率更高
每个任务把结果写到 m_pPoseResponse 的固定区间，任务之间没有写冲突
评分函数本身很短，主要开销在内存访问与三角函数预处理上

void ScanMatcher::operator()(const kt_double & y) const
{
  kt_int32u y_pose = std::find(m_yPoses.begin(), m_yPoses.end(), y) - m_yPoses.begin();
  const kt_int32u size_x = m_xPoses.size();

  // 外层遍历 y 偏移，内层遍历 x 偏移，构成平移候选
  kt_double newPositionY = m_rSearchCenter.GetY() + y;
  kt_double squareY = math::Square(y);

  for (std::vector<kt_double>::const_iterator xIter = m_xPoses.begin();
    xIter != m_xPoses.end(); ++xIter)
  {
    kt_int32u x_pose = std::distance(m_xPoses.begin(), xIter);
    kt_double x = *xIter;
    kt_double newPositionX = m_rSearchCenter.GetX() + x;
    kt_double squareX = math::Square(x);

    Vector2<kt_int32s> gridPoint =
      m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(newPositionX, newPositionY));
    kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);

    kt_double startAngle =
      m_rSearchCenter.GetHeading() - m_searchAngleOffset;

    for (kt_int32u angleIndex = 0; angleIndex < m_nAngles; angleIndex++) {
      kt_double angle = startAngle + angleIndex * m_searchAngleResolution;
      // 当前平移与角度候选的相关栅格响应
      kt_double response = GetResponse(angleIndex, gridIndex);

      if (m_doPenalize && (math::DoubleEqual(response, 0.0) == false)) {
        // 距离惩罚
        kt_double squaredDistance = squareX + squareY;
        kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN *
          squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue());
        distancePenalty = math::Maximum(distancePenalty,
          m_pMapper->m_pMinimumDistancePenalty->GetValue());

        // 角度惩罚
        kt_double squaredAngleDistance =
          math::Square(angle - m_rSearchCenter.GetHeading());
        kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN *
          squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue());
        anglePenalty = math::Maximum(anglePenalty,
          m_pMapper->m_pMinimumAnglePenalty->GetValue());

        response *= (distancePenalty * anglePenalty);
      }

      kt_int32u poseResponseCounter =
        (y_pose * size_x + x_pose) * m_nAngles + angleIndex;
      m_pPoseResponse[poseResponseCounter] =
        std::pair<kt_double, Pose2>(response,
          Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle)));
    }
  }
}

GetResponse 使用预计算的偏移数组，逐个端点取栅格值
如果端点落在栅格外，或者该角度下没有有效偏移，直接跳过
最后除以偏移数组长度与 $100$ ，把响应归一化到 $[0, 1]$

kt_double ScanMatcher::GetResponse(
  kt_int32u angleIndex, kt_int32s gridPositionIndex) const
{
  kt_double response = 0.0;
  kt_int8u * pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex;

  const LookupArray * pOffsets = m_pGridLookup->GetLookupArray(angleIndex);
  kt_int32u nPoints = pOffsets->GetSize();
  if (nPoints == 0) {
    return response;
  }

  kt_int32s * pAngleIndexPointer = pOffsets->GetArrayPointer();
  for (kt_int32u i = 0; i < nPoints; i++) {
    kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i];

    // 越界或无效偏移直接跳过
    if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) ||
      pAngleIndexPointer[i] == INVALID_SCAN)
    {
      continue;
    }

    response += pByte[pAngleIndexPointer[i]];
  }

  // 归一化到 0 到 1
  response /= (nPoints * GridStates_Occupied);
  return response;
}

注意这里的分母是 $\cdot 100$ ，不是有效点数
如果有端点落到栅格外被跳过，响应会偏低
这正是相关栅格四周要留边距的原因：正常搜索范围内不应该有端点越界

3-3-3 最优候选、细搜索与协方差

粗搜索给出的是离散栅格上的最优候选，在需要精匹配的调用中，扫描匹配还会在这个结果附近做一次细搜索
- 以粗搜索得到的最优位姿为中心，重新设置一个更小的搜索空间：位置范围收缩为粗搜索位置步长的一半，朝向范围收缩为粗搜索角度步长的一半
- 在这个邻域内用更小的步长重新枚举候选，默认位置范围 $±0.01 \pm 0.01$ 米、步长 $0.01$ 米，朝向范围 $\pm 1^\circ$ 、步长 $0.2^\circ$
- 细搜索只有 $\times 3 \times 11 = 99$ 个候选，用来在粗结果附近进一步修正位姿，而不是重新做一次大范围搜索
细搜索的中心来自粗搜索选出的最优候选，因此先看粗搜索如何确定这个中心
粗搜索结束后，先找最高响应
- 如果有多个候选并列最高，不能随便取第一个
Karto 对所有并列候选取平均：位置直接求算术平均，朝向用圆周平均

$\bar{\theta} = \operatorname{atan2}\left(\sum_i \sin\theta_i,\ \sum_i \cos\theta_i\right)$

这样处理后，离散搜索的量化误差会被摊平，位姿不会卡在某个栅格边界上

// 找最高响应
kt_double bestResponse = -1;
for (kt_int32u i = 0; i < poseResponseSize; i++) {
  bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first);
}

// 对所有并列最高的位姿取平均
Vector2<kt_double> averagePosition;
kt_double thetaX = 0.0;
kt_double thetaY = 0.0;
kt_int32s averagePoseCount = 0;

for (kt_int32u i = 0; i < poseResponseSize; i++) {
  if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) {
    averagePosition += m_pPoseResponse[i].second.GetPosition();

    kt_double heading = m_pPoseResponse[i].second.GetHeading();
    thetaX += cos(heading);
    thetaY += sin(heading);
    averagePoseCount++;
  }
}

Pose2 averagePose;
if (averagePoseCount > 0) {
  averagePosition /= averagePoseCount;
  thetaX /= averagePoseCount;
  thetaY /= averagePoseCount;
  averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
}

得到粗搜索的中心位姿后，MatchScan 会以它为搜索中心再调用一次 CorrelateScan，并把这次调用标记为细匹配
- 细搜索不会重新分配相关栅格，只在更小的邻域内重新计算响应数组，因此开销远小于粗搜索
- 细搜索同样会处理并列最高的情况，并把结果写回 rMean，作为最终的匹配位姿
- 位置协方差仍由粗搜索的响应分布估计，朝向协方差则在细搜索的角度维度上单独统计
位姿只是扫描匹配的一半输出，另一半是协方差
- 协方差决定了这条约束在后续优化中的权重：协方差越小，约束越硬
Karto 用最优位姿附近的响应分布来估计位置协方差
只统计响应不低于 $S_{\max} - 0.1$ 的候选，并按响应加权

$\Sigma_{xy} = \frac{1}{W}\sum_{i} w_i \begin{bmatrix} (x_i - \bar{x})^{2} & (x_i - \bar{x})(y_i - \bar{y}) \\ (x_i - \bar{x})(y_i - \bar{y}) & (y_i - \bar{y})^{2} \end{bmatrix}$

其中 $w_i$ 是该候选的响应， $\sum_i w_i$
为了不让约束过紧，协方差还要设下限

$\Sigma_{xx} \leftarrow \max\left(\Sigma_{xx},\ 0.1\,r_x^{2}\right),\quad \Sigma_{yy} \leftarrow \max\left(\Sigma_{yy},\ 0.1\,r_y^{2}\right)$

最后再按最优响应的倒数放大：响应越差，协方差越大

$\Sigma_{xy} \leftarrow \frac{1}{S_{\max}}\Sigma_{xy}$

说人话：除了确定匹配后的位姿，Karto 还会估计这次匹配有多可靠，并通过协方差告诉图优化：这条位姿约束应该有多大的可信度。匹配越稳定，约束通常越可靠；匹配越模糊，不确定性就越大。同时，Karto 会设置协方差下限，避免估计结果过于乐观，让图优化过度相信某一条约束。

void ScanMatcher::ComputePositionalCovariance(
  const Pose2 & rBestPose, kt_double bestResponse,
  const Pose2 & rSearchCenter,
  const Vector2<kt_double> & rSearchSpaceOffset,
  const Vector2<kt_double> & rSearchSpaceResolution,
  kt_double searchAngleResolution, Matrix3 & rCovariance)
{
  rCovariance.SetToIdentity();

  // 响应太低，直接给最大方差
  if (bestResponse < KT_TOLERANCE) {
    rCovariance(0, 0) = MAX_VARIANCE;
    rCovariance(1, 1) = MAX_VARIANCE;
    rCovariance(2, 2) = 4 * math::Square(searchAngleResolution);
    return;
  }

  kt_double accumulatedVarianceXX = 0;
  kt_double accumulatedVarianceXY = 0;
  kt_double accumulatedVarianceYY = 0;
  kt_double norm = 0;

  kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
  kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();

  kt_double offsetX = rSearchSpaceOffset.GetX();
  kt_double offsetY = rSearchSpaceOffset.GetY();

  for (kt_int32u yIndex = 0;
    yIndex < math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1;
    yIndex++)
  {
    kt_double y = -offsetY + yIndex * rSearchSpaceResolution.GetY();

    for (kt_int32u xIndex = 0;
      xIndex < math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1;
      xIndex++)
    {
      kt_double x = -offsetX + xIndex * rSearchSpaceResolution.GetX();

      Vector2<kt_int32s> gridPoint =
        m_pSearchSpaceProbs->WorldToGrid(
          Vector2<kt_double>(rSearchCenter.GetX() + x,
          rSearchCenter.GetY() + y));
      kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint));

      // 只统计接近最优响应的候选
      if (response >= (bestResponse - 0.1)) {
        norm += response;
        accumulatedVarianceXX += (math::Square(x - dx) * response);
        accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
        accumulatedVarianceYY += (math::Square(y - dy) * response);
      }
    }
  }

  if (norm > KT_TOLERANCE) {
    kt_double varianceXX = accumulatedVarianceXX / norm;
    kt_double varianceXY = accumulatedVarianceXY / norm;
    kt_double varianceYY = accumulatedVarianceYY / norm;
    kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);

    // 方差下限，避免约束过紧
    kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
    kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
    varianceXX = math::Maximum(varianceXX, minVarianceXX);
    varianceYY = math::Maximum(varianceYY, minVarianceYY);

    // 响应越差，方差越大
    kt_double multiplier = 1.0 / bestResponse;
    rCovariance(0, 0) = varianceXX * multiplier;
    rCovariance(0, 1) = varianceXY * multiplier;
    rCovariance(1, 0) = varianceXY * multiplier;
    rCovariance(1, 1) = varianceYY * multiplier;
    rCovariance(2, 2) = varianceTHTH;
  }

  if (math::DoubleEqual(rCovariance(0, 0), 0.0)) {
    rCovariance(0, 0) = MAX_VARIANCE;
  }
  if (math::DoubleEqual(rCovariance(1, 1), 0.0)) {
    rCovariance(1, 1) = MAX_VARIANCE;
  }
}

角度协方差在精匹配阶段单独计算
- 它沿用位置协方差的对角块，再用最优角度附近的响应分布估计朝向方差
- 如果精匹配没有算出有效值，朝向方差回退为 $1000 r^2$ ；有效但响应分布退化为零时，至少抬到一个角度步长的平方，位置方差始终保留粗匹配结果
这种"粗匹配给位置、精匹配给朝向"的分工，与两级搜索的结构是对应的

void ScanMatcher::ComputeAngularCovariance(
  const Pose2 & rBestPose, kt_double bestResponse,
  const Pose2 & rSearchCenter, kt_double searchAngleOffset,
  kt_double searchAngleResolution, Matrix3 & rCovariance)
{
  // 注意：这里不清空协方差，保留粗匹配算出的位置块
  kt_double bestAngle = math::NormalizeAngleDifference(
    rBestPose.GetHeading(), rSearchCenter.GetHeading());

  kt_double accumulatedVarianceThTh = 0;
  kt_double norm = 0;

  kt_int32u nAngles = static_cast<kt_int32u>(
    math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);

  kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;

  // 最优位置处的栅格索引，整个循环复用
  Vector2<kt_int32s> gridPoint =
    m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
  kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);

  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++) {
    kt_double angle = startAngle + angleIndex * searchAngleResolution;
    kt_double response = GetResponse(angleIndex, gridIndex);

    if (response >= (bestResponse - 0.1)) {
      norm += response;
      accumulatedVarianceThTh +=
        math::Square(angle - bestAngle) * response;
    }
  }

  if (norm > KT_TOLERANCE) {
    // 响应分布过于集中时，至少给一个搜索步长的方差
    if (accumulatedVarianceThTh < KT_TOLERANCE) {
      accumulatedVarianceThTh = math::Square(searchAngleResolution);
    }

    rCovariance(2, 2) = accumulatedVarianceThTh / norm;
  } else {
    // 回退值，避免角度约束退化
    rCovariance(2, 2) = 1000 * math::Square(searchAngleResolution);
  }
}

协方差不是越小越好
- 如果扫描匹配把协方差估得过小，回环边会变得过硬，优化时会把整条轨迹拉向一个可能有误的回环
代码里的方差下限与响应倒数放大，都是为了防止这种过度自信
- 反过来，如果响应很差，协方差会被放大到 MAX_VARIANCE，这条边在优化中几乎不起作用

3-3-4 多约束融合与连边

匹配给出当前扫描与参考扫描之间的位姿及协方差后，AddEdges 以此建立多条约束，并融合部分匹配结果
建边与位姿融合不是一回事，并非每建一条边就产生一个候选位姿
- 与上一帧的连接只建立一条顺序边，不会向候选数组中加入新的位姿
- 与运行扫描窗口的连接只连到窗口内离当前帧最近的一帧，然后把当前帧位姿及其协方差加入候选数组一次
- 近邻链匹配会遍历多条近邻链，每条链在响应超过阈值时建立一条边，并把各自的 bestPose 与协方差加入候选数组
- 首帧与其他传感器首帧匹配时，每个候选传感器各建立一条边，响应超过阈值的结果同样加入候选数组

void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovariance)
{
  MapperSensorManager * pSensorManager = m_pMapper->m_pMapperSensorManager;
  const Name rSensorName = pScan->GetSensorName();

  // 与上一帧连接
  kt_int32s previousScanNum = pScan->GetStateId() - 1;
  if (pSensorManager->GetLastScan(rSensorName) != NULL) {
    LocalizedRangeScan * pPrevScan =
      pSensorManager->GetScan(rSensorName, previousScanNum);
    if (!pPrevScan) {
      return;
    }
    LinkScans(pPrevScan, pScan, pScan->GetSensorPose(), rCovariance);
  }

  Pose2Vector means;
  std::vector<Matrix3> covariances;

  if (pSensorManager->GetLastScan(rSensorName) == NULL) {
    // 首帧：与其他传感器的首帧建立连接
    std::vector<Name> deviceNames = pSensorManager->GetSensorNames();
    forEach(std::vector<Name>, &deviceNames)
    {
      const Name & rCandidateSensorName = *iter;
      if ((rCandidateSensorName == rSensorName) ||
        (pSensorManager->GetScans(rCandidateSensorName).empty()))
      {
        continue;
      }

      Pose2 bestPose;
      Matrix3 covariance;
      kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(
        pScan, pSensorManager->GetScans(rCandidateSensorName),
        bestPose, covariance);
      LinkScans(pSensorManager->GetScan(rCandidateSensorName, 0),
        pScan, bestPose, covariance);

      if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue()) {
        means.push_back(bestPose);
        covariances.push_back(covariance);
      }
    }
  } else {
    // 与运行扫描窗口连接
    Pose2 scanPose = pScan->GetSensorPose();
    means.push_back(scanPose);
    covariances.push_back(rCovariance);
    LinkChainToScan(pSensorManager->GetRunningScans(rSensorName),
      pScan, scanPose, rCovariance);
  }

  // 与其他近邻链连接
  LinkNearChains(pScan, means, covariances);

  // 按协方差加权融合所有候选位姿
  if (!means.empty()) {
    pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
  }
}

ComputeWeightedMean 对候选数组做加权融合，位置分量由协方差加权，朝向单独做圆周平均

$\Sigma = \left(\sum_i \Sigma_i^{-1}\right)^{-1},\qquad\bar{c} = \Sigma \sum_i \Sigma_i^{-1} c_i$

其中， $i$ 遍历候选数组

Pose2 MapperGraph::ComputeWeightedMean(
  const Pose2Vector & rMeans,
  const std::vector<Matrix3> & rCovariances) const
{
  std::vector<Matrix3> inverses;
  Matrix3 sumOfInverses;

  // 累加各候选协方差的逆矩阵
  const_forEach(std::vector<Matrix3>, &rCovariances)
  {
    Matrix3 inverse = iter->Inverse();
    inverses.push_back(inverse);
    sumOfInverses += inverse;
  }

  Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();

  Pose2 accumulatedPose;
  kt_double thetaX = 0.0;
  kt_double thetaY = 0.0;

  Pose2Vector::const_iterator meansIter = rMeans.begin();
  const_forEach(std::vector<Matrix3>, &inverses)
  {
    Pose2 pose = *meansIter;
    kt_double angle = pose.GetHeading();
    thetaX += cos(angle);
    thetaY += sin(angle);

    // 位置分量按信息矩阵加权累加
    Matrix3 weight = inverseOfSumOfInverses * (*iter);
    accumulatedPose += weight * pose;
    ++meansIter;
  }

  // 朝向做圆周平均
  thetaX /= rMeans.size();
  thetaY /= rMeans.size();
  accumulatedPose.SetHeading(atan2(thetaY, thetaX));

  return accumulatedPose;
}

3-4 回环检测

 // 6. 回环检测：尝试建立当前扫描与历史扫描之间的回环约束
      if (m_pDoLoopClosing->GetValue()) {
        // 获取传感器管理器中登记的传感器名称
        std::vector<Name> deviceNames =
          m_pMapperSensorManager->GetSensorNames();

        // 遍历传感器，尝试对当前扫描执行回环检测
        const_forEach(std::vector<Name>, &deviceNames)
        {
          m_pGraph->TryCloseLoop(pScan, *iter);
        }
      }

回环检测要解决两件事：判断当前扫描是否回到了历史某处，以及在成立时把这段误差摊回整条轨迹
- 前者是候选筛选与扫描匹配问题，后者是位姿图优化问题
Karto 把回环检测拆成四步，四步按顺序串联
- 先在历史扫描中找出可能构成回环的候选链
- 再用大范围、粗分辨率的匹配判断"这里有没有可能"
- 然后在粗匹配结果附近做一次小范围、细分辨率的匹配
- 最后建立回环边，调用位姿图优化修正整条轨迹
下面的动图把四步连起来演示了一遍
- 橙色点是历史候选链，空心圆是被排除的近邻扫描
- 蓝色扫描先在 $8$ 米见方的窗口里粗扫，再在最优位置附近细扫
- 绿色圆环标出最后建立的回环边，地图中的重影在修正后收拢

请添加图片描述

动图使用合成的房间与扫描数据，参数取 slam_toolbox humble 的默认值
- 重点是把四步的衔接和搜索规模的差异画出来，不代表某一组真实数据的精度
读图时可以对照后文的四个小节
- 3-4-1 对应候选链的展开与近邻扫描的排除
- 3-4-2 对应粗匹配扫过 $81 \times 81 \times 21$ 个候选、约 $13.8$ 万次评分的过程
- 3-4-3 对应精匹配的 $\times 3 \times 11 = 99$ 个候选
- 3-4-4 对应回环边的建立与 CorrectPoses 的全局修正

3-4-1 候选链搜索

回环检测先筛选历史候选，不直接匹配
- Karto 按历史编号遍历，把位于搜索半径内、且未与当前帧近邻相连的连续扫描组织成候选链
- 候选链只圈定搜索范围，不是匹配时直接比较的对象，从而避免遍历所有历史扫描

思考一下，如果直接匹配所有历史候选，将会是灾难级别的

候选链的搜索由两个参数控制
- loop_search_maximum_distance：当前帧与历史扫描重心之间的最大距离，记作 $r$ ，默认 $3$ 米
- loop_match_minimum_chain_size：候选链的最短长度，记作 $g$
构建过程
- 按历史编号逐帧遍历，计算每帧扫描重心与当前帧重心的距离
- 距离小于 $r$ ，且该帧没有通过位姿图与当前帧附近相连，则加入当前链
- 距离小于 $r$ ，但该帧已与当前帧附近的轨迹相连，则清空整条链，而不只是跳过这一帧，避免把来时的路径误认为回环
- 距离超出 $r$ 时，连续段结束：链长达到 $g$ 就返回该链，否则清空并继续搜索
- 如果遍历到最后一帧链仍未结束，函数会在循环之后直接返回它，不再补做最小长度检查
因此，链长条件只在“距离超出半径”这一分支生效，不能理解为候选链一旦构建就一定满足最小长度

LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(
  LocalizedRangeScan * pScan,
  const Name & rSensorName,
  kt_int32u & rStartNum)
{
  LocalizedRangeScanVector chain;

  Pose2 pose = pScan->GetReferencePose(
    m_pMapper->m_pUseScanBarycenter->GetValue());

  // 与当前帧已有路径相连的扫描不能进入候选链
  const LocalizedRangeScanVector nearLinkedScans =
    FindNearLinkedScans(pScan,
      m_pMapper->m_pLoopSearchMaximumDistance->GetValue());

  kt_int32u nScans = static_cast<kt_int32u>(
    m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());

  for (; rStartNum < nScans; rStartNum++) {
    LocalizedRangeScan * pCandidateScan =
      m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum);

    if (pCandidateScan == NULL) {
      continue;
    }

    Pose2 candidateScanPose = pCandidateScan->GetReferencePose(
      m_pMapper->m_pUseScanBarycenter->GetValue());

    kt_double squaredDistance =
      candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());

    if (squaredDistance <
      math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
    {
      // 近邻相连的扫描会让当前链失效
      if (find(nearLinkedScans.begin(), nearLinkedScans.end(),
        pCandidateScan) != nearLinkedScans.end())
      {
        chain.clear();
      } else {
        chain.push_back(pCandidateScan);
      }
    } else {
      // 离开搜索半径后，链长足够才返回
      if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) {
        return chain;
      } else {
        chain.clear();
      }
    }
  }

  return chain;
}

距离使用扫描端点重心，而不是激光设备位姿
- 重心更接近扫描覆盖区域的中心，比设备位置更能代表这段观测的位置
- 机器人原地旋转时，设备位置不变，重心会随视野变化，候选链的划分更合理
“近邻相连”指位姿图上的连通关系，不是欧氏距离
- 两帧空间上可能很近，但图上已通过一串边连在一起，说明这是来时的路
- 这种判断无法用纯半径搜索替代，因此必须先有图再找链

3-4-2 粗匹配

找到候选链后，Karto 会利用回环匹配器，在候选历史扫描范围内依次尝试匹配，寻找当前扫描与历史扫描之间可能成立的相对位姿。
- 回环匹配器的搜索空间边长 $8$ 米（LoopSearchSpaceDimension），以当前帧位姿为中心，所以位置搜索范围是 $\pm4$ 米；搜索空间分辨率 $0.05$ 米，相关栅格在此基础上四周还要按量程留出边距；角度范围 $±20∘ \pm 20^\circ$ ，角度步长 $2^\circ$
- 注意： $8\text{ m}$ 是搜索窗口的边长，并非允许的位姿漂移量。候选链的筛选半径 $r$ 则用于前一步的历史扫描筛选，两者作用不同。
- 粗搜索的位置步长是分辨率的两倍，即 $0.1$ 米
- 搜索概率栅格每条边有 $8/0.05 + 1 = 161$ 个单元，但粗搜索每隔一个单元取一个候选，所以每个平移方向是 $81$ 个候选，而不是 $161$ 个
因此粗搜索的候选数为

$n_x = \mathrm{round}\left(\frac{2 \times 4.0}{0.1}\right) + 1 = 81,\quad n_y = 81,\quad n_\theta = \mathrm{round}\left(\frac{2 \times 20^\circ}{2^\circ}\right) + 1 = 21$

总候选数约 $13.8$ 万
- 这个数量看起来很大，但只在候选链存在时才会触发，而且评分是并行的
粗匹配只做粗搜索，不做精搜索，也不加距离与角度惩罚，只求快速判断"这里有没有可能"
通过条件是三个不等式
- 粗响应大于 loop_match_minimum_response_coarse
- 两个方向的协方差都小于 loop_match_maximum_variance_coarse

kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSensorName)
{
  kt_bool loopClosed = false;
  kt_int32u scanIndex = 0;

  LocalizedRangeScanVector candidateChain =
    FindPossibleLoopClosure(pScan, rSensorName, scanIndex);

  while (!candidateChain.empty()) {
    Pose2 bestPose;
    Matrix3 covariance;

    // 粗匹配：大范围、粗分辨率、不做精搜索
    kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain,
        bestPose, covariance, false, false);

    if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
      (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
      (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
    {
      // 粗匹配通过后进入精匹配与建边，细节见 3-4-3
    }

    // 继续寻找下一条候选链
    candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
  }

  return loopClosed;
}

注意 MatchScan 的第二个参数虽然是候选链，但匹配本身并没有"链与链"的比较：AddScans 会把链内各帧的点一起铺进同一张相关栅格（背离当前视点的点会被跳过），评分是当前帧的点对这张栅格求相关。链在这里仍然只是候选的组织形式，决定参考栅格用哪一段历史观测来填。
粗匹配的方差阈值来自参数 $3.0$ 的平方，即 $9.0$ 平方米，两个平移方向分别比较，对应的标准差上限是 $3.0$ 米
- 这个阈值比较宽松，目的是先召回，后筛选
粗匹配的响应阈值默认是 $0.35$ ，比近邻链连边用的 link_match_minimum_response_fine 的 $0.1$ 更严格，但比精匹配的 $0.45$ 更宽松
三个阈值共同构成"粗筛"，任何一项不满足都直接拒绝

3-4-3 精匹配

粗匹配通过后，Karto 会构造一个临时扫描对象
- 临时扫描复用当前帧的编号、时间与修正位姿，但把传感器位姿设为粗匹配得到的 bestPose
- 然后用顺序匹配器在候选链上做一次精匹配
精匹配调用的是顺序匹配器，它的相关栅格分辨率是 $0.01$ 米，搜索空间是 $0.5$ 米
- 顺序匹配器内部也会先做一次粗搜索，位置范围 $±0.25 \pm 0.25$ 米、步长 $0.02$ 米，角度范围 $±20∘ \pm 20^\circ$
- 紧随其后的精搜索位置范围是自己粗步长的一半，即 $±0.01 \pm 0.01$ 米，步长 $0.01$ 米
精搜索的角度范围是粗角度步长的一半，即 $\pm 1^\circ$ ，步长 $0.2^\circ$
- 因此精搜索阶段只有 $\times 3 \times 11 = 99$ 个候选，主要开销在顺序匹配器自己的粗搜索上

// 构造临时扫描，复用当前帧的元信息
LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector());
tmpScan.SetUniqueId(pScan->GetUniqueId());
tmpScan.SetTime(pScan->GetTime());
tmpScan.SetStateId(pScan->GetStateId());
tmpScan.SetCorrectedPose(pScan->GetCorrectedPose());
tmpScan.SetSensorPose(bestPose);

// 精匹配：小范围、细分辨率、做精搜索
kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(
  &tmpScan, candidateChain, bestPose, covariance, false);

if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue()) {
  // 精匹配拒绝
} else {
  // 精匹配通过，建立回环边
  pScan->SetSensorPose(bestPose);
  LinkChainToScan(candidateChain, pScan, bestPose, covariance);
  CorrectPoses();
  loopClosed = true;
}

为什么精匹配要用临时扫描，而不是直接改当前帧
- 因为粗匹配只是"候选"，如果精匹配失败，当前帧的位姿不应该被污染
- 临时扫描让粗匹配的结果与当前帧解耦，失败时可以直接丢弃
- 只有精匹配通过后，才把 bestPose 写回当前帧并建立回环边

3-4-4 约束建立与全局修正

精匹配通过后，回环边不是连到整条候选链，而是连到链中离当前帧最近的一帧
- 这样做可以避免一条回环边对应多个节点，保持图的稀疏性
LinkChainToScan 先找最近扫描，再检查距离是否小于 link_scan_maximum_distance
- 如果超过这个距离，即使响应很高也不建边

void MapperGraph::LinkChainToScan(
  const LocalizedRangeScanVector & rChain, LocalizedRangeScan * pScan,
  const Pose2 & rMean, const Matrix3 & rCovariance)
{
  Pose2 pose = pScan->GetReferencePose(
    m_pMapper->m_pUseScanBarycenter->GetValue());

  // 找链中离当前帧最近的扫描
  LocalizedRangeScan * pClosestScan = GetClosestScanToPose(rChain, pose);
  assert(pClosestScan != NULL);

  Pose2 closestScanPose = pClosestScan->GetReferencePose(
    m_pMapper->m_pUseScanBarycenter->GetValue());

  kt_double squaredDistance =
    pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());

  // 距离太远的链不建边
  if (squaredDistance <
    math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
  {
    LinkScans(pClosestScan, pScan, rMean, rCovariance);
  }
}

回环边建立后，调用 CorrectPoses 触发一次全局优化
CorrectPoses 本身不实现优化算法，它只负责三件事
- 调用求解器的 Compute
- 把求解器返回的修正位姿写回每个扫描
- 清空求解器的修正缓存
写回时使用 SetCorrectedPoseAndUpdate，因为端点坐标必须跟着位姿一起更新
如果只改位姿不更新端点，下一次地图重建会用到旧端点，地图上会出现错位的墙
CeresSolver::Clear 只清空 corrections_，不会删除 Ceres 问题中的参数块、残差块或已添加的约束；下一次 Compute 仍在同一个问题上继续求解

void MapperGraph::CorrectPoses()
{
  ScanSolver * pSolver = m_pMapper->m_pScanOptimizer;
  if (pSolver != NULL) {
    // 求解全部节点
    pSolver->Compute();

    // 把修正量写回扫描，并刷新端点
    const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())
    {
      LocalizedRangeScan * scan =
        m_pMapper->m_pMapperSensorManager->GetScan(iter->first);
      if (scan == NULL) {
        continue;
      }
      scan->SetCorrectedPoseAndUpdate(iter->second);
    }

    // 清空缓存，等待下一次回环
    pSolver->Clear();
  }
}

GetCorrections 返回的是"节点编号到修正位姿"的映射
如果某个节点在定位模式下已经被删除，GetScan 会返回空指针，这里直接跳过
这也是定位模式与建图模式共用同一套优化流程的关键：删除节点后，求解器里也不再有该节点，修正表自然不会包含它

3-4-5 参数调优：代码行为与论文结论

回环检测有两个直接决定候选是否出现的参数
- loop_search_maximum_distance，记为 $r$ ，回环搜索半径
- loop_match_minimum_chain_size，记为 $g$ ，候选链的最少帧数

参数	符号	作用	取值过大	取值过小
回环搜索半径	$r$	决定候选历史帧的空间范围	无关区域进入候选，误匹配增多	真实回环落在范围外，漏检
最短链长	$g$	决定候选链的最少帧数	短回环被拒绝，召回下降	单帧噪声进入候选，误匹配增多

这两个参数在 FindPossibleLoopClosure 中各用一次

kt_double squaredDistance =
  candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
if (squaredDistance <
  math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
{
  // 已与当前帧连通的帧不能进入候选链
  if (find(nearLinkedScans.begin(), nearLinkedScans.end(),
      pCandidateScan) != nearLinkedScans.end()) {
    chain.clear();
  } else {
    chain.push_back(pCandidateScan);
  }
} else {
  // 离开半径时，链长达到 g 才返回
  if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) {
    return chain;
  } else {
    chain.clear();
  }
}

由此得到三个不直观的结论
- 候选链是扫描序号连续的一段，碰到任何一帧已连通的扫描就整条清空，所以回环找的是"空间接近、但图上尚未连通"的轨迹
- $g$ 数帧不数距离，默认 0.5 米一帧， $g = 10$ 约等于 5 米；改 minimum_travel_distance 时必须同步改 $g$
- $r$ 作用在修正位姿上，漂移超过 $r$ 后真实回环永远进不了半径，阈值再低也没用；slam_toolbox 默认 $r = 3.0$ 米，大场地一圈的漂移很容易超过它
每加入一个新节点都会从 0 号扫描重新遍历， $r$ 只筛选链成员，不降低这部分开销

现象	调整方向
回访处地图错开，日志里没有候选链	增大 $r$ ，减小 $g$
有候选链，但粗匹配低于 0.35	增大 `loop_search_space_dimension`，最后才考虑降阈值
粗匹配通过，精匹配低于 0.45	增大 `correlation_search_space_dimension`，或放宽角度偏移
回环频繁但地图被拉歪，或重复场景误匹配	增大 $g$ 与 0.45 阈值，减小 $r$

前两行是漏检，后两行是误检，方向相反，不要同时朝一个方向调
参数没有理论最优。Han 等人在《Multi-Objective Optimization of Loop Closure Detection Parameters for Indoor 2D Simultaneous Localization and Mapping》（Sensors，2020）中把 $(r, g)$ 写成三目标优化
- 目标为占据栅格比例 $\eta$ 、角点数 $n_c$ 、封闭区域数 $n_e$ ，分别反映冗余、结构完整性与拓扑一致性
- 每个个体完整跑一遍 Karto 建图来评价，NSGA-III 种群 66、迭代 60 代，约四千次建图
- 粗匹配阈值 0.7 与协方差上限 0.16 固定不优化
目标函数为

$\min_{r,g}\ F(r,g) = \left[\eta(r,g),\ n_c(r,g),\ n_e(r,g)\right]^{T}$

论文得到的最优、最差参数如下

数据集	最优 $(r, g)$	最差 $(r, g)$
ACES	$(5.29, 17)$	$(2.16, 18)$
Intel	$(2.27, 19)$	$(3.28, 16)$
MIT-Killian	$(5.05, 11)$	$(2.00, 18)$
MIT-CSAIL	$(4.02, 10)$	$(2.21, 20)$
长走廊	$(3.13, 6)$	$(3.34, 13)$
地下车库	$(5.51, 16)$	$(2.19, 19)$

三点结论
- 最优值随环境从 $r = 2.27$ 米到 $5.51$ 米、 $g = 6$ 到 $19$ 大幅移动，没有通用取值
- slam_karto 默认 $r = 4$ 、 $g = 10$ 在多数数据集接近最优，在 MIT-Killian 上明显失效；slam_toolbox 默认 $r = 3.0$ 比六组最优中的五组都小
- 论文只优化两个参数，且固定在 0.7 与 0.16 的阈值下，与 slam_toolbox 的 0.35 和实际生效的 9.0 平方米方差上限不同，表值不能直接照搬；评价只能离线，三项指标也只是无真值代理
实际操作顺序：先调 $r$ 让候选链出现，再调 $g$ 让它稳定，最后才动 0.35 与 0.45 两级阈值

3-5 位姿图优化

有了回环检测，接下来看看 OpenKarto 如何用这些回环约束做位姿图优化
顺序匹配与回环匹配给出的相对位姿并不完全一致，位姿图优化要在全部约束下重新求解每个扫描的位姿

3-5-1 回顾

图位姿优化（Pose Graph Optimization, PGO）建模为非线性最小二乘问题（Nonlinear Least Squares, NLS）
图中有两类元素
- 节点表示机器人各时刻的位姿 $x_i=(x_i,y_i,\theta_i)$
- 边表示两个节点之间的相对位姿约束，来自里程计、扫描匹配或回环
每条边给出观测相对位姿 $z_{ij}$ ，与当前估计的预测相对位姿比较，得到残差：

$e_{ij}=h(x_i,x_j)-z_{ij}$

其中， $h(x_i,x_j)=x_i^{-1}\oplus x_j$ 是由当前估计得到的预测相对位姿， $\oplus$ 表示位姿复合。这里把预测与观测都表示在节点 $i$ 的坐标系下，减法按分量进行，朝向分量再归一化到 $[-\pi,\pi)$ ，也就是“预测值减观测值”；后文的雅可比与 Ceres 残差都沿用这一约定。由于位姿变换涉及旋转矩阵或三角函数，残差通常是节点位姿的非线性函数。

考虑每条边的可信度，优化目标为加权最小二乘

$\min_{\{x_i\}}\sum_{(i,j)\in\mathcal E} e_{ij}^{T}\Lambda_{ij}e_{ij}$

其中：
- $e_{ij}$ ：节点 $i, j$ 之间的位姿约束残差。
- $\Lambda_{ij}=\Sigma_{ij}^{-1}$ ：信息矩阵，通常由约束协方差矩阵的逆得到。
- $\mathcal E$ ：图中所有约束边的集合。
目标函数非线性，通常从当前估计出发做局部线性化，再迭代更新
以 Gauss–Newton 为例，每轮执行
- 根据当前节点位姿计算每条边的残差。
- 计算残差对节点位姿的雅可比矩阵。
- 构建并求解线性化后的增量方程。
- 更新节点位姿，继续迭代，直到满足收敛条件。
相对约束无法确定全局基准，还需要固定一个参考节点或加入先验，消除整张图整体平移与旋转的自由度
后文依次展开节点与边、误差函数、雅可比以及优化器

3-5-2 节点、边与相对约束

位姿图由两类对象组成，节点保存扫描位姿，边保存相对位姿观测
- 节点 Vertex<LocalizedRangeScan>：持有一个扫描对象，节点的位姿就是该扫描的修正位姿 $c_i=(x_i,y_i,\theta_i)$
- 边 Edge<LocalizedRangeScan>：连接两个节点，本身只记录源、目标与一个标签
边的定义很薄，真正的观测放在标签 LinkInfo 里

template<typename T>
class Edge
{
public:
  Edge(Vertex<T> * pSource, Vertex<T> * pTarget)
  : m_pSource(pSource), m_pTarget(pTarget), m_pLabel(NULL)
  {
    // 构造时把边同时挂到源与目标端点
    m_pSource->AddEdge(this);
    m_pTarget->AddEdge(this);
  }

  Vertex<T> * GetSource() const { return m_pSource; }
  Vertex<T> * GetTarget() const { return m_pTarget; }
  EdgeLabel * GetLabel() { return m_pLabel; }

private:
  Vertex<T> * m_pSource;
  Vertex<T> * m_pTarget;
  EdgeLabel * m_pLabel;
};

边有两个来源
- 顺序边：当前扫描与上一帧或运行窗口内的扫描匹配成功后建立
- 回环边：3-4 的 LinkChainToScan 在当前帧与历史候选链匹配成功后建立
两类边都通过 LinkScans 进入图，并同时交给优化器

void MapperGraph::LinkScans(
  LocalizedRangeScan * pFromScan, LocalizedRangeScan * pToScan,
  const Pose2 & rMean, const Matrix3 & rCovariance)
{
  kt_bool isNewEdge = true;
  Edge<LocalizedRangeScan> * pEdge = AddEdge(pFromScan, pToScan, isNewEdge);
  if (pEdge == NULL) {
    return;
  }

  // 只有新建的边才需要写入观测
  if (isNewEdge == true) {
    pEdge->SetLabel(new LinkInfo(pFromScan->GetCorrectedPose(),
      pToScan->GetCorrectedAt(rMean), rCovariance));

    // 把边交给位姿图求解器
    if (m_pMapper->m_pScanOptimizer != NULL) {
      m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
    }
  }
}

LinkInfo 保存三样东西
- 两个扫描建立连接时的位姿
- 相对位姿差 m_PoseDifference，也就是观测 $z_{ij}=(t_{ij},\theta_{ij})$
- 该观测的协方差 m_Covariance
信息矩阵不单独保存，而是由协方差求逆得到，即 $\Lambda_{ij}=\Sigma_{ij}^{-1}$
构造 LinkInfo 时，相对位姿被写成从源节点 $i$ 到目标节点 $j$ 的变换，协方差也被旋转到源节点的坐标系

void LinkInfo::Update(const Pose2 & rPose1, const Pose2 & rPose2,
  const Matrix3 & rCovariance)
{
  m_Pose1 = rPose1;
  m_Pose2 = rPose2;

  // 把第二个位姿变换到第一个位姿的坐标系
  Transform transform(rPose1, Pose2());
  m_PoseDifference = transform.TransformPose(rPose2);

  // 把协方差旋转到第一个位姿的坐标系
  Matrix3 rotationMatrix;
  rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
  m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
}

协方差的旋转不能省略
- 扫描匹配给出的协方差是在世界坐标系下表达的，而相对位姿是在源节点坐标系下表达的
- 如果协方差不跟着旋转，信息矩阵的方向就会错，优化会把误差往错误的方向压
- 这也是很多位姿图实现里最容易出错的地方之一

3-5-3 误差函数与雅可比

3-5-3-1 误差函数

对连接节点 $i$ 与 $j$ 的边，已知位姿 $c_i,c_j$ 与相对位姿观测 $z_{ij}$ ，先把 $j$ 变换到 $i$ 的坐标系，得到预测相对位姿

$h(c_i,c_j)=\begin{bmatrix}R_i^{T}(t_j-t_i)\\\theta_j-\theta_i\end{bmatrix}$

其中， $R_i$ 和 $t_i$ 分别是节点 $i$ 的旋转矩阵与平移

记观测为 $z_{ij}=(t_{ij},\theta_{ij})$ ，残差取预测值减观测值

$e_{ij}=h(c_i,c_j)-z_{ij}= \begin{bmatrix} R_i^{T}(t_j-t_i)-t_{ij}\\ \theta_j-\theta_i-\theta_{ij} \end{bmatrix}$

残差是三维向量，前两维为平移误差，第三维为朝向误差
- 朝向具有周期性，需要用 NormalizeAngle 归一化到 $[-\pi,\pi)$
每条边带有协方差 $\Sigma_{ij}$ ，其逆矩阵为信息矩阵

$\Lambda_{ij}=\Sigma_{ij}^{-1}$

因此，整张图的优化目标可以写成加权最小二乘形式

$\chi^{2}=\sum_{(i,j)\in\mathcal{E}} e_{ij}^{T}\Lambda_{ij}e_{ij}$

其中， $\Lambda_{ij}=\Sigma_{ij}^{-1}$ 为相对位姿观测的信息矩阵，用于衡量不同方向残差的相对重要程度。这个加权误差本质上就是前几期介绍过的马氏距离的平方。

若假设观测噪声服从高斯分布 $z_{ij}\sim\mathcal{N}(h(c_i,c_j),\Sigma_{ij})$ ，那么最小化该目标函数等价于最大似然估计，忽略了与待优化位姿无关的常数项

3-5-3-2 雅可比

残差是非线性的，优化器在当前位姿附近线性化，用雅可比描述位姿微小变化对残差的影响
令

$J_2= \begin{bmatrix} 0 & -1\\ 1 & 0 \end{bmatrix}$

则一条边的残差对源节点 $i$ 和目标节点 $j$ 的位姿雅可比分别为

$\frac{\partial e_{ij}}{\partial c_i} = \begin{bmatrix} -R_i^{T} & -R_i^{T}J_2(t_j-t_i)\\ 0^{T} & -1 \end{bmatrix}$

$\frac{\partial e_{ij}}{\partial c_j} = \begin{bmatrix} R_i^{T} & 0\\ 0^{T} & 1 \end{bmatrix}$

每个节点位姿按 $(x,y,\theta)$ 排列，单个雅可比为 $3\times3$ ，一条边的两个局部雅可比组成 $3\times6$ 的块
每条边只在这两个节点对应的位置产生非零块，组装后得到稀疏的全局雅可比 $\mathbf{J}$
设所有边的残差堆叠为 $\mathbf{e}$ ，所有信息矩阵组成块对角矩阵 $\mathbf{\Lambda}$ ，则线性化后的正规方程为

$\mathbf{H}\Delta c=-\mathbf{b}$

$\mathbf{H}=\mathbf{J}^{T}\mathbf{\Lambda}\mathbf{J},\qquad \mathbf{b}=\mathbf{J}^{T}\mathbf{\Lambda}\mathbf{e}$

其中， $\Delta c$ 是所有节点的位姿增量， $\mathbf{H}$ 是近似 Hessian 矩阵， $\mathbf{b}$ 是梯度项
为提高非线性优化的稳定性，Levenberg-Marquardt（LM）算法会在正规方程中加入阻尼项

$\left(\mathbf{H}+\lambda\operatorname{diag}(\mathbf{H})\right)\Delta c=-\mathbf{b}$

$\lambda$ 较小时，求解行为接近高斯-牛顿法；较大时，更新步长通常更保守
求出位姿增量后，优化器更新节点位姿，重新计算残差与雅可比，并重复迭代，直到满足收敛条件或达到迭代上限

3-5-4 稀疏位姿图优化：SPA

请添加图片描述

有了误差函数和雅可比，接下来就需要求解所有节点的位姿增量，并通过迭代更新位姿，使整张图的约束误差逐步减小
OpenKarto 最初使用 SPA（Sparse Pose Adjustment）实现 ScanSolver，负责组织位姿图优化中的雅可比、构建线性方程并求解位姿增量
SPA 的核心思想是利用位姿图的稀疏结构加速求解。
- 在位姿图中，每条边只连接两个节点，因此对应的残差只依赖这两个节点的位姿
- 由此构建的全局雅可比矩阵，以及进一步得到的近似 Hessian 矩阵，都具有稀疏结构
SPA 不需要把这些矩阵当作普通的稠密矩阵处理，而是利用稀疏矩阵存储和求解方法，减少计算量与内存开销
在线性化后，优化器需要求解正规方程

$\mathbf{H}\Delta c=-\mathbf{b}$

其中， $\mathbf{H}$ 是近似 Hessian 矩阵， $\mathbf{b}$ 是梯度项， $\Delta c$ 是所有节点的位姿增量

SPA 利用稀疏 Cholesky 分解等方法求解该线性系统，再更新节点位姿并继续迭代
- SPA 的相关方法可追溯到 Konolige 等人在 IROS 2010 发表的论文《Efficient Sparse Pose Adjustment for 2D Mapping》
  - 论文研究了二维建图中的稀疏位姿调整方法，并讨论了利用稀疏结构提高优化效率，以及在增量建图中复用已有求解结果的思路。
后来，slam_toolbox 引入 Ceres 作为位姿图优化后端，当前运行时实际使用的是配置项 solver_plugin 指定的求解器，默认即 CeresSolver
这里的 SPA 更多是历史实现与算法参照，用来理解稀疏位姿调整的组织方式；它与 Ceres 不会同时求解同一张图
上面介绍了 SPA 的稀疏求解思路，下面说明 Ceres 如何把同一类位姿图问题转换为非线性最小二乘

3-5-5 Ceres 求解流程

前面介绍了 SPA 如何利用稀疏矩阵求解位姿图优化问题，它是理解位姿图优化结构的参照
本节再看 slam_toolbox 当前使用的 Ceres 实现，了解它如何将 Karto 的节点与边转换为非线性最小二乘问题
Ceres Solver 是 Google 开发的开源 C++ 非线性最小二乘库，常用于 SLAM、三维重建和传感器标定，它求解的标准形式为

$\min_{x}\ \frac{1}{2}\sum_i \rho_i\left(\|f_i(x)\|^{2}\right)$

其中：
- $x$ 是待优化参数，即所有节点的位姿参数
- $f_i(x)$ 是第 $i$ 个残差函数，即一条边对应的三维残差
- $\rho_i$ 是鲁棒损失函数，这是 Ceres 目标函数的一般写法；是否真正启用鲁棒核，取决于添加残差块时为 LossFunction 传入的配置
- 没有配置鲁棒核时， $\rho_i$ 取恒等函数，目标退化为普通的加权非线性最小二乘
- slam_toolbox 的默认值是 None，此时不会构造 HuberLoss 或 CauchyLoss

3-5-5-1 将位姿图转换为 Ceres 问题

Ceres 通过几个核心对象组织优化任务
- Problem：管理待优化参数块与残差块
- CostFunction：计算残差，并可提供雅可比
- LossFunction：定义鲁棒损失函数
- 参数化接口：控制参数如何更新，例如将角度归一化
- Solver::Options：配置信赖域策略、线性求解器和收敛条件等
在 CeresSolver::AddConstraint() 中，Karto 的节点与边会被转换成 Ceres 能够处理的参数和残差
- 每个节点对应一个包含 $(x,y,\theta)$ 的三维参数块
- 每条边对应一个三维残差块，连接两个节点的参数块
- 边上的 LinkInfo 提供相对位姿观测与协方差
因此，Ceres 不需要直接理解 Karto 的 Vertex、Edge 等图结构，只需要接收参数块、残差函数以及相应的权重信息

3-5-5-2 为什么要使用平方根信息矩阵

添加约束时，代码先将协方差求逆得到信息矩阵，再通过 Cholesky 分解获得平方根信息矩阵

karto::Matrix3 precisionMatrix =
  pLinkInfo->GetCovariance().Inverse();

// 将 3×3 信息矩阵逐元素拷贝到 Eigen
Eigen::Matrix3d information;
information << precisionMatrix(0, 0), precisionMatrix(0, 1), precisionMatrix(0, 2),
  precisionMatrix(1, 0), precisionMatrix(1, 1), precisionMatrix(1, 2),
  precisionMatrix(2, 0), precisionMatrix(2, 1), precisionMatrix(2, 2);

// Cholesky 分解，取上三角因子 U
Eigen::Matrix3d sqrt_information =
  information.llt().matrixU();

这里的 matrixU() 返回上三角因子 $U$ ，满足

$U^{T}U=\Lambda_{ij}$

设原始残差为 $e_{ij}$ ，则传给 Ceres 的加权残差为

$r_{ij}=Ue_{ij}$

于是

$\|r_{ij}\|^{2} =e_{ij}^{T}U^{T}Ue_{ij} =e_{ij}^{T}\Lambda_{ij}e_{ij}$

Ceres 本身只需要最小化残差的平方和
通过预先乘上平方根信息矩阵，就能将加权最小二乘转换为普通的残差平方和形式
这样既保留了观测协方差带来的权重，也不需要让 Ceres 在每次计算残差时再显式处理完整的信息矩阵

3-5-5-3 残差计算与自动微分

实际的残差由 PoseGraph2dErrorTerm 计算，核心代码如下

// 相对平移：将节点 b 的位置变换到节点 a 的坐标系
residuals_map.template head<2>() =
  RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a)
  - p_ab_.cast<T>();

// 相对朝向：计算角度残差并归一化
residuals_map(2) = NormalizeAngle(
  (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));

// 使用平方根信息矩阵加权
residuals_map =
  sqrt_information_.template cast<T>() * residuals_map;

前两维计算相对平移误差，第三维计算相对朝向误差，与前面介绍的位姿图残差定义一致
最后一行用平方根信息矩阵加权，使 Ceres 最小化的残差平方和等价于前面定义的马氏距离平方和
仿函数只需要描述残差如何计算，雅可比则由自动微分机制计算，不必像手写雅可比那样逐项推导

3-5-5-4 角度参数化

位姿中的角度具有周期性
虽然残差计算时使用 NormalizeAngle()，但如果参数更新时不对角度进行处理，节点参数本身仍可能超出约定的角度区间
因此，代码还通过角度参数化接口，在参数更新后将角度归一化

// 仅对角度分量启用局部参数化，参数更新时归一化
problem_->SetParameterization(
  &node1it->second(2),
  angle_local_parameterization_);

problem_->SetParameterization(
  &node2it->second(2),
  angle_local_parameterization_);

需要区分两个操作
- NormalizeAngle() 处理的是残差中的角度差
- AngleLocalParameterization 处理的是优化过程中的角度参数更新
两者配合，可以避免角度参数持续累积到区间之外
不过，角度归一化并不意味着在 $\pm\pi$ 处天然可微，残差函数仍需要正确处理角度的周期性

3-5-5-5 求解器配置

CeresSolver::Configure() 中有几个值得关注的配置

// 稀疏正规方程 Cholesky 求解
options_.linear_solver_type =
  ceres::SPARSE_NORMAL_CHOLESKY;

// LM 信赖域策略，调节每步更新幅度
options_.trust_region_strategy_type =
  ceres::LEVENBERG_MARQUARDT;

// 目标函数、梯度与参数的收敛判据
options_.function_tolerance = 1e-3;
options_.gradient_tolerance = 1e-6;
options_.parameter_tolerance = 1e-3;

options_.sparse_linear_algebra_library_type =
  ceres::SUITE_SPARSE;

options_.num_threads = 50;
options_.use_nonmonotonic_steps = true;
options_.jacobi_scaling = true;

SPARSE_NORMAL_CHOLESKY：使用稀疏正规方程 Cholesky 求解方法
LEVENBERG_MARQUARDT：采用 LM 信赖域策略，调节更新步长
function_tolerance、gradient_tolerance、parameter_tolerance：分别控制目标函数变化、梯度和参数变化相关的收敛判据
SUITE_SPARSE：指定稀疏线性代数后端
num_threads：设置 Ceres 可使用的线程数上限，实际并行程度取决于求解过程和任务规模
dynamic_sparsity：在相应配置下启用动态稀疏结构处理，适合约束结构可能变化的问题
这些参数决定了 Ceres 如何执行优化，而不是改变位姿图本身的基本约束定义

3-5-5-6 SPA 与 Ceres 的区别

实现要点	SPA	Ceres
问题组织	围绕稀疏位姿调整构建	通过参数块与残差块描述问题
雅可比	由具体实现计算与组装	可手写，也可使用自动微分
稀疏求解	利用稀疏矩阵结构求解	可通过 `SPARSE_NORMAL_CHOLESKY` 等方式求解
阻尼策略	取决于具体实现	可配置 LM 或 Dogleg
约束复用	取决于具体实现	可复用 `Problem` 中已有的参数与残差块

两者的主要区别在于问题组织方式和具体求解实现，而不是位姿图优化的基本目标
只要采用相同的残差定义与权重，两者就可以针对同一类优化问题进行求解
但这只是算法层面的等价性，实际运行时只会由 solver_plugin 加载一个求解器，不存在 SPA 与 Ceres 同时参与同一次优化的情形
还需要注意：复用同一个 ceres::Problem，不等于增量优化
如果每次新增约束后都重新调用 ceres::Solve()，求解器仍会针对当前问题执行一次完整的求解过程
至于能否复用内部线性系统或分解结果，要看具体实现，不能仅凭 Problem 对象被复用就下结论
随着建图时间增长，节点和约束不断增加，优化成本也可能随之上升
因此，定位模式下通过裁剪不再需要的节点和边控制图规模，是值得关注的工程实现

3-5-6 位姿回写与地图刷新

求解完成后，Ceres 把优化后的参数块写回 nodes_，再组装成 IdPoseVector
Compute 里还有一个重要动作：把第一个节点设为常量
位姿图中的边主要约束节点之间的相对位姿，而不是节点在世界坐标系中的绝对位置
假设把所有节点的位姿同时平移一段距离、再统一旋转一个角度，节点之间的相对位姿不会改变，所有边的残差也不会改变
- 因此仅靠相对位姿约束，无法唯一确定整张图在世界坐标系中的绝对位置和朝向
- 这种整体平移与旋转的自由度称为规范自由度（gauge freedom）
为了消除这个自由度，需要选定一个参考节点并固定它的位姿，让优化器只调整其他节点
- 这个参考节点通常称为锚点（anchor），也可以叫固定参考节点
- 在 slam_toolbox 中，求解器通过 SetParameterBlockConstant 把首节点设为常量参数块，它在优化过程中不再更新，其他节点则在它提供的参考坐标系下调整
固定首节点不代表它的位姿一定最准确，这样做主要是为了消除整体运动自由度，使优化问题具有确定的参考坐标系
固定哪个节点都可以，选择第一个只是实现上的便利
锚点是在加入第一个节点时确定的，AddNode 只记录该节点在 nodes_ 中的迭代器

// 加入第一个节点时记录锚点
nodes_->insert(std::pair<int, Eigen::Vector3d>(id, pose2d));

if (nodes_->size() == 1) {
  first_node_ = nodes_->find(id);
}

真正把锚点固定下来发生在 Compute 中：先确认三个参数块存在，再用 SetParameterBlockConstant 禁止优化器更新它们

void CeresSolver::Compute()
{
  boost::mutex::scoped_lock lock(nodes_mutex_);

  if (nodes_->size() == 0) {
    return;
  }

  // 固定第一个节点，消除整体平移与旋转自由度
  if (!was_constant_set_ && first_node_ != nodes_->end() &&
    problem_->HasParameterBlock(&first_node_->second(0)) &&
    problem_->HasParameterBlock(&first_node_->second(1)) &&
    problem_->HasParameterBlock(&first_node_->second(2)))
  {
    problem_->SetParameterBlockConstant(&first_node_->second(0));
    problem_->SetParameterBlockConstant(&first_node_->second(1));
    problem_->SetParameterBlockConstant(&first_node_->second(2));
    was_constant_set_ = !was_constant_set_;
  }

  ceres::Solver::Summary summary;
  ceres::Solve(options_, problem_, &summary);

  if (!summary.IsSolutionUsable()) {
    RCLCPP_WARN(node_->get_logger(),
      "CeresSolver: Ceres could not find a usable solution to optimize.");
    return;
  }

  // 收集修正位姿
  corrections_.clear();
  corrections_.reserve(nodes_->size());
  for (ConstGraphIterator iter = nodes_->begin(); iter != nodes_->end(); ++iter) {
    karto::Pose2 pose;
    pose.SetX(iter->second(0));
    pose.SetY(iter->second(1));
    pose.SetHeading(iter->second(2));
    corrections_.push_back(std::make_pair(iter->first, pose));
  }
}

was_constant_set_ 只用一次
第一次优化时固定首节点，后续优化不再重复设置，因为参数块常量属性会一直保留
代码层面还有一个需要留意的边界：first_node_ 只在加入第一个节点时赋值，RemoveNode 删除节点后不会更新它
- Compute 只有在 was_constant_set_ 仍为假时才会访问 first_node_，正常情况下第一次优化后这个标志就变为真
- 当前定位流程中，第一个节点来自已加载的地图，不在定位缓冲区里，按现有调用顺序不会走到删除它的分支
- 因此这更像一个未被现有流程触发的潜在风险：如果后续改动在第一次优化前删除首节点，first_node_ 就会悬空，再访问它属于未定义行为
- 这里不把它写成当前版本已经发生的缺陷，触发条件是“首节点在首次设置常量之前被删除”，现有代码没有显式防护这一条件
修正位姿写回扫描后，地图并不会自动刷新
下一次 updateMap 调用 CreateFromScans 时，才会用新的位姿重新投影所有端点
如果希望回环后立刻看到地图变化，需要主动触发一次地图发布，或者等待下一个发布周期

3-6 地图生成

3-6-1 扫描投影与射线追踪

地图生成的第一步是把每帧扫描的端点按修正位姿投影到世界坐标系
设扫描位姿为 $\theta)$ ，第 $k$ 束激光的距离为 $r_k$ ，角度为 $\phi_k$
端点坐标为

$p_k = \begin{bmatrix} x + r_k\cos(\theta + \phi_k) \\ y + r_k\sin(\theta + \phi_k) \end{bmatrix}$

如果 $r_k$ 超过距离阈值 $r_{th}$ ，端点会被截断到阈值处，方向保持不变

$p_k' = t + \frac{r_{th}}{r_k}\left(p_k - t\right)$

截断的意义是：超过阈值的读数不认为是真实障碍，只用来标记"这条射线穿过了这些栅格"
端点是否算作命中，取决于原始读数是否小于阈值
如果读数本身超过阈值，射线只增加穿过计数，不增加命中计数

virtual kt_bool AddScan(LocalizedRangeScan * pScan, kt_bool doUpdate = false)
{
  LaserRangeFinder * laserRangeFinder = pScan->GetLaserRangeFinder();
  kt_double rangeThreshold = laserRangeFinder->GetRangeThreshold();
  kt_double maxRange = laserRangeFinder->GetMaximumRange();
  kt_double minRange = laserRangeFinder->GetMinimumRange();

  Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
  const PointVectorDouble & rPointReadings = pScan->GetPointReadings(false);

  kt_bool isAllInMap = true;
  int pointIndex = 0;

  const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
  {
    Vector2<kt_double> point = *pointsIter;
    kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];

    // 只有小于阈值的读数才算有效端点
    kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE);

    if (rangeReading <= minRange || rangeReading >= maxRange ||
      std::isnan(rangeReading))
    {
      pointIndex++;
      continue;
    } else if (rangeReading >= rangeThreshold) {
      // 超阈值读数截断到阈值处
      kt_double ratio = rangeThreshold / rangeReading;
      kt_double dx = point.GetX() - scanPosition.GetX();
      kt_double dy = point.GetY() - scanPosition.GetY();
      point.SetX(scanPosition.GetX() + ratio * dx);
      point.SetY(scanPosition.GetY() + ratio * dy);
    }

    // 从传感器位置到端点画一条射线
    kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
    if (!isInMap) {
      isAllInMap = false;
    }

    pointIndex++;
  }

  return isAllInMap;
}

射线追踪使用 Bresenham 直线算法
- 它只访问直线穿过的栅格，不需要按固定步长采样，效率高且不会漏格
- 每个被穿过的栅格增加穿过计数，端点所在栅格会再增加一次穿过计数与一次命中计数

void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1,
  Functor * f = NULL)
{
  // 按斜率绝对值决定是否交换 x 与 y
  kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
  if (steep) {
    std::swap(x0, y0);
    std::swap(x1, y1);
  }
  if (x0 > x1) {
    std::swap(x0, x1);
    std::swap(y0, y1);
  }

  kt_int32s deltaX = x1 - x0;
  kt_int32s deltaY = abs(y1 - y0);
  kt_int32s error = 0;
  kt_int32s ystep;
  kt_int32s y = y0;

  if (y0 < y1) {
    ystep = 1;
  } else {
    ystep = -1;
  }

  kt_int32s pointX;
  kt_int32s pointY;
  for (kt_int32s x = x0; x <= x1; x++) {
    if (steep) {
      pointX = y;
      pointY = x;
    } else {
      pointX = x;
      pointY = y;
    }

    error += deltaY;
    if (2 * error >= deltaX) {
      y += ystep;
      error -= deltaX;
    }

    Vector2<kt_int32s> gridIndex(pointX, pointY);
    if (IsValidGridIndex(gridIndex)) {
      kt_int32s index = GridIndex(gridIndex, false);
      T * pGridPointer = GetDataPointer();
      // 穿过计数加一
      pGridPointer[index]++;

      if (f != NULL) {
        (*f)(index);
      }
    }
  }
}

RayTrace 先对整条射线调用 TraceLine，再单独处理端点
端点命中只在 isEndPointValid 为真时增加
如果端点落在栅格外，函数返回 false，AddScan 会把整帧标记为"未完全落入地图"
这个返回值用于判断地图尺寸是否足够，必要时会扩展栅格

3-6-2 占据判定

每个栅格维护两个计数器
- 穿过计数 pass：有多少条射线穿过该栅格
- 命中计数 hit：有多少条射线在该栅格结束
命中比例定义为

$\rho = \frac{\text{hit}}{\text{pass}}$

占据判定规则为

$\text{state}(c) = \begin{cases} \text{Occupied}, & \text{pass} > p_{\min}\ \text{且}\ \rho > \rho_{occ} \\ \text{Free}, & \text{pass} > p_{\min}\ \text{且}\ \rho \le \rho_{occ} \\ \text{Unknown}, & \text{pass} \le p_{\min} \end{cases}$

其中 $p_{\min}$ 是 min_pass_through， $\rho_{occ}$ 是 occupancy_threshold
穿过次数不足的栅格保持未知，不参与占据与空闲的判断
这样可以避免少量杂散射线把栅格误判为空闲或占据

virtual void UpdateCell(kt_int8u * pCell, kt_int32u cellPassCnt,
  kt_int32u cellHitCnt)
{
  // 穿过次数不足，保持未知
  if (cellPassCnt > m_pMinPassThrough->GetValue()) {
    kt_double hitRatio =
      static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);

    if (hitRatio > m_pOccupancyThreshold->GetValue()) {
      *pCell = GridStates_Occupied;
    } else {
      *pCell = GridStates_Free;
    }
  }
}

这套判定不是概率意义上的对数几率更新，而是计数比例
- 它的好处是实现简单、结果可复现、参数直观
- 代价是对动态障碍与传感器噪声的抑制能力弱于概率栅格
min_pass_through 与 occupancy_threshold 就是为这个代价准备的补偿手段
- 提高 min_pass_through 可以让地图更保守，减少杂散点；
- 提高 occupancy_threshold 可以让更多栅格被判为空闲

3-6-3 地图重建与 ROS 发布

CreateFromScans 是地图重建的总入口
- 它先根据所有扫描的包围盒计算栅格尺寸与原点，再逐帧追迹，最后统一更新状态
- 尺寸计算会把所有端点与传感器位置都纳入包围盒，避免地图边缘被裁掉
分辨率由 ROS 参数 resolution 决定，默认 $0.05$ 米

static OccupancyGrid * CreateFromScans(
  const LocalizedRangeScanVector & rScans,
  kt_double resolution, kt_int32u min_pass_through,
  kt_double occupancy_threshold)
{
  if (rScans.empty()) {
    return NULL;
  }

  kt_int32s width, height;
  Vector2<kt_double> offset;

  // 根据所有扫描的包围盒计算栅格尺寸与原点
  ComputeDimensions(rScans, resolution, width, height, offset);

  OccupancyGrid * pOccupancyGrid =
    new OccupancyGrid(width, height, offset, resolution);

  // 把 ROS 参数传入栅格
  pOccupancyGrid->SetMinPassThrough(min_pass_through);
  pOccupancyGrid->SetOccupancyThreshold(occupancy_threshold);

  // 逐帧追迹并统一更新
  pOccupancyGrid->CreateFromScans(rScans);
  return pOccupancyGrid;
}

CreateFromScans 内部先调整两个计数栅格的尺寸与原点，再逐帧调用 AddScan
- 所有帧处理完后调用一次 Update，把计数转换成状态
- 这样避免每帧都做一次全图状态更新，时间复杂度从"帧数乘栅格数"降到"帧数乘射线长度加栅格数"

virtual void CreateFromScans(const LocalizedRangeScanVector & rScans)
{
  // 初始化两个计数栅格
  m_pCellPassCnt->Resize(GetWidth(), GetHeight());
  m_pCellPassCnt->GetCoordinateConverter()->SetOffset(
    GetCoordinateConverter()->GetOffset());

  m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
  m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(
    GetCoordinateConverter()->GetOffset());

  // 逐帧追迹，只累加计数
  const_forEach(LocalizedRangeScanVector, &rScans)
  {
    if (*iter == nullptr) {
      continue;
    }
    AddScan(*iter);
  }

  // 统一把计数转换成占据状态
  Update();
}

转换到 ROS 消息时，Karto 的栅格原点在左下角，ROS 地图的原点也在左下角，但两者的索引方向需要对齐
toNavMap 逐格读取 Karto 栅格值，再按 ROS 的索引公式写入
未知、占据、空闲三种状态分别映射为 $- 1$ 、 $100$ 、 $0$

inline void toNavMap(
  const karto::OccupancyGrid * occ_grid,
  nav_msgs::msg::OccupancyGrid & map)
{
  kt_int32s width = occ_grid->GetWidth();
  kt_int32s height = occ_grid->GetHeight();
  karto::Vector2<kt_double> offset =
    occ_grid->GetCoordinateConverter()->GetOffset();

  // 尺寸或原点变化时重新分配
  if (map.info.width != (unsigned int) width ||
    map.info.height != (unsigned int) height ||
    map.info.origin.position.x != offset.GetX() ||
    map.info.origin.position.y != offset.GetY())
  {
    map.info.origin.position.x = offset.GetX();
    map.info.origin.position.y = offset.GetY();
    map.info.width = width;
    map.info.height = height;
    map.data.resize(map.info.width * map.info.height);
  }

  for (kt_int32s y = 0; y < height; y++) {
    for (kt_int32s x = 0; x < width; x++) {
      kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));

      // Karto 栅格状态映射到 ROS 占据值
      switch (value) {
        case karto::GridStates_Unknown:
          map.data[MAP_IDX(map.info.width, x, y)] = -1;
          break;
        case karto::GridStates_Occupied:
          map.data[MAP_IDX(map.info.width, x, y)] = 100;
          break;
        case karto::GridStates_Free:
          map.data[MAP_IDX(map.info.width, x, y)] = 0;
          break;
      }
    }
  }
}

这里没有做插值或平滑
每个 Karto 栅格对应一个 ROS 栅格，分辨率一致
如果希望地图更适合导航，可以在下游做膨胀或降采样，但 slam_toolbox 本身只负责输出原始占据栅格

3-6-4 地图与位姿图的持久化

slam_toolbox 把地图保存拆成两个文件
- .posegraph：位姿图结构、节点位姿、边与协方差
- .data：扫描数据、激光参数与数据集元信息
这种拆分让位姿图可以独立加载与优化，不必每次都反序列化全部扫描
两个文件都使用 Boost 序列化，写入时先调用 Mapper::SaveToFile，再调用 Dataset::SaveToFile
读取时顺序相同，也是先加载位姿图，再加载扫描数据

namespace serialization
{

inline bool write(
  const std::string & filename,
  karto::Mapper & mapper,
  karto::Dataset & dataset,
  rclcpp::Node::SharedPtr node)
{
  try {
    // 位姿图与扫描数据分开保存
    mapper.SaveToFile(filename + std::string(".posegraph"));
    dataset.SaveToFile(filename + std::string(".data"));
    return true;
  } catch (boost::archive::archive_exception e) {
    RCLCPP_ERROR(node->get_logger(), "Failed to write file: Exception %s", e.what());
    return false;
  }
}

inline bool read(
  const std::string & filename,
  karto::Mapper & mapper,
  karto::Dataset & dataset,
  rclcpp::Node::SharedPtr node)
{
  if (!fileExists(filename + std::string(".posegraph"))) {
    RCLCPP_ERROR(node->get_logger(),
      "serialization::Read: Failed to open requested file: %s.",
      filename.c_str());
    return false;
  }

  try {
    // 先恢复位姿图，再恢复扫描
    mapper.LoadFromFile(filename + std::string(".posegraph"));
    dataset.LoadFromFile(filename + std::string(".data"));
    return true;
  } catch (boost::archive::archive_exception e) {
    RCLCPP_ERROR(node->get_logger(),
      "serialization::Read: Failed to read file. Exception: %s", e.what());
  }

  return false;
}

}  // namespace serialization

反序列化时，LoadFromFile 会把 m_Deserialized 置真，同时把 m_Initialized 置假
下一次调用 Initialize 时因此会走恢复分支，只更新运行扫描缓冲区与回环匹配器的参数
这一步保证已加载的位姿图不会被初始化流程重新构造并覆盖
这也是 3-2-3 里 m_Deserialized 分支存在的原因

4 slam_toolbox 配置

4-1 安装

直接使用，或查看安装后的程序与配置时，通过二进制包安装

sudo apt update
sudo apt install ros-humble-slam-toolbox

需要阅读、修改或调试源码时，将其放入 ROS 2 工作空间编译

mkdir -p ~/slam_ws/src
cd ~/slam_ws/src
git clone https://github.com/SteveMacenski/slam_toolbox.git
cd ~/slam_ws
source /opt/ros/humble/setup.bash
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install

4-2 参数配置

五份参数文件共享大部分键，差异集中在模式相关项

参数文件	对应节点	主要差异
`mapper_params_online_async.yaml`	async	默认建图配置，`map_update_interval` 为 5.0
`mapper_params_online_sync.yaml`	sync	与异步版基本一致，扫描进入队列后由独立线程处理
`mapper_params_offline.yaml`	sync	`tf_buffer_duration` 为 14400，`map_update_interval` 为 10.0，适合离线包
`mapper_params_localization.yaml`	localization	`scan_buffer_size` 为 3，`loop_match_minimum_chain_size` 为 3
`mapper_params_lifelong.yaml`	lifelong	增加 8 个 `lifelong_*` 参数，用于节点回收与约束筛选

4-2-1 求解器参数

参数	默认值	作用
`solver_plugin`	`solver_plugins::CeresSolver`	位姿图求解器插件，当前分支仅注册 `CeresSolver`
`ceres_linear_solver`	`SPARSE_NORMAL_CHOLESKY`	Ceres 线性求解器
`ceres_preconditioner`	`SCHUR_JACOBI`	迭代求解器的预条件器，仅对 `ITERATIVE_SCHUR` 有效
`ceres_trust_strategy`	`LEVENBERG_MARQUARDT`	信赖域策略，可选 `DOGLEG`
`ceres_dogleg_type`	`TRADITIONAL_DOGLEG`	仅 `DOGLEG` 生效，可选 `SUBSPACE_DOGLEG`
`ceres_loss_function`	`None`	鲁棒核，可选 `HuberLoss` 或 `CauchyLoss`，阈值固定为 0.7
`mode`	`mapping`	设为 `localization` 时启用快速删边，不切换节点逻辑

表中默认值取自随包 YAML，代码 declare_parameter 的默认值可能不同。

4-2-2 坐标系与输入参数

参数	默认值	作用
`odom_frame`	`odom`	里程计坐标系
`map_frame`	`map`	地图坐标系
`base_frame`	`base_footprint`	机器人基座坐标系
`scan_topic`	`/scan`	激光话题，必须是绝对话题名
`transform_publish_period`	0.02	map 到 odom 的发布周期，0 表示不发布
`transform_timeout`	0.2	查询 TF 的超时时间
`tf_buffer_duration`	30.0	TF 缓存时长
`minimum_time_interval`	0.5	两帧被处理扫描之间的最小时间间隔
`throttle_scans`	1	每 N 帧只处理一帧
`scan_queue_size`	1	TF 消息过滤器队列长度，异步模式应保持为 1
`restamp_tf`	false	用当前时间重打 map 到 odom 的 TF 时间戳
`use_sim_time`	true	launch 参数，真机上应设为 false

4-2-3 地图与运行参数

参数	默认值	作用
`resolution`	0.05	占据栅格分辨率，单位米
`map_update_interval`	5.0	地图发布间隔，在线为 5.0，离线为 10.0
`map_name`	`/map`	占据栅格话题名
`min_laser_range`	0.0	栅格化使用的最小激光距离
`max_laser_range`	20.0	栅格化使用的最大激光距离
`use_map_saver`	true	是否创建 `save_map` 服务并订阅地图
`enable_interactive_mode`	true	交互式位姿图编辑，定位与 lifelong 强制关闭
`stack_size_to_use`	40000000	序列化与反序列化使用的栈大小
`min_pass_through`	2	判为占据或空闲所需的最少穿过次数
`occupancy_threshold`	0.1	命中比例阈值，高于该值判为占据

4-2-4 扫描匹配参数

参数	默认值	作用
`use_scan_matching`	true	是否用扫描匹配修正里程计位姿
`use_scan_barycenter`	true	是否用扫描重心作为参考点
`minimum_travel_distance`	0.5	触发新扫描处理的最小移动距离
`minimum_travel_heading`	0.5	触发新扫描处理的最小朝向变化
`scan_buffer_size`	10	扫描缓冲区长度
`scan_buffer_maximum_scan_distance`	10.0	缓冲区扫描与当前位姿的最大距离，超过则移除
`link_match_minimum_response_fine`	0.1	顺序连边的精匹配响应下限
`link_scan_maximum_distance`	1.5	允许建立连边的最大扫描间距
`correlation_search_space_dimension`	0.5	顺序匹配的搜索窗口大小
`correlation_search_space_resolution`	0.01	顺序匹配的搜索分辨率
`correlation_search_space_smear_deviation`	0.1	顺序匹配响应的多峰平滑程度
`distance_variance_penalty`	0.5	匹配位姿偏离里程计时的距离惩罚系数
`angle_variance_penalty`	1.0	匹配位姿偏离里程计时的角度惩罚系数
`fine_search_angle_offset`	0.00349	精搜索的角度偏移范围
`coarse_search_angle_offset`	0.349	粗搜索的角度偏移范围，约 20 度
`coarse_angle_resolution`	0.0349	粗搜索的角度步长，约 2 度
`minimum_angle_penalty`	0.9	角度惩罚下限
`minimum_distance_penalty`	0.5	距离惩罚下限，代码默认 0.05，YAML 覆盖为 0.5
`use_response_expansion`	true	是否对响应做扩张

4-2-5 回环参数

参数	默认值	作用
`do_loop_closing`	true	是否启用回环检测
`loop_search_maximum_distance`	3.0	候选链搜索半径，比 `slam_karto` 的 4.0 保守
`loop_match_minimum_chain_size`	10	候选链最短帧数
`loop_match_maximum_variance_coarse`	3.0	粗匹配单个平移方向的标准差上限，比较时平方为方差
`loop_match_minimum_response_coarse`	0.35	粗匹配响应下限
`loop_match_minimum_response_fine`	0.45	精匹配响应下限
`loop_search_space_dimension`	8.0	回环匹配的搜索窗口大小
`loop_search_space_resolution`	0.05	回环匹配的搜索分辨率
`loop_search_space_smear_deviation`	0.03	回环匹配响应的多峰平滑程度

参数在节点启动时读取一次，修改后需重启节点。

4-3 launch 启动

launch 文件	节点可执行文件	参数文件	行为
`online_async_launch.py`	`async_slam_toolbox_node`	`mapper_params_online_async.yaml`	在激光回调里直接处理扫描
`online_sync_launch.py`	`sync_slam_toolbox_node`	`mapper_params_online_sync.yaml`	扫描入队，由独立线程处理
`localization_launch.py`	`localization_slam_toolbox_node`	`mapper_params_localization.yaml`	加载已有位姿图，订阅 `initialpose`，关闭地图保存与交互编辑
`offline_launch.py`	`sync_slam_toolbox_node`	`mapper_params_offline.yaml`	离线回放，TF 缓存更长
`lifelong_launch.py`	`lifelong_slam_toolbox_node`	`mapper_params_lifelong.yaml`	实验性长期建图，当前安装规则不安装该 launch 与节点
`merge_maps_kinematic_launch.py`	`merge_maps_kinematic`	无	合并多个子图

所有 launch 都把节点名固定为 slam_toolbox，YAML 第一层键需与之一致。

4-3-1 加载已有位姿图

启动时加载已有位姿图依赖三个字段
- map_file_name：序列化文件的名字
- map_start_pose：给定初始位姿，格式为三个数
- map_start_at_dock：从第一个节点开始
map_start_pose 与 map_start_at_dock 互斥，同时给出时以位姿为准

4-3-2 常用服务

服务	作用
`slam_toolbox/serialize_map`	保存位姿图
`slam_toolbox/deserialize_map`	加载位姿图
`slam_toolbox/save_map`	保存占据栅格图像，需要 `use_map_saver` 为 true
`slam_toolbox/pause_new_measurements`	暂停或恢复处理新扫描
`slam_toolbox/clear_queue`	清空同步模式积压的扫描队列
`slam_toolbox/merge_submaps`	合并多个子图
`slam_toolbox/add_submap`	向图中添加子图

总结

本文从 slam_toolbox 的工程结构出发，沿着扫描匹配、回环检测、位姿图优化与地图生成四条链路，对照了 Karto 的核心公式与本地实现
需要再次强调的是，文中的 Karto 是 slam_toolbox 修改后的版本，定位模式、节点裁剪、近邻查询、序列化与占据栅格参数都已经与原版 OpenKarto 不同
核心要点回顾：
- 扫描匹配的本质是在相关栅格上做由粗到精的离散搜索，响应归一化与距离、角度惩罚共同决定候选位姿的得分
- 协方差不是附属产物，它决定约束在优化中的权重，方差下限与响应倒数放大都是为了防止约束过度自信
- 回环检测先用搜索半径与最短链长筛选候选链，再做粗匹配与精匹配，任何一级不通过都不会建立回环边
- 位姿图优化的残差是相对位姿预测与观测之差，Ceres 通过平方根信息矩阵把马氏距离转成欧氏范数，并用角度参数化保证朝向合法
- 地图生成是全量重建，所有扫描按当前修正位姿重新投影，射线追踪用 Bresenham 算法累加穿过与命中计数，再按比例判定占据
- 定位模式通过裁剪旧节点控制图规模，这也是 slam_toolbox 对 Karto 最具工程价值的修改之一
如有错误，欢迎指出！感谢观看！