在激光SLAM的定位场景中，几何匹配在长走廊、空旷区域或对称环境中容易失效。反光柱、路标等人造特征能提供额外的强度信息，辅助定位。Cartographer 的 FastCorrelativeScanMatcher2D 支持将强度特征融合到匹配过程中。本文描述完整的技术路径：从建图阶段高反地图的两种生成方式，到权重网格的预计算，再到在线定位时的融合匹配。

一、建图阶段：高反地图的两种生成方式

1.1 离线方式：使用 Assets Writer 提取强度信息

Cartographer 在建图过程中生成的 .pbstream 文件包含位姿图、子图概率网格和优化后的轨迹，但不保留原始点云的强度信息。为了生成一张包含反射强度信息的高反地图，Cartographer 提供了 assets_writer 工具。该工具在建图完成后，根据优化后的轨迹重新处理原始传感器数据包（.bag 或 .pcd 文件），输出带强度的点云文件（如 PLY 格式）。具体流程：

• 输入：建图时录制的原始数据包、优化后的 .pbstream 文件。

• 处理：assets_writer 将每个子图对应的点云按照轨迹位姿投影到世界坐标系，并保留每个点的反射强度值。

• 输出：一系列 PLY 文件或一个合并的点云地图，其中每个点包含 (x, y, z, intensity)。

这种离线方式的优点是最终的全局高反点云使用了优化后的全局一致位姿，因此几何精度高。缺点是需要保存原始数据包，并需要额外的后处理步骤，无法在建图过程中实时获得高反地图。

1.2 在线方式：实时累积高反点

另一种方式是在建图过程中实时生成高反地图。Cartographer 的前端 LocalTrajectoryBuilder2D 每处理一帧激光数据时，会得到该帧相对于子图的位姿估计。结合 PoseGraph2D 提供的当前轨迹的全局位姿，可以将当前帧中强度高于阈值的点转换到全局坐标系下，并累积到一张全局高反点图中。具体步骤：

• 在 AddNode 或 LocalTrajectoryBuilder2D::AddAccumulatedRangeData 中，获取当前帧点云及其强度值。

• 对于每个强度大于预设阈值（例如 200）的点，利用当前的全局位姿将其转换到世界坐标系。

• 将转换后的高反点存储在一个内存容器中（如 std::vector<Eigen::Vector3f>），并可选择性进行体素滤波以降低密度。

• 建图结束后，将这些高反点保存为 PLY 或 PCD 文件。

在线方式的优点是不需要保存原始数据包，也无需后处理步骤，可以在建图完成的同时直接获得高反点云。缺点是在线位姿尚未经过全局优化，累积误差可能导致高反点的局部一致性略低于离线方式。

两种方式可以互补：对于要求高精度的高反地图，使用离线方式；对于快速建图或低算力平台，使用在线方式。实际工程中也可先在线生成粗略高反地图，再用离线方式精化。

1.3 从高反点云生成权重网格

无论采用哪种方式获得高反点云，后续生成权重网格的步骤是相同的。权重网格是一张与概率网格尺寸相同、像素对齐的二维数组，每个单元格存储一个浮点数，表示该位置的特征显著性。生成步骤：

• 将高反点云按照概率网格的分辨率和原点进行离散化。

• 对于每个像素，统计落入该像素内的所有点的强度值。如果最大强度超过预设阈值，则将该像素的权重设为 w > 1（如 1.5 或 2.0）；否则权重设为 1.0。

• 可选地，对权重网格进行形态学膨胀，以增加匹配的宽容度。

生成的权重网格与概率网格一起序列化保存，供定位时加载。

二、在线定位：权重网格与强度扫描的融合

2.1 权重网格的预计算金字塔

为了支持分支定界搜索，需要对权重网格构建与概率网格相同的多分辨率金字塔。每一层存储每个像素周围 width × width 窗口内的最大权重。Cartographer 使用滑动窗口最大值算法同时处理概率网格和权重网格：

• 水平方向滑动：对每一行，分别维护概率值和权重值的最大值队列。

• 垂直方向滑动：对中间结果再次滑动，得到每个像素邻域内的最大概率和最大权重。

最终，每一层预计算网格包含两个数组：cells（最大概率）和 weight_cells（最大权重）。

2.2 匹配得分计算公式

在候选位姿的得分计算中，融合强度信息后的得分为：

text

score = ( Σ(P) + Σ(P × (W – 1)) ) / N

其中 P 是点命中像素在概率网格中的值，W 是相同像素在权重网格中的值，N 是点云的点数。第一项为几何得分，第二项为加权得分。当 W = 1 时，加权贡献为零；当 W > 1 时，该点的概率贡献被放大。

分支定界搜索中，上界可用预计算的最大概率 max_P 和最大权重 max_W 计算：upper_bound = max_P × max_W。该上界紧于纯几何的上界，因此剪枝更高效。

2.3 强度扫描与自适应缩放

当激光雷达提供强度信息时，MatchWithSearchParametersandIntensity 函数处理带有强度值的点云。匹配过程：

• 根据候选位姿变换点云，获取每个像素的预计算概率 P 和预计算权重 W。

• 如果 W 大于阈值且该点的原始强度也大于强度阈值，则判定为一次有效的特征匹配，统计反射板匹配数。

• 加权贡献计算：Δ = (W – 1) × P，累加得到 sum_weighted。

• 最终得分 = (sum_prob + sum_weighted) / N。

自适应缩放根据几何得分的高低调整加权贡献系数，目的是在几何信息充分时不过度依赖特征，在退化区域则适当放大特征影响。

三、工程应用要点

• 权重网格必须与概率网格的坐标系、分辨率、原点完全一致。

• 在线累积高反点时建议使用体素滤波（如 0.05m）以降低内存占用。

• 权重值建议设置在 1.5–2.0 之间，强度阈值需根据传感器型号标定。

• 自适应缩放系数需结合实际环境调整。

四、总结

Cartographer 将强度特征融合到匹配中的技术路径包括建图阶段的高反地图生成和定位阶段的权重网格匹配。高反地图可通过离线 assets_writer 后处理生成，也可在线实时累积高反点。无论哪种方式，最终生成的权重网格与概率网格对齐，在预计算金字塔和分支定界搜索中实现融合匹配。这一方案不改变原有匹配框架的复杂度，仅增加常数倍内存和计算，能够有效提升退化环境中的定位稳定性。对于需要利用人工路标的自主移动机器人，该方案具有实际工程价值。