CHANGE LOG
2024/04/17: 1. 修改代码中的一些错误；2. 添加一些优化改进策略；
新版本的 Cartographer 是没有初始全局重定位的功能的，不过旧版本中有提供一个简单的实现参考，可以看我先前的文章《Cartographer 中的简易全局重定位方法》，但是旧版源码中并没有实际使用这个函数。其实这个函数是不够用的，地图大点都要匹配很久。
下面将会介绍如何在新版 carto 中添加上全局重定位功能，并提供接口供 cartographer_ros 调用。同时也会介绍如何用 carto 自带的线程池加速重定位的计算。
可以先看看原版重定位函数的实现，我删除了一些检查和日志，只关注核心代码：
bool PerformGlobalLocalization(
const float cutoff,
const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter,
const std::vector<cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& matchers,
const cartographer::sensor::PointCloud& point_cloud,
transform::Rigid2d* const best_pose_estimate, float* const best_score) {
// 输入有效性检查 ...
*best_score = cutoff;
transform::Rigid2d pose_estimate;
// 对输入点云做滤波
const sensor::PointCloud filtered_point_cloud = voxel_filter.Filter(point_cloud);
bool success = false;
// 遍历匹配器，对所有子图做全图匹配
for (auto& matcher : matchers) {
float score = -1;
transform::Rigid2d pose_estimate;
if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score, &pose_estimate)) {
*best_score = score;
*best_pose_estimate = pose_estimate;
success = true;
}
}
return success;
}
可以看到，原版的实现真的很简单，就是拿一帧点云，对每一幅 Submap 都做一次全图匹配。这里用的匹配器 是 carto 用来做回环检测的匹配器，匹配的成功率还是很不错的，不过匹配后还位姿还会差一点点才能对上，需要再调用一次 匹配器进行精确匹配得到正确的位姿。
全局重定位功能实现过程
因为全局重定位需要读取所有的 Submap 数据和最新的点云数据，所以把代码加在 模块里是最方便的，cartographer_ros 里也能调用到。
首先就是接口的定义，cartographer_ros 里能通过 访问到 carto 的 实例，而 又能访问到 ，所以我们只需要在 接口类中添加全局重定位的虚函数，然后在他的派生类中添加对应的实现。
中添加的全局重定位的接口，我们的接口不需要传入点云滤波器，内部直接调体素滤波就行了。
// 文件 cartographer\mapping\pose_graph.h
class PoseGraphInterface
{
public:
virtual bool PerformGlobalLocalization(
float cutoff, const cartographer::sensor::PointCloud& point_cloud,
transform::Rigid2d* best_pose_estimate, float* best_score) {};
};然后到 里添加对应的函数。
// 文件 cartographer\mapping\internal\2d\pose_graph_2d.h
class PoseGraph2D : public PoseGraph
{
bool PerformGlobalLocalization(
float cutoff, const cartographer::sensor::PointCloud& point_cloud,
transform::Rigid2d* best_pose_estimate, float* best_score) override;
};到这里先介绍一下这个重定位函数的流程，然后逐个部分给出实现代码。
创建线程池；
点云过一次体素滤波；
为每一个 Submap 创建 FastCorrelativeScanMatcher 匹配器，在线程池中执行，并等待所有匹配器创建完成；
遍历每一个匹配器，调用 MatchFullSubmap 方法，记录匹配结果和分数，在线程池中执行，并等待所有匹配完成；
找出匹配得分最高的位姿，创建 CeresScanMatcher2D 匹配器再次匹配得到准确的位姿。
无论是创建匹配器还是全图匹配，计算量都很大，使用线程池来执行这些操作是必要的，我们直接用 carto 本身的线程池，线程池就用 CPU 核心数作为线程数量。
auto pool = std::make_unique<common::ThreadPool>(
std::thread::hardware_concurrency());
assert(pool != nullptr);点云滤波也是必须的，一两千个点云的计算量非常大，按 0.05 米的参数过一遍体素滤波后只剩几百个点，匹配速度能提升一个数量级，计算结果几乎也没差别。
const sensor::PointCloud filtered_point_cloud = sensor::VoxelFilter(0.05).Filter(point_cloud);
接着是为每一幅 Submap 创建匹配器，这一步需要在线程池里加速执行。
int32_t submap_size = static_cast<int>(data_.submap_data.size());
absl::BlockingCounter created_counter{submap_size};
std::vector<std::shared_ptr<scan_matching::FastCorrelativeScanMatcher2D>> matchers;
std::vector<const cartographer::mapping::Grid2D*> submaps;
matchers.resize(submap_size);
submaps.resize(submap_size);
size_t index = 0;
for (const auto& submap_id_data : data_.submap_data) {
if (submap_id_data.id.trajectory_id != 0) {
continue;
}
auto task = std::make_unique<common::Task>();
task->SetWorkItem([this, &matchers, &created_counter, index, submap_id = submap_id_data.id, &submaps] {
submaps.at(index) = static_cast<const Submap2D*>(data_.submap_data.at(submap_id).submap.get())->grid();
matchers.at(index) = std::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
*(submaps.at(index)),
options_.constraint_builder_options().fast_correlative_scan_matcher_options());
created_counter.DecrementCount();
});
pool->Schedule(std::move(task));
index++;
}
created_counter.Wait();匹配器创建好之后就是调用全图匹配了，这一步同样需要在线程池里执行。
// 存储所有匹配分数和结果
std::vector<float> score_set;
std::vector<transform::Rigid2d> pose_set;
score_set.resize(submap_size);
pose_set.resize(submap_size);
absl::BlockingCounter matched_counter{submap_size};
std::atomic_bool has_matched{false};
for (size_t i = 0; i < matchers.size(); i++) {
// 循环创建任务放入线程池
auto task = std::make_unique<common::Task>();
task->SetWorkItem([i, &filtered_point_cloud, &matchers, &score_set, &pose_set, cutoff, &matched_counter, &has_matched] {
float score = -1;
transform::Rigid2d pose_estimate;
if (matchers[i]->MatchFullSubmap(filtered_point_cloud, cutoff, &score, &pose_estimate)) {
score_set.at(i) = score;
pose_set.at(i) = pose_estimate;
has_matched = true;
}
matched_counter.DecrementCount();
});
pool->Schedule(std::move(task));
}
matched_counter.Wait();
if (!has_matched)
{
return has_matched;
}
// 遍历结果，找出分数最高的位姿
int max_position = max_element(score_set.begin(), score_set.end()) - score_set.begin();
*best_score = score_set[max_position];
*best_pose_estimate = pose_set[max_position];FastCorrelativeScanMatcher2D 匹配器找出可能性最高的位姿后，就是最后一步使用 CeresScanMatcher2D 匹配器去匹配精确位姿了。
auto csm = std::make_unique<scan_matching::CeresScanMatcher2D>(
options_.constraint_builder_options().ceres_scan_matcher_options());
ceres::Solver::Summary unused_summary;
csm->Match(best_pose_estimate->translation(), *best_pose_estimate,
filtered_point_cloud, *(submaps.at(max_position)),
best_pose_estimate, &unused_summary);
return has_matched;
该函数的使用方法也很简单，我的方法是纯定位模式启动后，等加载完 pbstream 地图文件和缓存有效一帧点云数据，通过 暴露的接口调用全局重定位函数，得到有效的匹配位姿后缓存起来，然后析构掉当前的 Node 实例，重新构造一个并使用匹配到的位姿作为初始化位姿来重新开始纯定位模式。 可以把这个接口封装成一个 RPC 请求，grpc 或者是 ros service 都可以，外部调用请求，cartographer_ros 计算返回结果，然后外部再重启纯定位模式。
其实这个方法还是有缺陷的，它只能在机器人停在原地不动的时候才能用，不然等匹配完，机器人已经跑别的地方去了。
后续优化
（一）限制并发全图匹配的子图数量。文章中的方法是一次性为所有子图创建匹配器，当子图数量过多时，内存消耗会非常大，改为每次只对有限数量的子图进行匹配，可以是cpu核心数量。前一轮的 N 副子图匹配得到的 score 还能作为下一轮子图匹配的 cutoff，有效利用分支定界的特点加速重定位。
（二）提供大概的先验位置减少需要匹配的子图数量。可以是用户在上位机点击一个位置，这时候的重定位就可以只搜索用户指定位置半径3米内的子图，也可以是第三方系统提供一个先验位置，例如我工作的项目会在场地里布置大量的wifi发射器，并配置一个专用的接收器进行快速漫游切换网络，在建图阶段为每一个子图标记上对应 WIFI 发射器的 MAC 地址，后续重定位就能直接用当前连接的 WIFI 发射器的 MAC 地址进行子图筛选。
（三）使用新建轨迹和删除轨迹去应用计算出来的新位姿。
