Cartographer的slam解决方案

Cartographer的slam解决方案

Cartographer 解决 SLAM 问题的方案是一个两层优化架构：

• 前端（Local SLAM）：负责实时处理每一帧激光数据，消除单步累积误差，输出子图
• 后端（Global SLAM）：负责回环检测和全局位姿图优化，消除长距离累积误差

下面介绍基于激光雷达的点云数据处理到完整建图的过程简单介绍，雷达就按照三个雷达为例。

1. 前端------局部SLAM

1. 数据预处理

三个激光雷达各自以不同帧率发送 TimedPointCloudData。RangeDataCollator 按传感器 ID 分别缓存，当所有传感器的最新数据都到齐后，按时间戳对齐并合并成一个同步点云------这就是一帧。

如果一帧点云太稀疏（有些高频雷达数据量少），可以累积多帧（num_accumulated_range_data）再一起处理。

2. 运动畸变去除

激光扫描一圈需要时间（比如100ms），在这期间机器人在运动，所以一帧点云里的第一个点和最后一个点不是在同一个位置采集的。

PoseExtrapolator 用IMU和里程计数据估计出每个点被采集时刻的机器人位姿，把所有点变换到同一时刻的坐标系下。

3. 重力对齐与体素滤波

点云被旋转到重力对齐坐标系下，裁掉太高和太低的点（天花板和地面），保留水平平面内的有效信息。然后进行体素滤波降采样，大幅减少点数。

4. 位姿预测

PoseExtrapolator 根据历史位姿、IMU、里程计数据，预测当前机器人在地图中的位姿 T_pred。

5. 扫描匹配（核心定位算法）

匹配对象： submaps_.front()------当前活跃子图列表中最旧的那个子图。

目标： 找到最优位姿 T*，使得当前点云投影到子图上时，所有点所在栅格的概率值之和最大化。

匹配分为两步：

第一步：相关性扫描匹配（粗匹配）

在预测位姿 T_pred 附近一个搜索窗口内（比如 ±0.3m），穷举所有可能的平移修正量，对每一个候选位姿，将离散化后的点云点逐一投影到子图的概率栅格上，累加命中栅格被占据的概率值作为评分。评分函数为：

score = Σ 命中点的概率 / 总点数

评分最高的候选位姿作为粗匹配结果。

第二步：Ceres扫描匹配（精匹配）

以粗匹配结果为初值，构建非线性最小二乘优化问题。代价函数对点云中每个点，在子图栅格上进行双三次插值，计算该点在当前位姿下的空间匹配代价，同时加入平移和旋转的正则化约束，防止位姿偏离初值太远。通过 Ceres 求解器得到亚像素级精度的最优位姿 T*。

6. 更新位姿推测器

把精匹配后的位姿 T* 反馈给 PoseExtrapolator，更新其内部状态，提高下一帧的预测精度。

7. 插入子图

用 T* 把当前点云插入所有活跃子图 （submaps_ 列表中的每一个）。

插入的过程：从雷达原点向每个点的方向发射射线。射线经过的栅格用贝叶斯更新融合新旧信息------在让步比空间做乘法，实现前后多帧观测的自动融合。同一个栅格若在一帧内被多条射线穿过，kUpdateMarker 机制保证只处理一次。射线尽头的栅格同理融合新命中信息。新区域栅格直接初始化为首次观测概率。重叠区域被反复命中后，概率确定性越来越高。

8. 子图管理（双子图机制）

配置 num_range_data = 30。

创建新子图： 当 submaps_.back() 达到30帧时，AddSubmap() 被调用。如果列表中已有2个子图，先移除最旧的那个；然后创建新子图加入列表末尾。

封图： 当 submaps_.front() 达到60帧时，调用 Finish()。Finish() 内部执行栅格裁切（裁剪掉无数据的边缘空栅格）并打上完成标记。该子图随后在下一次 AddSubmap 中被移除。

匹配对象 始终是 submaps_.front()------即当前最旧的那个活跃子图。因为它被插入的帧数最多（30-59帧），地图内容最稳定可靠。

9. 通知后端

GlobalTrajectoryBuilder 每处理完一帧，就调用 pose_graph_->AddNode()，把新节点位姿和插入的子图信息注册到全局位姿图中。

---

2. 后端------全局SLAM

前端每处理完一帧，就调用 pose_graph_->AddNode()，把这一帧的位姿信息 和所插入子图的信息注册到后端。前端立刻返回继续处理下一帧，不等待后端。

后端在后台线程池中异步运行，做三件事：约束建立 → 回环检测 → 全局优化。

---

10. 约束建立：帧内约束

每来一个新节点，后端首先建立一个帧内约束（INTRA_SUBMAP）。

帧内约束记录的是：新节点在某个子图坐标系下的相对位姿是多少。这个信息来自前端扫描匹配的结果------前端已经算出了"这一帧在子图A里的位置是(x,y,θ)"。这个约束不经过任何额外的搜索或匹配，直接从前端结果转化而来，非常稳定。

帧内约束的作用是把新节点"拴"在它所属的子图上。有了这个约束，全局优化时就知道"节点3在子图A的旁边"。

节点3 → 子图A：相对位姿 = (1.02, 0.01, 0.5°)
节点4 → 子图A：相对位姿 = (1.04, 0.03, 0.7°)
...
节点30 → 子图A：相对位姿 = (30.10, 0.15, 1.2°)
节点31 → 子图B：相对位姿 = (1.00, 0.02, 0.3°)

---

11. 回环检测：帧间约束

每来一个新节点，除了建立帧内约束，后端还会尝试一个额外操作：把这个新节点和所有历史已完成子图进行匹配。

历史已完成子图是指那些已经被移除/封图的旧子图。机器人可能走了一大圈又回到了这里，但前端不知道------前端只和当前的活跃子图做匹配，不关心历史。

后端做的就是这个"回头看"的工作。它在新节点和历史子图之间运行扫描匹配，如果匹配得分超过阈值，就建立帧间约束（INTER_SUBMAP）。

帧间约束记录的是：节点i在历史子图j的坐标系下，和子图j里的某些特征对齐了。

节点300（当前位置）→ 子图A（开局时建的）：匹配得分 0.85 → 建立约束！

这个约束的意义是："节点300看到的墙，和子图A记录的是同一面墙。它们之间的全局相对距离应该很小，而不是轨迹记录的几百米。"

---

12. 全局位姿图优化

帧内约束和帧间约束都存放在 data_.constraints 列表中。当积累的约束达到一定数量后，触发全局优化。

优化的过程：

第一步：构建图 。把所有节点位姿和所有子图位姿作为图中的"顶点"，把所有约束（帧内 + 帧间）作为图中的"边"。每条边描述了两个顶点之间应该是什么样的相对位姿。

第二步：求解。用 Ceres 非线性最小二乘求解，调整所有节点和子图的全局位姿，使得所有约束的误差加权平方和最小。帧内约束（前端匹配结果）权重高，帧间约束（回环检测）权重略低但有决定性的纠偏作用。

第三步：更新。求解完成后，更新所有节点和所有子图的全局位姿。节点和子图的位置被整体重新排布，回环处的轨迹被拉正，累积漂移被消除。

---

13. 子图裁剪

优化完成后，裁剪器检查是否有过时的子图可以移除，以控制内存。

14. 完整流程（后端视角）

前端每处理一帧 → AddNode() → 提交工作任务到线程池

后台线程池循环：
    取出一个工作任务（对应一个新节点）
    ├─ 建立帧内约束（直接从前端结果转化，拴住节点和子图的关系）
    ├─ 回环检测（节点 vs 历史已完成子图的扫描匹配）
    │   └─ 匹配成功 → 建立帧间约束（"这里我们以前来过"）
    └─ 检查是否需要全局优化
        ├─ 否 → 继续等待更多约束
        └─ 是 → RunOptimization()
              ├─ 构建位姿图（所有节点+所有子图+所有约束）
              ├─ Ceres SPA 求解（调整所有位姿使约束误差最小）
              ├─ 更新所有节点和子图的全局位姿
              └─ 子图裁剪（释放过时数据）

3. 完整流程图

三个激光雷达各自发包（独立时间戳）
         ↓
RangeDataCollator :: 按传感器ID缓存 → 全部到齐 → 时间戳对齐合并成一帧
         ↓
LocalTrajectoryBuilder2D :: 前端局部SLAM
    ├─ 运动畸变去除（PoseExtrapolator 校正每个点的位置）
    ├─ 重力对齐 + Z轴裁切 + 体素滤波
    ├─ 位姿预测 T_pred
    ├─ 扫描匹配（vs submaps_.front()）
    │   ├─ 相关性匹配（暴力搜索最优平移+旋转）
    │   └─ Ceres匹配（非线性优化，亚像素精度）
    ├─ 更新 PoseExtrapolator（反馈 T*）
    ├─ 插入所有活跃子图（射线投射 + 贝叶斯更新）
    ├─ 子图管理
    │   ├─ back() 达30帧 → AddSubmap()（移除最旧的，创建新的）
    │   └─ front() 达60帧 → Finish()（裁切栅格，标记完成）
    └─ 通知后端：pose_graph_->AddNode()
         ↓
PoseGraph2D/3D :: 后端全局SLAM（后台线程池异步运行）
    ├─ 约束计算
    │   ├─ 帧内约束（节点 ↔ 插入的子图）
    │   └─ 帧间约束（节点 ↔ 历史子图匹配）
    ├─ 回环检测（BBS加速的历史子图扫描匹配）
    ├─ 全局位姿图优化（Ceres SPA）
    └─ 子图裁剪（控制内存）
         ↓
输出：全局一致的栅格概率地图（.pbstream 文件）

4. 前后端代码划分

层级	对应的核心类	文件位置
前端	LocalTrajectoryBuilder2D/3D	mapping/internal/2d/local_trajectory_builder_2d.cc
	ActiveSubmaps2D/3D	mapping/2d/submap_2d.cc
	RealTimeCorrelativeScanMatcher2D/3D	mapping/internal/2d/scan_matching/
	CeresScanMatcher2D/3D	mapping/internal/2d/scan_matching/
桥接	GlobalTrajectoryBuilder	mapping/internal/global_trajectory_builder.cc
后端	PoseGraph2D/3D	mapping/internal/2d/pose_graph_2d.cc
	ConstraintBuilder2D/3D	mapping/internal/constraints/constraint_builder_2d.cc
	OptimizationProblem2D/3D	mapping/internal/optimization/optimization_problem_2d.cc

对比维度	前端	后端
核心类	LocalTrajectoryBuilder	PoseGraph
运行线程	传感器数据线程	后台线程池
处理频率	每帧一次	按触发条件
操作数据	当前活跃子图	所有历史节点和子图
解决的问题	单步误差	累积漂移
代码边界	明确，通过 GlobalTrajectoryBuilder 桥接

5. 数据→建图完整源码解析

1. 数据入口

CollatedTrajectoryBuilder::AddSensorData() 是所有传感器数据的统一入口。

// mapping/internal/collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::AddSensorData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& timed_point_cloud_data) {
  AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}

它调用 AddData()，把数据包装成 Dispatchable 对象，然后交给 sensor_collator_：

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

sensor_collator_ 是 RangeDataCollator（当配置 collate_by_trajectory=false 时，对应 SensorCollator，但核心都是把数据交给 OrderedMultiQueue）。这个对象负责时间同步和合并多传感器数据。

2. 数据同步合并------RangeDataCollator

RangeDataCollator 维护一个 map，按传感器 ID 分别缓存最新数据包：

// mapping/internal/range_data_collator.cc
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
    const std::string& sensor_id,
    sensor::TimedPointCloudData timed_point_cloud_data) {
  
  // 如果这个传感器之前有数据没处理完，说明要"翻页"了
  if (id_to_pending_data_.count(sensor_id) != 0) {
    current_start_ = current_end_;
    current_end_ = id_to_pending_data_.at(sensor_id).time;
    auto result = CropAndMerge();  // 先把当前累积的合并输出
    id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
    return result;
  }
  
  // 存起来
  id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
  
  // 检查是否所有传感器都到齐了
  if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
    return {};  // 没到齐，返回空
  }
  
  // 全部到齐，执行合并
  current_start_ = current_end_;
  current_end_ = oldest_timestamp;  // 取所有传感器中最老的时间戳作为截止点
  return CropAndMerge();
}

CropAndMerge() 的核心逻辑：

// mapping/internal/range_data_collator.cc
sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
  sensor::TimedPointCloudOriginData result{current_end_, {}, {}};
  
  for (auto it = id_to_pending_data_.begin(); it != id_to_pending_data_.end();) {
    sensor::TimedPointCloudData& data = it->second;
    // 找到时间在 [current_start_, current_end_] 范围内的所有点
    auto overlap_begin = ...;
    auto overlap_end = ...;
    
    // 把落在区间内的点拷贝到 result 中
    for (auto overlap_it = overlap_begin; overlap_it != overlap_end; ++overlap_it) {
      result.ranges.push_back({*overlap_it, *intensities_overlap_it, origin_index});
    }
  }
  
  // 按时间戳排序所有点
  std::sort(result.ranges.begin(), result.ranges.end(),
            [](const RangeMeasurement& a, const RangeMeasurement& b) {
              return a.point_time.time < b.point_time.time;
            });
  return result;
}

合并后的数据被传递回 LocalTrajectoryBuilder2D::AddRangeData()。

3. 局部SLAM入口------LocalTrajectoryBuilder2D::AddRangeData

// mapping/internal/2d/local_trajectory_builder_2d.cc
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
  
  // 先通过 range_data_collator_ 做时间同步和合并
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    return nullptr;  // 没凑齐，这次不处理
  }

  // 检查是否初始化了 PoseExtrapolator
  if (extrapolator_ == nullptr) {
    return nullptr;  // 还没初始化，不处理
  }

  // 运动畸变去除：用 PoseExtrapolator 估计每个点被采集时刻的位姿
  std::vector<transform::Rigid3f> range_data_poses;
  for (const auto& range : synchronized_data.ranges) {
    common::Time time_point = time + common::FromSeconds(range.point_time.time);
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

  // 累计点云
  if (num_accumulated_ == 0) {
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    // 把每个点变换到最新时刻的坐标系下
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] * synchronized_data.origins.at(...);
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);
    const float range = (hit_in_local.position - origin_in_local).norm();
    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        accumulated_range_data_.returns.push_back(hit_in_local);  // 有效命中
      } else {
        accumulated_range_data_.misses.push_back(hit_in_local);  // 太远了，标记为空
      }
    }
  }
  ++num_accumulated_;

  // 判断是否积累够了
  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    num_accumulated_ = 0;
    // 执行匹配和插入！
    return AddAccumulatedRangeData(time, gravity_aligned_range_data, ...);
  }
  return nullptr;
}

4. 扫描匹配------AddAccumulatedRangeData

// mapping/internal/2d/local_trajectory_builder_2d.cc
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment,
    const absl::optional<common::Duration>& sensor_duration) {
  
  // 预测位姿
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

  // 自适应体素滤波，降采样
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());
  
  // 核心：扫描匹配！
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

  // 把匹配后的位姿反馈给位姿推测器
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  extrapolator_->AddPose(time, pose_estimate);

  // 插入子图
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());

  return matching_result;
}

5. ScanMatch------粗匹配+精匹配

// mapping/internal/2d/local_trajectory_builder_2d.cc
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time, const transform::Rigid2d& pose_prediction,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
  
  // 匹配对象：submaps_.front()------最旧的活跃子图
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 第一步：相关性扫描匹配（粗匹配）
  if (options_.use_online_correlative_scan_matching()) {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
  }

  // 第二步：Ceres 扫描匹配（精匹配）
  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);
  
  return pose_observation;
}

6. 相关性扫描匹配------RealTimeCorrelativeScanMatcher2D::Match

这个函数你之前已经看过，现在基于源码回顾它的核心逻辑：

// mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc
double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate) const {
  
  // 生成一系列旋转后的点云（一次性旋转，后续只用平移匹配）
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  
  // 离散化：把点云坐标转换成子图的栅格索引
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
  
  // 在搜索窗口内穷举所有候选位姿
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);
  
  // 对每个候选位姿评分
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
  
  // 取评分最高的
  const Candidate2D& best_candidate =
      *std::max_element(candidates.begin(), candidates.end());
  
  *pose_estimate = transform::Rigid2d({...}, {...});
  return best_candidate.score;
}

评分函数的实现：

// mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(...) {
  for (Candidate2D& candidate : *candidates) {
    candidate.score = ComputeCandidateScore(
        static_cast<const ProbabilityGrid&>(grid),
        discrete_scans[candidate.scan_index], 
        candidate.x_index_offset,
        candidate.y_index_offset);
    
    // 加入"距离惩罚"------倾向于选离初值更近的位姿
    candidate.score *= std::exp(-common::Pow2(
        std::hypot(candidate.x, candidate.y) * translation_delta_cost_weight +
        std::abs(candidate.orientation) * rotation_delta_cost_weight));
  }
}

概率栅格的评分函数：

float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
                            const DiscreteScan2D& discrete_scan,
                            int x_index_offset, int y_index_offset) {
  float candidate_score = 0.f;
  for (const Eigen::Array2i& xy_index : discrete_scan) {
    const Eigen::Array2i proposed_xy_index(
        xy_index.x() + x_index_offset,
        xy_index.y() + y_index_offset);
    const float probability =
        probability_grid.GetProbability(proposed_xy_index);
    candidate_score += probability;
  }
  candidate_score /= static_cast<float>(discrete_scan.size());
  return candidate_score;
}

7. 插入子图

// mapping/internal/2d/local_trajectory_builder_2d.cc
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(...) {
  
  // 运动滤波：如果机器人几乎没动，跳过此帧
  if (motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }
  
  // 插入所有活跃子图
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);
  
  return insertion_result;
}

ActiveSubmaps2D::InsertRangeData：

// mapping/2d/submap_2d.cc
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  
  // 如果需要，创建新子图
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
  
  // 把这一帧插入所有活跃子图
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  
  // 最前面的子图满了就封图
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    submaps_.front()->Finish();
  }
  
  return submaps();
}

AddSubmap：

// mapping/2d/submap_2d.cc
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
  if (submaps_.size() >= 2) {
    submaps_.erase(submaps_.begin());  // 移除最旧的子图
  }
  submaps_.push_back(absl::make_unique<Submap2D>(origin, ...));
}

Submap2D::InsertRangeData：

// mapping/2d/submap_2d.cc
void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const RangeDataInserterInterface* range_data_inserter) {
  range_data_inserter->Insert(range_data, grid_.get());  // 核心插入
  set_num_range_data(num_range_data() + 1);
}

8. 栅格更新------ProbabilityGridRangeDataInserter2D

// mapping/2d/probability_grid_range_data_inserter_2d.cc
void ProbabilityGridRangeDataInserter2D::Insert(
    const sensor::RangeData& range_data, GridInterface* const grid) const {
  
  ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
  
  CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
           probability_grid);
  probability_grid->FinishUpdate();
}

CastRays：

void CastRays(const sensor::RangeData& range_data,
              const std::vector<uint16>& hit_table,
              const std::vector<uint16>& miss_table,
              const bool insert_free_space,
              ProbabilityGrid* probability_grid) {
  
  GrowAsNeeded(range_data, probability_grid);  // 自动膨胀子图
  
  // 命中点：标记为"墙"
  for (const sensor::RangefinderPoint& hit : range_data.returns) {
    probability_grid->ApplyLookupTable(hit_cell, hit_table);
  }

  // 射线经过的中间点：标记为"空"
  if (insert_free_space) {
    for (const Eigen::Array2i& cell_index : ray) {
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }
}

贝叶斯更新核心：

// mapping/2d/probability_grid.h
bool ApplyLookupTable(const Eigen::Array2i& cell_index,
                      const std::vector<uint16>& table) {
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  if (*cell >= kUpdateMarker) {
    return false;  // 本轮已更新过，跳过
  }
  mutable_update_indices()->push_back(flat_index);
  *cell = table[*cell];  // 查表：旧值 → 新值。本质是让步比乘法
  return true;
}

9. 通知后端------AddNode

// mapping/internal/2d/pose_graph_2d.cc
NodeId PoseGraph2D::AddNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);
  
  // 提交工作到后台线程
  AddWorkItem([=]() {
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });
  
  return node_id;
}

10. 后端约束计算------ComputeConstraintsForNode

// mapping/internal/2d/pose_graph_2d.cc
WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {
  
  // 建立帧内约束
  for (size_t i = 0; i < insertion_submaps.size(); ++i) {
    data_.constraints.push_back(
        Constraint{submap_id, node_id,
                   {transform::Embed3D(constraint_transform),
                    matcher_translation_weight, matcher_rotation_weight},
                   Constraint::INTRA_SUBMAP});
  }
  
  // 对所有历史已完成子图，尝试建立帧间约束（回环检测）
  for (const auto& submap_id_data : data_.submap_data) {
    if (submap_id_data.data.state == SubmapState::kFinished) {
      ComputeConstraint(node_id, submap_id);
    }
  }
  
  // 如果新完成了一个子图，让历史节点也来和它匹配
  if (newly_finished_submap) {
    for (const auto& node_id_data : optimization_problem_->node_data()) {
      ComputeConstraint(node_id, newly_finished_submap_id);
    }
  }
  
  // 检查是否需要全局优化
  if (num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
    return WorkItem::Result::kRunOptimization;
  }
  return WorkItem::Result::kDoNotRunOptimization;
}

11. 全局优化------RunOptimization

// mapping/internal/2d/pose_graph_2d.cc
void PoseGraph2D::RunOptimization() {
  // 调用 Ceres 求解器
  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);
  
  // 更新所有节点的全局位姿
  for (const int trajectory_id : node_data.trajectory_ids()) {
    for (const auto& node : node_data.trajectory(trajectory_id)) {
      mutable_trajectory_node.global_pose =
          transform::Embed3D(node.data.global_pose_2d) *
          transform::Rigid3d::Rotation(
              mutable_trajectory_node.constant_data->gravity_alignment);
    }
  }
  
  // 更新所有子图的全局位姿
  data_.global_submap_poses_2d = submap_data;
}