Cartographer 在纯定位场景中，若沿用建图模式的后端流程，会产生大量冗余计算。通过代码级和参数级优化，可以在不牺牲精度的前提下显著降低资源占用，实现稳定实时的定位。
一、引入定位模式标志，实现行为分离
在 PoseGraph2D 中新增 bool localization_mode_ 标志，与建图模式严格区分。定位模式下：
-
禁止新增子图，所有
AddSubmap调用被跳过。 -
已有的子图（来自预先构建的地图）状态设为
kFinished且不可变。 -
ComputeConstraintsForNode中仅与这些冻结子图进行约束计算，不产生新的
INTRA_SUBMAP约束。
此改动将定位后端的变量规模锁定在常量范围，避免随时间增长而膨胀。
二、动态触发：不让每帧数据都进入后端
PoseGraph2D::AddNode 默认每帧都会触发约束计算。定位模式应增加前置判断：
-
位移触发
：仅当机器人移动超过阈值（如 0.3 m）时，才调用
AddWorkItem进入后端队列。 -
时间触发
：距离上次优化超过 1 秒时强制执行，避免静止时系统停滞。
代码需维护 last_optimized_pose 和 last_optimized_time，符合条件的节点才被传递。此改动能将后端调用频率从传感器帧率（10~20 Hz）降至 1~2 Hz，CPU 占用显著下降。
三、冻结子图与稀疏约束计算
定位模式下的约束搜索应限制在空间邻近区域内。具体实现：
-
预计算每个冻结子图的中心位姿（由其包含的所有节点的平均全局位姿得到），并缓存于
submap_center_cache。 -
对于当前节点，计算其与所有冻结子图中心的距离，使用优先队列选出距离最近的 K 个子图（K 可取 2~5）。
-
仅对这些选中的子图执行
ComputeConstraint，其余的跳过。
该优化将单帧约束计算从 O(N_submaps) 降为 O(K)，对于包含数百子图的大规模地图，单帧耗时可从数十毫秒降至 5 毫秒以内。
四、前端点云下采样与体素滤波
Cartographer 前端的 LocalTrajectoryBuilder2D 中，点云体素滤波尺寸 voxel_filter_size 默认 0.05 m。定位模式下可放宽至 0.10~0.15 m，甚至采用自适应策略：当匹配分数稳定且较高时，动态增大体素尺寸。
此外，可增加距离滤波：剔除距离大于 20~30 m 的点云，减少无效匹配点。同时，在 RealTimeCorrelativeScanMatcher 前对点云进行一次均匀采样，进一步降低匹配计算量。
五、约束搜索窗口参数调优
constraint_builder_2d 中的扫描匹配参数在建图与定位模式下应差异化配置。定位时大幅缩小搜索窗口：
-
fast_correlative_scan_matcher_options.linear_search_window：从 10 m 降至 0.5 m
-
angular_search_window：从 30° 降至 10°~15°
-
ceres_scan_matcher_options迭代次数：从 10 次减至 4 次，
ceres_solver_options.max_num_iterations减半
这些参数改动能成倍降低每帧扫描匹配耗时，且定位精度几乎不受影响（因为初始猜测已由里程计或前端匹配提供）。
六、后端优化降频与滑动窗口
定位模式下，后端优化的触发条件应当更加宽松。修改 num_nodes_since_last_loop_closure 的比较阈值：
-
将
options_.optimize_every_n_nodes()从默认 10 提高至 50 甚至 100。 -
同时配合滑动窗口：在
OptimizationProblem2D中只保留最近 N 个节点（如 N=200），更早的节点被边缘化为先验信息固定，不再参与变量更新。代码需实现TrimTrajectoryNode和边缘化舒尔补。
此改动确保优化问题的规模恒定，避免长期运行后单次优化耗时线性增长。
七、线程池与异步队列优化
定位模式下，ConstraintBuilder2D 内部的线程池数量可以缩减（例如从 hardware_concurrency 降至 2~3），因为并行任务量减少。同时，在 DrainWorkQueue 中为任务队列添加优先级：优先处理距离最近的关键帧约束，延迟非紧急的全局约束计算，保证前端不被积压任务阻塞。
总结
通过上述代码级改动和参数调优，Cartographer 在定位模式下的 CPU 占用可降低 60%~80%，单帧约束计算延迟控制在 5 ms 以内，长期漂移可控。核心原则：将地图静态化、约束稀疏化、前端轻量化、后端低频化。这些优化对原代码侵入小，却能显著提升定位的实时性与稳定性，使其能够稳定运行于嵌入式平台或资源受限的机器人系统中。