1. Cartographer 是什么？
Cartographer 是 Google 开源的实时 SLAM 系统，支持 2D 和 3D，同一套系统可以适配不同传感器配置，例如 2D 激光、3D 点云、IMU、轮速里程计等。它的核心思想是：前端构建局部一致的子地图，后端通过位姿图优化把这些子地图和轨迹节点约束到全局一致的位置上。官方文档也把 Cartographer 抽象成两个相关子系统：Local SLAM 负责生成一系列局部一致的 submap，Global SLAM 在后台寻找约束和回环，并把这些 submap 更一致地连接起来。
Cartographer 的基本数据流可以理解为：
传感器数据
↓
数据同步与预处理
↓
PoseExtrapolator 位姿预测
↓
点云滤波
↓
Scan Matching 当前 scan 匹配 active submap
↓
插入 submap
↓
生成 trajectory node
↓
PoseGraph 添加 node-submap 约束
↓
后台回环检测
↓
全局优化
↓
输出全局一致地图和轨迹这里面有两个关键词最重要：
submap：局部子地图，是 Cartographer 地图构建的基本单元。
pose graph：位姿图，是 Cartographer 后端优化的核心结构。Cartographer 不是直接维护一张从头到尾不断更新的巨大地图，而是不断生成很多小的局部地图。每个小地图局部比较准确，但是不同小地图之间可能存在累计误差，所以需要后端把它们整体优化。
2. Cartographer 的整体框架
Cartographer 可以拆成两个主模块：
Cartographer
├── Local SLAM / LocalTrajectoryBuilder
│ ├── 接收 range data
│ ├── 传感器时间同步
│ ├── PoseExtrapolator 位姿预测
│ ├── 重力对齐
│ ├── 点云滤波
│ ├── scan-to-submap 匹配
│ ├── active submap 插入
│ └── 生成 trajectory node
│
└── Global SLAM / PoseGraph
├── 保存 trajectory node
├── 保存 submap node
├── 建立 node-submap 约束
├── 后台回环检测
├── 添加 loop closure constraint
└── 全局 sparse pose adjustment 优化官方调参文档中明确说，Local SLAM 的任务是生成好的 submaps，而 Global SLAM 的任务是把这些 submaps 更一致地连接起来；Global SLAM 本质上是 pose graph optimization，通过节点和子地图之间的 constraints 进行优化。
3. Cartographer 输入输出
Cartographer 的输入通常包括：
LaserScan / PointCloud2：激光或点云数据，是主要建图数据。
IMU：提供角速度、线加速度、重力方向等信息。
Odometry：提供轮速里程计或外部里程计预测。
TF：提供传感器坐标系到机器人坐标系之间的外参关系。在 ROS 里，Cartographer 算法库本身不直接等同于 ROS 节点，cartographer_ros 负责把 ROS 的 topic、tf、参数、服务封装到 Cartographer 算法接口里。Cartographer ROS 官方仓库说明它是 Cartographer 的 ROS 集成工程。
常见输出包括：
/map：全局栅格地图。
/submap_list：当前所有子地图的信息。
/trajectory_node_list：轨迹节点信息。
/scan_matched_points2：匹配后的点云。
TF：通常发布 map、odom、base_link 等坐标系之间的关系。4. Local SLAM：前端到底做什么？
Local SLAM 是 Cartographer 前端。它的目标不是一次性得到全局最优地图，而是稳定地产生局部一致的子地图和轨迹节点。
核心过程是：
1. 收到一帧或多帧 range data；
2. 通过 PoseExtrapolator 预测当前 scan 的初始位姿；
3. 对点云做距离过滤、重力对齐、体素滤波；
4. 将当前 scan 和当前 active submap 做 scan matching；
5. 得到 scan 在 local map frame 下的位姿；
6. 如果运动量足够大，就把 scan 插入 active submap；
7. 生成 trajectory node，交给后端 PoseGraph。官方文档说明，Local SLAM 会把组装并滤波后的 scan 插入当前 submap，scan matching 使用 PoseExtrapolator 给出的初始猜测。这个初始猜测会利用除 range finder 以外的传感器数据来预测当前 scan 应该插入到 submap 的位置。
5. AddRangeData()：前端主入口
Cartographer 2D 前端的一个核心入口是 LocalTrajectoryBuilder2D::AddRangeData()。这个函数接收一批还没有完全同步的 range data，然后进行同步、时间处理、位姿外推、距离过滤、点云累积等操作。源码中可以看到，它先通过 range_data_collator_ 做数据整理，然后使用 extrapolator_ 对每个点时间做位姿外推，接着根据 min_range、max_range 把点分成 returns 和 misses。
下面是接近源码逻辑的简化版，每一行都加了注释：
std::unique_ptr<MatchingResult> AddRangeData( // 定义前端接收 range data 的函数，返回匹配结果
const std::string& sensor_id, // sensor_id 表示当前数据来自哪个传感器
const sensor::TimedPointCloudData& unsynchronized_data) { // unsynchronized_data 是还未完全同步整理的带时间点云数据
auto synchronized_data = // 创建同步后的数据变量
range_data_collator_.AddRangeData(sensor_id, // 把当前传感器 ID 传给数据整理器
unsynchronized_data); // 把未同步的点云数据传给数据整理器
if (synchronized_data.ranges.empty()) { // 如果同步后没有有效 range 点
return nullptr; // 直接返回空，表示这一帧暂时不处理
} // 结束空数据判断
const common::Time& time = synchronized_data.time; // 取出当前同步数据的时间戳
if (!options_.use_imu_data()) { // 如果配置中不使用 IMU 数据
InitializeExtrapolator(time); // 就用当前时间初始化位姿外推器
} // 结束 IMU 配置判断
if (extrapolator_ == nullptr) { // 如果位姿外推器还没有初始化
return nullptr; // 当前帧无法预测位姿，直接返回空
} // 结束外推器判断
std::vector<transform::Rigid3f> range_data_poses; // 创建数组，保存每个点对应时刻的预测位姿
range_data_poses.reserve(synchronized_data.ranges.size()); // 提前分配空间，避免循环中频繁扩容
for (const auto& range : synchronized_data.ranges) { // 遍历同步数据中的每个 range 点
common::Time time_point = // 计算当前点的真实时间
time + common::FromSeconds(range.point_time.time); // 当前帧时间加上点自身的相对时间
range_data_poses.push_back( // 把当前点对应的预测位姿加入数组
extrapolator_->ExtrapolatePose(time_point).cast<float>()); // 使用 PoseExtrapolator 外推当前点时刻的位姿
} // 结束逐点位姿外推
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) { // 再次遍历每个 range 点
const auto& hit = synchronized_data.ranges[i].point_time; // 取出当前点的测量信息
Eigen::Vector3f origin_in_local = // 计算当前点所属激光原点在 local 坐标系下的位置
range_data_poses[i] * // 使用该点时刻的位姿
synchronized_data.origins.at( // 取出该点对应的传感器原点
synchronized_data.ranges[i].origin_index); // origin_index 表示当前点对应哪个原点
sensor::RangefinderPoint hit_in_local = // 创建当前击中点在 local 坐标系下的位置
range_data_poses[i] * sensor::ToRangefinderPoint(hit); // 把点从传感器坐标系变换到 local 坐标系
Eigen::Vector3f delta = hit_in_local.position - origin_in_local; // 计算从激光原点到击中点的向量
float range = delta.norm(); // 计算当前点的距离长度
if (range >= options_.min_range()) { // 如果距离大于最小有效距离
if (range <= options_.max_range()) { // 如果距离小于最大有效距离
accumulated_range_data_.returns.push_back(hit_in_local); // 把该点作为有效 hit 点保存
} else { // 如果距离超过最大有效距离
hit_in_local.position = // 修改该点位置
origin_in_local + // 从激光原点出发
options_.missing_data_ray_length() / range * delta; // 截断到 missing_data_ray_length 长度处
accumulated_range_data_.misses.push_back(hit_in_local); // 把该点作为 miss 点保存
} // 结束最大距离判断
} // 结束最小距离判断
} // 结束点遍历
++num_accumulated_; // 累积的 range data 数量加一
if (num_accumulated_ >= options_.num_accumulated_range_data()) { // 如果累积帧数达到配置要求
return AddAccumulatedRangeData(time, // 调用下一阶段处理累积后的 range data
accumulated_range_data_, // 传入累积后的 hit/miss 数据
gravity_alignment, // 传入重力对齐姿态
sensor_duration); // 传入传感器时间间隔
} // 结束累积数量判断
return nullptr; // 如果没有达到累积数量，就暂时不输出匹配结果
} // 函数结束这里有几个关键变量：
synchronized_data：同步整理后的 range data。
range_data_poses：每个点对应时刻的预测位姿。
accumulated_range_data_.returns：有效击中点，后面用于地图占据更新。
accumulated_range_data_.misses：超过最大距离或未击中的点，后面用于空闲区域更新。
num_accumulated_：当前已经累积了多少帧 range data。
num_accumulated_range_data：配置参数，控制多少帧 range data 合成一个 scan。6. 位姿外推 PoseExtrapolator
Cartographer 在 scan matching 前需要一个初始位姿。这个初始位姿由 PoseExtrapolator 生成。它会结合历史 scan matching 位姿、IMU 数据、里程计数据等信息，预测当前 scan 或当前点在 local 坐标系下的大致位姿。
源码中典型调用是：
extrapolator_->ExtrapolatePose(time_point)每一行解释版如下：
common::Time time_point = // 定义当前点的时间
time + common::FromSeconds(range.point_time.time); // 当前 scan 时间加上点的相对时间
transform::Rigid3d pose_at_point_time = // 定义当前点时刻的预测位姿
extrapolator_->ExtrapolatePose(time_point); // 使用位姿外推器预测当前时间的机器人位姿
range_data_poses.push_back( // 把预测位姿保存起来
pose_at_point_time.cast<float>()); // 转成 float 类型，供后续点云变换使用这里要注意一个点：Cartographer 不是只对整帧 scan 用一个时间，它会考虑 range data 中点的时间信息。官方文档也说明，距离测量是在一段时间内完成的，而 ROS 消息通常是批量发送的；Cartographer 会根据每条消息时间处理机器人运动造成的扫描形变，更多的 range data 有助于组装更一致的 scan。
7. 点云距离过滤：returns 和 misses
Cartographer 把 range data 分成两类：
returns：有效击中点，表示激光打到了障碍物或结构表面。
misses：未击中或超过最大距离的点，表示射线方向上一定范围内可以认为是空闲。这一点对概率栅格地图非常重要，因为地图更新时不只更新障碍物，也更新空闲空间。
核心逻辑是：
Eigen::Vector3f delta = hit_in_local.position - origin_in_local; // 计算激光原点到当前点的向量
float range = delta.norm(); // 计算当前激光点距离
if (range >= options_.min_range()) { // 小于最小距离的点通常视为无效噪声
if (range <= options_.max_range()) { // 在最大距离内说明是可靠击中点
accumulated_range_data_.returns.push_back(hit_in_local); // 放入 returns，用于提高占据概率
} else { // 超过最大距离说明不作为真实障碍物
hit_in_local.position = // 修改点的位置
origin_in_local + // 从激光原点出发
options_.missing_data_ray_length() / range * delta; // 按比例截断到指定长度
accumulated_range_data_.misses.push_back(hit_in_local); // 放入 misses，用于更新空闲区域
} // 最大距离判断结束
} // 最小距离判断结束变量解释：
origin_in_local：激光雷达原点在 local 坐标系下的位置。
hit_in_local：激光点在 local 坐标系下的位置。
delta：从雷达原点指向击中点的向量。
range：当前点的距离。
min_range：最小有效距离，小于它的点通常被认为太近或不可靠。
max_range：最大有效距离，大于它的点通常不作为真实 hit。
missing_data_ray_length：超过 max_range 时，用这个长度表示空闲射线。8. AddAccumulatedRangeData()：累积点云进入匹配流程
当累积的 range data 数量达到 num_accumulated_range_data 后，就会进入 AddAccumulatedRangeData()。源码中可以看到，它会先计算重力对齐后的位姿预测，然后对点云做自适应体素滤波，再调用 ScanMatch()，成功后把点云变换到 local 坐标系，并插入 submap。
下面是每一行注释版：
std::unique_ptr<MatchingResult> AddAccumulatedRangeData( // 定义处理累积 range data 的函数
const common::Time time, // 当前 scan 的时间
const sensor::RangeData& gravity_aligned_range_data, // 已经重力对齐后的点云数据
const transform::Rigid3d& gravity_alignment, // 重力对齐旋转
const absl::optional<common::Duration>& sensor_duration) { // 当前传感器数据的时间间隔
if (gravity_aligned_range_data.returns.empty()) { // 如果没有有效 hit 点
return nullptr; // 直接返回空，表示不生成匹配结果
} // 空点云判断结束
const transform::Rigid3d non_gravity_aligned_pose_prediction = // 定义未重力对齐的预测位姿
extrapolator_->ExtrapolatePose(time); // 使用位姿外推器预测当前 scan 位姿
const transform::Rigid2d pose_prediction = // 定义 2D 平面上的预测位姿
transform::Project2D( // 将 3D 位姿投影成 2D 位姿
non_gravity_aligned_pose_prediction * // 当前预测位姿
gravity_alignment.inverse()); // 乘以重力对齐的逆变换，得到水平平面内的位姿
const sensor::PointCloud& filtered_gravity_aligned_point_cloud = // 定义滤波后的重力对齐点云
sensor::AdaptiveVoxelFilter( // 使用自适应体素滤波
gravity_aligned_range_data.returns, // 输入有效 hit 点
options_.adaptive_voxel_filter_options()); // 输入自适应体素滤波参数
if (filtered_gravity_aligned_point_cloud.empty()) { // 如果滤波后点云为空
return nullptr; // 直接返回空
} // 滤波后空点云判断结束
std::unique_ptr<transform::Rigid2d> pose_estimate_2d = // 定义 scan matching 后的 2D 位姿
ScanMatch(time, // 传入当前时间
pose_prediction, // 传入预测初值
filtered_gravity_aligned_point_cloud); // 传入滤波后的点云
if (pose_estimate_2d == nullptr) { // 如果 scan matching 失败
return nullptr; // 直接返回空
} // 匹配失败判断结束
const transform::Rigid3d pose_estimate = // 定义最终 3D 位姿估计
transform::Embed3D(*pose_estimate_2d) * gravity_alignment; // 将 2D 位姿嵌入 3D，并恢复重力对齐旋转
extrapolator_->AddPose(time, pose_estimate); // 把当前估计位姿加入外推器，更新运动状态
sensor::RangeData range_data_in_local = // 定义 local 坐标系下的 range data
TransformRangeData( // 对 range data 做坐标变换
gravity_aligned_range_data, // 输入重力对齐点云
transform::Embed3D(pose_estimate_2d->cast<float>())); // 使用匹配后的 2D 位姿转换到 local 坐标系
std::unique_ptr<InsertionResult> insertion_result = // 定义插入 submap 的结果
InsertIntoSubmap(time, // 传入当前时间
range_data_in_local, // 传入 local 坐标系下的 range data
filtered_gravity_aligned_point_cloud, // 传入滤波点云
pose_estimate, // 传入当前估计位姿
gravity_alignment.rotation()); // 传入重力对齐旋转
return absl::make_unique<MatchingResult>( // 创建并返回前端匹配结果
MatchingResult{time, // 保存时间
pose_estimate, // 保存估计位姿
std::move(range_data_in_local), // 保存 local 坐标系下 range data
std::move(insertion_result)}); // 保存插入 submap 的结果
} // 函数结束这里最核心的是三步：
1. pose_prediction：先预测当前 scan 大概在哪里。
2. ScanMatch()：再用 scan matching 把位姿优化到更准确的位置。
3. InsertIntoSubmap()：最后把当前 scan 插入 active submap。9. 点云滤波：VoxelFilter 和 AdaptiveVoxelFilter
Cartographer 会对点云做滤波，主要目的有两个：
1. 降低点数，减少 scan matching 计算量；
2. 保持点云空间分布更加均匀，避免近处密集点过度主导优化。官方文档说明，Cartographer 先应用固定大小体素滤波，再使用自适应体素滤波；自适应体素滤波会在最大体素边长限制下尽量达到目标点数。
关键代码：
const sensor::PointCloud& filtered_gravity_aligned_point_cloud = // 定义滤波后的点云变量
sensor::AdaptiveVoxelFilter( // 调用自适应体素滤波函数
gravity_aligned_range_data.returns, // 输入有效击中点
options_.adaptive_voxel_filter_options()); // 输入滤波参数，包括 max_length、min_num_points 等变量解释：
gravity_aligned_range_data.returns：重力对齐后的有效点云。
adaptive_voxel_filter_options：自适应体素滤波参数。
max_length：最大体素边长。
min_num_points：希望滤波后至少保留的点数。
filtered_gravity_aligned_point_cloud：最终用于 scan matching 的点云。10. ScanMatch()：当前 scan 和 active submap 匹配
ScanMatch() 是 Local SLAM 的核心。它输入预测位姿和滤波点云，输出 scan matching 后的位姿估计。源码中可以看到，它优先取 active submaps 中的第一个 submap 作为匹配对象；如果启用在线相关匹配，会先做一次粗匹配，然后再调用 CeresScanMatcher 做精匹配。
每一行注释版：
std::unique_ptr<transform::Rigid2d> ScanMatch( // 定义 scan matching 函数，返回 2D 位姿
const common::Time time, // 当前 scan 的时间
const transform::Rigid2d& pose_prediction, // PoseExtrapolator 给出的预测位姿
const sensor::PointCloud& filtered_point_cloud) { // 滤波后的点云
if (active_submaps_.submaps().empty()) { // 如果当前还没有 active submap
return absl::make_unique<transform::Rigid2d>(pose_prediction); // 直接返回预测位姿作为估计结果
} // active submap 判断结束
std::shared_ptr<const Submap2D> matching_submap = // 定义用于匹配的子地图
active_submaps_.submaps().front(); // 取 active submaps 中的第一个 submap
transform::Rigid2d initial_ceres_pose = pose_prediction; // 初始化 Ceres 优化初值为预测位姿
if (options_.use_online_correlative_scan_matching()) { // 如果开启在线相关匹配
const double score = // 定义粗匹配得分
real_time_correlative_scan_matcher_.Match( // 调用实时相关匹配器
pose_prediction, // 输入预测位姿
filtered_point_cloud, // 输入当前 scan 点云
*matching_submap->grid(), // 输入当前 submap 的概率栅格
&initial_ceres_pose); // 输出更好的 Ceres 初值
kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score); // 记录相关匹配得分指标
} // 在线相关匹配结束
auto pose_observation = // 定义 Ceres 优化后的位姿观测
absl::make_unique<transform::Rigid2d>(); // 创建一个空的 2D 位姿对象
ceres::Solver::Summary summary; // 定义 Ceres 求解摘要，用于记录优化信息
ceres_scan_matcher_.Match( // 调用 CeresScanMatcher 做精匹配
pose_prediction.translation(), // 输入预测平移，用作平移先验
initial_ceres_pose, // 输入 Ceres 初始位姿
filtered_point_cloud, // 输入当前点云
*matching_submap->grid(), // 输入 submap 概率栅格
pose_observation.get(), // 输出优化后的位姿
&summary); // 输出优化摘要
return pose_observation; // 返回 scan matching 后的位姿
} // 函数结束这里的关键变量：
pose_prediction：外推器预测的初值。
matching_submap：当前 scan 要匹配的 active submap。
initial_ceres_pose：Ceres 优化初值，可能经过相关匹配修正。
pose_observation：最终 scan matching 得到的位姿。
filtered_point_cloud：用于匹配的滤波点云。
matching_submap->grid()：当前 submap 的概率栅格地图。11. CeresScanMatcher：非线性最小二乘精匹配
CeresScanMatcher 的目标是：调整当前 scan 的位姿，让点云落到 submap 中占据概率更高的位置，同时不要偏离预测初值太远。
源码中 CeresScanMatcher2D::Match() 会添加三类残差：
1. occupied space residual：点云和概率栅格的匹配残差；
2. translation residual：平移不要偏离预测值太多；
3. rotation residual：旋转不要偏离初始值太多。源码中还可以看到，occupied_space_weight、translation_weight、rotation_weight 都从配置中读取，Ceres 求解器使用 DENSE_QR。
每一行注释版：
void CeresScanMatcher2D::Match( // 定义 Ceres scan matching 函数
const Eigen::Vector2d& target_translation, // 预测位姿的平移部分，用作平移先验
const transform::Rigid2d& initial_pose_estimate, // 输入初始位姿估计
const sensor::PointCloud& point_cloud, // 输入当前 scan 点云
const Grid2D& grid, // 输入 submap 的概率栅格
transform::Rigid2d* const pose_estimate, // 输出优化后的位姿
ceres::Solver::Summary* const summary) const { // 输出 Ceres 求解摘要
double ceres_pose_estimate[3] = { // 定义 Ceres 优化变量数组
initial_pose_estimate.translation().x(), // 第 0 维是 x 平移初值
initial_pose_estimate.translation().y(), // 第 1 维是 y 平移初值
initial_pose_estimate.rotation().angle()}; // 第 2 维是旋转角初值
ceres::Problem problem; // 创建 Ceres 优化问题
problem.AddResidualBlock( // 添加占据空间残差块
CreateOccupiedSpaceCostFunction2D( // 创建概率栅格匹配代价函数
options_.occupied_space_weight() / // 使用 occupied_space_weight 作为权重
std::sqrt(static_cast<double>(point_cloud.size())), // 按点数开方归一化，避免点数影响权重尺度
point_cloud, // 输入当前 scan 点云
grid), // 输入 submap 概率栅格
nullptr, // 不使用鲁棒核函数
ceres_pose_estimate); // 优化变量是 x、y、theta
problem.AddResidualBlock( // 添加平移先验残差块
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( // 创建平移差代价函数
options_.translation_weight(), // 输入平移权重
target_translation), // 输入预测平移
nullptr, // 不使用鲁棒核函数
ceres_pose_estimate); // 优化变量仍然是 x、y、theta
problem.AddResidualBlock( // 添加旋转先验残差块
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction( // 创建旋转差代价函数
options_.rotation_weight(), // 输入旋转权重
ceres_pose_estimate[2]), // 输入旋转初值
nullptr, // 不使用鲁棒核函数
ceres_pose_estimate); // 优化变量仍然是 x、y、theta
ceres::Solve(ceres_solver_options_, &problem, summary); // 调用 Ceres 求解最小二乘问题
*pose_estimate = transform::Rigid2d( // 将优化后的数组重新封装成 Rigid2d
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, // 设置优化后的 x、y 平移
ceres_pose_estimate[2]); // 设置优化后的旋转角 theta
} // 函数结束12. Scan Matching 的核心公式
Cartographer 2D 中，一个 scan 的位姿可以表示为：
ξ = (ξx, ξy, ξθ)其中：
ξx：scan 在 submap 坐标系下的 x 方向平移。
ξy：scan 在 submap 坐标系下的 y 方向平移。
ξθ：scan 在 submap 坐标系下的旋转角。scan 点从自身坐标系变换到 submap 坐标系的公式是：
Tξ p = Rξ p + tξ展开就是：
Rξ = [ cosξθ -sinξθ
sinξθ cosξθ ]
tξ = [ ξx
ξy ]变量解释：
p：当前 scan 中的一个点。
Rξ：由 ξθ 构成的二维旋转矩阵。
tξ：二维平移向量。
Tξp：点 p 经过位姿 ξ 变换后在 submap 坐标系下的位置。论文中给出的 Ceres scan matching 目标函数是让变换后的 scan 点尽量落在 submap 高占据概率位置上，可以写成：
argmin_ξ Σ_k (1 - M_smooth(Tξ h_k))²变量解释：
h_k：当前 scan 中第 k 个激光点。
Tξ h_k：第 k 个点通过位姿 ξ 变换到 submap 坐标系后的坐标。
M_smooth：submap 概率栅格的平滑插值函数。
M_smooth(Tξ h_k)：变换后的点落在 submap 上对应位置的占据概率。
1 - M_smooth(Tξ h_k)：点没有落在高占据概率区域时产生的残差。直观理解就是：如果位姿估计正确，激光点应该落在地图里"更像障碍物"的地方，也就是占据概率高的位置；如果落在空闲区域，残差就会变大。论文中明确把 scan matching 表述为非线性最小二乘问题，并通过 Ceres 优化 scan pose。
13. Submap：Cartographer 的局部地图单元
Submap 是 Cartographer 的核心地图单元。论文中将 submap 表示为概率栅格：
M : rZ × rZ → [pmin, pmax]变量解释：
M：子地图概率栅格。
r：地图分辨率，例如 0.05m。
Z：整数网格坐标。
rZ × rZ：以分辨率 r 离散化后的二维栅格坐标空间。
[pmin, pmax]：每个栅格占据概率的取值范围。简单说，submap 里面每个格子都存一个概率：
概率高：这个格子更可能是障碍物。
概率低：这个格子更可能是空闲区域。
未知：这个格子还没有被可靠观测。论文中说明，若干连续 scan 会构建一个 submap，scan 插入 submap 时会计算 hit set 和 miss set；hit 对应激光击中的格子，miss 对应激光射线路径穿过但没有击中的格子。
14. 概率栅格更新公式
Cartographer 使用 odds 形式更新概率。odds 定义为：
odds(p) = p / (1 - p)如果某个格子被激光击中，即 hit，那么更新形式可以写成：
M_new(x) = clamp( odds⁻¹( odds(M_old(x)) × odds(p_hit) ) )miss 的更新类似，只是把 p_hit 换成 p_miss。
变量解释：
x：地图中的某个栅格。
M_old(x)：该栅格更新前的占据概率。
M_new(x)：该栅格更新后的占据概率。
p_hit：激光击中时使用的概率更新参数。
p_miss：激光穿过但未击中时使用的概率更新参数。
odds(p)：把概率转成 odds 形式。
odds⁻¹：把 odds 转回概率。
clamp：把概率限制在 [pmin, pmax] 范围内。直观理解：
激光终点所在格子：占据概率上升。
激光路径经过的格子：占据概率下降。
没有观测到的格子：保持原状。论文中给出了 odds 更新公式，并说明 hit 和 miss 使用类似的更新方式。
15. InsertIntoSubmap()：把当前 scan 插入 active submap
当前 scan 匹配成功后，还不一定会插入 submap。Cartographer 会先用 motion_filter_ 判断当前位姿和上一次插入节点是否太接近。如果变化太小，就跳过，避免图优化节点过多。源码中 InsertIntoSubmap() 先调用 motion_filter_.IsSimilar()，如果不相似，才调用 active_submaps_.InsertRangeData() 插入数据，并生成 TrajectoryNode::Data。
每一行注释版：
std::unique_ptr<InsertionResult> InsertIntoSubmap( // 定义插入 submap 的函数
const common::Time time, // 当前 scan 时间
const sensor::RangeData& range_data_in_local, // 当前 scan 在 local 坐标系下的 range data
const sensor::PointCloud& filtered_point_cloud, // 滤波后的点云
const transform::Rigid3d& pose_estimate, // scan matching 后的位姿估计
const Eigen::Quaterniond& gravity_alignment) { // 重力对齐旋转
if (motion_filter_.IsSimilar(time, pose_estimate)) { // 如果当前帧和上一关键帧时间/距离/角度都很接近
return nullptr; // 不插入 submap，也不生成新的 trajectory node
} // 运动滤波判断结束
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps = // 定义当前 scan 插入的 submap 列表
active_submaps_.InsertRangeData(range_data_in_local); // 将当前 range data 插入 active submaps
return absl::make_unique<InsertionResult>( // 创建插入结果对象
InsertionResult{ // 构造 InsertionResult
std::make_shared<const TrajectoryNode::Data>( // 创建 trajectory node 数据
TrajectoryNode::Data{ // 构造节点数据结构
time, // 保存节点时间
gravity_alignment, // 保存重力对齐旋转
filtered_point_cloud, // 保存滤波后的点云
{}, // 2D 中不使用 high_resolution_point_cloud
{}, // 2D 中不使用 low_resolution_point_cloud
{}, // 2D 中不使用 rotational_scan_matcher_histogram
pose_estimate}), // 保存当前 scan matching 得到的位姿
std::move(insertion_submaps)}); // 保存当前 scan 插入的 submaps
} // 函数结束变量解释：
motion_filter_：运动滤波器，用于减少冗余节点。
active_submaps_：当前正在构建的子地图管理器。
range_data_in_local：已经根据匹配位姿变换到 local 坐标系的点云数据。
insertion_submaps：当前 scan 被插入的 submap 列表。
TrajectoryNode::Data：后端 PoseGraph 中一个轨迹节点的数据。16. Motion Filter：为什么不是每一帧都插入？
Motion Filter 的作用是控制节点数量。它一般根据三个量判断当前帧是否和上一插入帧过于接近：
时间差是否足够大。
平移差是否足够大。
旋转差是否足够大。如果机器人几乎没动，但是激光还在高频输入，Cartographer 没必要把每一帧都加入图优化。否则后端会产生大量重复节点，优化变慢，内存也会增加。
可以把它理解为：
变化很小的 scan：只参与短期前端处理，不生成图优化节点。
变化足够大的 scan：插入 submap，并生成 trajectory node。这样能控制 PoseGraph 规模，让系统保持实时性。
17. Trajectory Node：后端图优化的轨迹节点
当 scan 被成功插入 submap 后，Cartographer 会生成一个 trajectory node。这个节点保存：
time：节点时间戳。
gravity_alignment：重力对齐姿态。
filtered_point_cloud：滤波后的点云。
pose_estimate：当前 scan matching 后的位姿。
insertion_submaps：当前节点插入过哪些 submap。这些信息会进入 PoseGraph。后端会根据 node 和 submap 之间的相对关系建立约束。
可以这样理解：
Local SLAM 输出：
当前 scan 的局部位姿
当前 scan 的滤波点云
当前 scan 插入的 submaps
PoseGraph 接收：
trajectory node
submap node
node-submap constraint18. Global SLAM：后端 PoseGraph 做什么？
Global SLAM 在后台运行，它的任务是全局一致性优化。Local SLAM 会不断产生 submap 和 trajectory node，但由于每次 scan matching 只依赖当前局部 submap，时间久了会出现累计误差。因此后端需要把所有节点和子地图统一放进一个图里优化。
官方文档说明，Global SLAM 是一种 GraphSLAM，本质是通过 nodes 和 submaps 之间的 constraints 构建 pose graph，然后优化整个约束图。文档还把 constraints 形象地描述成连接节点的"绳子"：局部约束保持局部结构一致，全局约束把回环位置拉回一致。
PoseGraph 里面主要有：
Submap node：子地图节点。
Trajectory node：轨迹节点。
Intra-submap constraint：当前 scan 插入当前 submap 产生的局部约束。
Loop closure constraint：历史 scan 和历史 submap 匹配成功后产生的回环约束。
Optimization problem：根据所有约束优化节点和子地图的全局位姿。19. PoseGraph 优化公式
论文中的全局优化可以写成：
argmin_{Ξm, Ξs} 1/2 Σ_ij ρ( E²(ξ_i^m, ξ_j^s; Σ_ij, ξ_ij) )变量解释：
Ξm：所有 submap 位姿的集合。
Ξs：所有 scan 节点位姿的集合。
ξ_i^m：第 i 个 submap 的全局位姿。
ξ_j^s：第 j 个 scan 节点的全局位姿。
ξ_ij：scan j 在 submap i 中匹配得到的相对位姿约束。
Σ_ij：该约束的不确定性或协方差信息。
E：当前全局位姿下，该约束产生的误差。
ρ：鲁棒核函数，用于减小异常约束的影响。这个优化问题的意义是：
调整所有 submap 和 scan 节点的全局位姿，
让所有局部约束和回环约束尽可能同时满足。论文中说明，回环优化同样被建模为非线性最小二乘问题，优化变量包括 scan 位姿和 submap 位姿，约束来自 scan 和 submap 的相对匹配结果。
20. 回环检测：scan-to-submap 约束搜索
Cartographer 的回环检测不是只看当前帧和当前 submap，而是在后台把历史节点和已经完成的 submap 做匹配。如果匹配得分足够高，就添加一个新的约束到 PoseGraph。
流程可以理解为：
某个 submap 完成
↓
不再继续插入新 scan
↓
后台线程拿历史 trajectory node 与该 submap 尝试匹配
↓
如果匹配得分足够高
↓
生成 loop closure constraint
↓
加入 PoseGraph
↓
下一次全局优化时修正轨迹和地图论文中提到，当 submap 完成后，它会参与 loop closure 的 scan matching；如果在搜索窗口中找到足够好的匹配，就把这个匹配作为 loop closing constraint 加入优化问题。为了实时完成回环匹配，Cartographer 使用 branch-and-bound 方法和每个完成 submap 的多层预计算栅格。
21. Branch-and-Bound：为什么能实时回环？
回环检测需要在比较大的搜索窗口里找 scan 和 submap 的匹配位姿。如果暴力枚举所有 x、y、θ 候选，计算量会很大。Branch-and-Bound 的思想是：
Branch：把搜索空间逐层拆分成更小区域。
Bound：估计某个区域理论上能达到的最高匹配得分。
Prune：如果这个最高得分都不可能超过当前最优解，就直接剪掉这个区域。论文中 branch-and-bound scan matching 的目标可以写成：
ξ* = argmax_{ξ ∈ W} Σ_k M_nearest(Tξ h_k)变量解释：
ξ*：搜索得到的最佳匹配位姿。
W：搜索窗口。
h_k：当前 scan 中第 k 个点。
Tξ h_k：点 h_k 经过候选位姿 ξ 变换后的坐标。
M_nearest：把坐标映射到最近栅格后的概率值。
Σ_k M_nearest(Tξ h_k)：当前候选位姿的匹配得分。这个目标和前面的 Ceres 精匹配不同。这里更像是粗搜索，通过遍历候选位姿计算匹配得分，找到一个足够好的候选，再进一步精化。论文明确给出了该匹配目标，并说明可以通过后续 Ceres scan matching 改善匹配质量。
22. 参数理解
下面这些参数对 Cartographer 效果影响很大。
TRAJECTORY_BUILDER_2D.min_range = 0.3 -- 小于 0.3m 的激光点被过滤，避免近距离噪声或机器人自身结构影响
TRAJECTORY_BUILDER_2D.max_range = 30.0 -- 大于 30m 的激光点不作为有效 hit 点，用于控制可靠测距范围
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.0 -- 超过 max_range 的射线按 5m 长度处理，用于更新空闲区域
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 -- 累积多少个 range data 后组成一个 scan 进入前端匹配
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025 -- 固定体素滤波尺寸，尺寸越小点云越密，计算越慢
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false -- 是否启用前端实时相关匹配，开启后更鲁棒但更耗时
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.0 -- 栅格占据匹配残差权重，越大越相信地图匹配
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.0 -- 平移先验权重，越大越不允许匹配结果偏离预测平移
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.0 -- 旋转先验权重，越大越不允许匹配结果偏离预测角度
POSE_GRAPH.optimize_every_n_nodes = 90 -- 每插入 90 个节点执行一次全局优化
POSE_GRAPH.constraint_builder.min_score = 0.55 -- 回环约束最低匹配得分，低于该分数不接受约束
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3 -- 后端约束搜索的采样比例，用于控制计算量官方文档中也指出，POSE_GRAPH.optimize_every_n_nodes 控制全局优化批次大小；将其设为 0 可以禁用 Global SLAM，从而专注调试 Local SLAM 行为。文档还说明，后端 constraint builder 会使用 sampling ratio 控制约束构建数量，采样太少可能漏掉约束，采样太多会降低实时性。
23. Cartographer 完整工作流程
1. ROS 数据进入系统
LaserScan / PointCloud2 / IMU / Odometry / TF
↓
2. SensorBridge 转换数据
ROS 消息转换为 Cartographer 内部数据结构
↓
3. RangeDataCollator 同步数据
整理多传感器 range data，形成时间有序点云
↓
4. PoseExtrapolator 预测位姿
根据历史位姿、IMU、里程计等预测当前 scan 位姿
↓
5. 点云预处理
min_range / max_range 过滤
returns / misses 分类
重力对齐
固定体素滤波
自适应体素滤波
↓
6. ScanMatch
使用预测位姿作为初值
可选实时相关匹配粗配准
CeresScanMatcher 精配准
↓
7. 得到 pose_estimate
当前 scan 在 local 坐标系下的估计位姿
↓
8. MotionFilter 判断
如果运动太小，则跳过插入
如果运动足够大，则继续
↓
9. InsertIntoSubmap
将当前 range data 插入 active submaps
更新 hit / miss 栅格概率
↓
10. 生成 TrajectoryNode
保存时间、点云、位姿、重力对齐信息
↓
11. PoseGraph 添加节点和约束
建立 node-submap 局部约束
↓
12. 后台回环检测
finished submap 与历史 node 做匹配
↓
13. 添加 loop closure constraint
匹配成功则添加回环约束
↓
14. 全局优化
优化所有 submap 和 trajectory node 的全局位姿
↓
15. 输出地图和轨迹
发布 map、submap、trajectory、tf24. 总结
Cartographer 是一个以 submap 和 pose graph 为核心的实时 SLAM 系统。它的整体设计不是把所有激光数据直接塞进一张全局地图里，也不是只依赖单帧之间的短期匹配，而是把建图过程拆成两个层次：前端 Local SLAM 负责局部准确，后端 Global SLAM 负责全局一致。前端不断把当前 scan 匹配到当前 active submap 上，并把匹配后的 scan 插入 submap；后端则把 scan 节点、submap 节点以及它们之间的相对位姿约束组织成 pose graph，周期性地做全局优化。这样设计的好处是前端可以保持实时，后端可以在后台慢慢修正累计漂移，最终输出一张全局更一致的地图。
Cartographer 前端最核心的入口是 LocalTrajectoryBuilder2D::AddRangeData()。它接收带时间信息的 range data，先通过 RangeDataCollator 做数据同步与排序，然后使用 PoseExtrapolator 对每个点的时间进行位姿外推。这个细节非常重要，因为激光扫描不是严格瞬时完成的，机器人在扫描过程中会运动。如果直接把一整帧点云当成同一时刻的数据，点云会出现运动形变。Cartographer 通过每个点或每批 range data 的时间信息，结合外推器预测该时刻机器人位姿，再把点转换到统一坐标系下。经过这一步后，系统再根据 min_range 和 max_range 对点做距离过滤，把可靠击中点放入 returns，把超过最大距离的射线按 missing_data_ray_length 截断后放入 misses。这两个集合后面会分别用于更新占据区域和空闲区域。
点云进入 scan matching 前，还要经过滤波。滤波的目的不是随便减少点数，而是控制计算复杂度并改善点云空间分布。Cartographer 中常见的是固定体素滤波和自适应体素滤波。固定体素滤波用固定的 voxel size 减少点数，自适应体素滤波则在最大体素尺寸限制下尽量保留目标数量的点。这样可以避免点云过密导致 Ceres 优化计算量过大，也可以避免某些近距离高密度区域对匹配产生过强影响。滤波后的点云会进入 ScanMatch()，也就是 Local SLAM 中最核心的位姿估计阶段。
ScanMatch() 的主要任务是把当前 scan 对齐到当前 active submap。它先使用 PoseExtrapolator 给出的 pose_prediction 作为初始位姿。如果开启在线相关匹配，系统会先在一定搜索窗口内寻找一个更好的初始位姿，得到 initial_ceres_pose；随后调用 CeresScanMatcher2D::Match() 做精匹配。CeresScanMatcher 的优化变量是二维位姿 [x, y, θ]，它添加了三类残差：占据空间残差、平移先验残差、旋转先验残差。占据空间残差希望当前 scan 点经过位姿变换后落在 submap 中占据概率高的位置；平移和旋转先验则限制优化结果不要过度偏离预测初值。这样做能兼顾地图匹配和运动连续性，避免匹配结果因为局部噪声而发生过大跳变。
Submap 是 Cartographer 地图表达的核心。每个 submap 是一张概率栅格地图，栅格内保存占据概率。激光终点对应 hit，会提高对应栅格的占据概率；激光射线路径经过但没有击中的区域对应 miss，会降低这些栅格的占据概率。概率更新使用 odds 形式，即 odds(p)=p/(1-p)，这样多次观测可以通过 odds 的乘法形式融合，再通过 odds^-1 转回概率，并用 clamp 限制概率范围。这个机制让 Cartographer 能够稳定地融合多帧激光观测：被多次击中的位置逐渐变成障碍物区域，被多次穿过的位置逐渐变成空闲区域，没有观测到的位置保持未知。
当 scan matching 得到 pose_estimate 后，Cartographer 并不会无条件把每一帧都插入 pose graph。它会通过 MotionFilter 判断当前帧和上一关键帧之间的时间、平移、旋转变化是否足够大。如果变化很小，当前帧可以跳过，不生成新的 trajectory node。这样可以减少冗余节点，降低后端优化压力。如果变化足够大，系统会调用 InsertIntoSubmap()，把当前 scan 插入 active submaps，并生成 TrajectoryNode::Data。这个节点保存当前时间、重力对齐信息、滤波点云和当前估计位姿。后端 PoseGraph 会基于这些节点和 submap 建立约束。
Cartographer 后端的核心是 PoseGraph。前端每生成一个 trajectory node，并把它插入某些 submap，后端就可以记录 node 和 submap 之间的相对位姿关系。这种局部约束保持了局部地图结构的连续性。但是只靠局部约束，长期运行仍然会产生累计误差。因此 Cartographer 还会在后台进行回环检测。当某个 submap 完成、不再插入新 scan 后，它会参与历史节点匹配。如果某个历史 trajectory node 能够和这个 finished submap 在搜索窗口中匹配成功，并且得分足够高，系统就会添加一个 loop closure constraint。这个回环约束会告诉后端：某个历史节点和某个子地图其实对应同一个空间区域，只是当前估计位姿存在偏差。全局优化时，PoseGraph 会同时调整所有 submap 和 trajectory node 的全局位姿，使局部约束和回环约束整体误差最小。
为了让回环检测实时运行，Cartographer 使用 branch-and-bound 思想做 scan-to-submap 匹配。它不会暴力搜索所有候选位姿，而是把搜索空间分层，用预计算栅格快速估计某个区域可能达到的最高匹配得分。如果某个区域的得分上界已经低于当前最优解，就直接剪枝，不再继续细分搜索。这样可以在较大的搜索窗口中快速找到高质量候选位姿。找到候选后，还可以继续用 CeresScanMatcher 做精匹配，提高最终约束质量。
从工程角度看，Cartographer 的关键不是某一个单独函数，而是整套链路的配合：AddRangeData() 负责接收和整理数据，PoseExtrapolator 负责预测位姿，AdaptiveVoxelFilter 负责控制点云规模，ScanMatch() 负责当前 scan 和 active submap 对齐，InsertIntoSubmap() 负责更新局部子地图并生成轨迹节点，PoseGraph 负责后台约束搜索和全局优化。理解 Cartographer 时，最重要的是抓住三条主线：第一，前端通过 scan-to-submap 构建局部一致 submap；第二，submap 使用概率栅格表达 hit 和 miss 的占据更新；第三，后端通过 node-submap constraints 和 loop closure constraints 优化全局一致性。
一句话概括：Cartographer 的本质是"局部子地图构建 + 扫描匹配 + 位姿图优化"。前端保证每个 submap 内部尽量准确，后端通过约束和回环把所有 submap 与轨迹节点拉到全局一致的位置上，最终得到实时更新、可回环修正的地图和轨迹。
版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！
欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。