(02)Cartographer源码无死角解析-(63) 2D后端优化→PoseGraph2D::ComputeConstraint()计算约束

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$

一、前言

上一篇博客中，已经对 $疑问3$ 进行了解答，目前还存在如下疑问，并没有在源码中找到答案

$疑问1:$ global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。
$疑问2:$ 为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。
$疑问4:$ ComputeConstraintsForNode() 如果返回需要优化，源码中是在哪里执行优化的呢？

不过不用太心急，随着对源码深入分析，相信都会水落石出的。该篇博客主要分析 PoseGraph2D::ComputeConstraint() 这个函数，该函数在上一篇博客中讲解的 PoseGraph2D::ComputeConstraintsForNode() 中被调用了两次，主要是计算节点与子图间的约束。那么现在就开始分析吧

二、整体注释

在进行细节分析之前，先来看一下该函数的整体注释(注意，后面有十分详细的细节分析)

/** * @brief 进行子图间约束计算, 也可以说成是回环检测 ** @param[in] node_id 节点的id * @param[in] submap_id submap的id */void PoseGraph2D::ComputeConstraint(const NodeId& node_id,const SubmapId& submap_id) {bool maybe_add_local_constraint = false;bool maybe_add_global_constraint = false;const TrajectoryNode::Data* constant_data;const Submap2D* submap;{absl::MutexLock locker(&mutex_);CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);// 如果是未完成状态的地图不进行约束计算if (!data_.submap_data.at(submap_id).submap->insertion_finished()) {// Uplink server only receives grids when they are finished, so skip// constraint search before that.return;}// 获取该 node 和该 submap 中的 node 中较新的时间const common::Time node_time = GetLatestNodeTime(node_id, submap_id);// 两个轨迹的最后连接时间const common::Time last_connection_time =data_.trajectory_connectivity_state.LastConnectionTime(node_id.trajectory_id, submap_id.trajectory_id);// 如果节点和子图属于同一轨迹, 或者时间小于阈值// 则只需进行 局部搜索窗口 的约束计算(对局部子图进行回环检测)if (node_id.trajectory_id == submap_id.trajectory_id ||node_time <last_connection_time +common::FromSeconds(options_.global_constraint_search_after_n_seconds())) {// If the node and the submap belong to the same trajectory or if there// has been a recent global constraint that ties that node's trajectory to// the submap's trajectory, it suffices to do a match constrained to a// local search window.maybe_add_local_constraint = true;}// 如果节点与子图不属于同一条轨迹 并且 间隔了一段时间, 同时采样器为true// 才进行 全局搜索窗口 的约束计算(对整体子图进行回环检测)else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {maybe_add_global_constraint = true;}// 获取节点信息数据与地图数据constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();submap = static_cast<const Submap2D*>(data_.submap_data.at(submap_id).submap.get());} // end {}// 建图时只会执行这块, 通过局部搜索进行回环检测if (maybe_add_local_constraint) {// 计算约束的先验估计值// submap原点在global坐标系下的坐标的逆 * 节点在global坐标系下的坐标 = submap原点指向节点的坐标变换const transform::Rigid2d initial_relative_pose =optimization_problem_->submap_data().at(submap_id).global_pose.inverse() *optimization_problem_->node_data().at(node_id).global_pose_2d;// 进行局部搜索窗口 的约束计算 (对局部子图进行回环检测)constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, constant_data, initial_relative_pose);} // 定位时才有可能执行这块else if (maybe_add_global_constraint) {// 全局搜索窗口 的约束计算 (对整体子图进行回环检测)constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id, constant_data);}}

三、函数输入

该函数的输入就可以说是十分简单了，就是 NodeId 与 SubmapId 对象。接着可以看到如下代码：

bool maybe_add_local_constraint = false;bool maybe_add_global_constraint = false;const TrajectoryNode::Data* constant_data;const Submap2D* submap;

这几个变量比较重要的，constant_data 与 submap 就不重复讲解了，前面涉及太多了。关于 maybe_add_local_constraint 与 maybe_add_global_constraint 这两个变量初始默认设置为 false，其决定了后续建立的约束是全局约束还是局部约束，都是由这两个变量控制的，十分重要，这里重点提示一下、

四、求得最新时间

absl::MutexLock locker(&mutex_);CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);// 如果是未完成状态的地图不进行约束计算if (!data_.submap_data.at(submap_id).submap->insertion_finished()) {// Uplink server only receives grids when they are finished, so skip// constraint search before that.return;}// 获取该 node 和该 submap 中的 node 中较新的时间const common::Time node_time = GetLatestNodeTime(node_id, submap_id);// 两个轨迹的最后连接时间const common::Time last_connection_time =data_.trajectory_connectivity_state.LastConnectionTime(node_id.trajectory_id, submap_id.trajectory_id);

该部分代码先判断一下 submap_id 在 data_.submap_data 对应的子图是否为完成状态，如果不是，则直接 return，不建立约束。也就是说，ComputeConstraint() 函数只会建立节点与完成子图之间的约束。

接着获得 node_id 节点与 submap_id 对应子图中所有节点最大的时间，记为 node_time。同时还会计算 node_id.trajectory_id 与 submap_id.trajectory_id 这两条轨迹最后的连接时间，该时间是在 TrajectoryConnectivityState::Connect() 函数中记录的。

这里需要提及一点：如果是以建图模式其中，那么 node_id.trajectory_id 与 submap_id.trajectory_id 是相同的，后面会了解到 last_connection_time 没有意义会被忽略。如果是纯定位模式，那么 node_id.trajectory_id 与 submap_id.trajectory_id 是不一致的，此时 last_connection_time 会记录他们最后的连接时间。

五、选择global或者local约束

// 如果节点和子图属于同一轨迹, 或者时间小于阈值// 则只需进行 局部搜索窗口 的约束计算(对局部子图进行回环检测)if (node_id.trajectory_id == submap_id.trajectory_id ||node_time <last_connection_time +common::FromSeconds(options_.global_constraint_search_after_n_seconds())) {// If the node and the submap belong to the same trajectory or if there// has been a recent global constraint that ties that node's trajectory to// the submap's trajectory, it suffices to do a match constrained to a// local search window.maybe_add_local_constraint = true;}// 如果节点与子图不属于同一条轨迹 并且 间隔了一段时间, 同时采样器为true// 才进行 全局搜索窗口 的约束计算(对整体子图进行回环检测)else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {maybe_add_global_constraint = true;}// 获取节点信息数据与地图数据constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();submap = static_cast<const Submap2D*>(data_.submap_data.at(submap_id).submap.get());

首先来看第一个条件 node_id.trajectory_id == submap_id.trajectory_id，也就是建图模式下，表示可以使用局部约束，即 maybe_add_local_constraint = true。如果该条件满足，因为其是 || 的关系，所以不会对条件二：

node_time <last_connection_time +common::FromSeconds(options_.global_constraint_search_after_n_seconds())

进行判断了，该条件的意思大致就是说，在上次两轨迹连接一定时长 global_constraint_search_after_n_seconds 内，则使用 local 约束。需要注意，对于条件二只有纯定位的时候才会使用到。

如果第一个 if 条件不成立，则会进入到 else if 的条件判断：

// 如果节点与子图不属于同一条轨迹 并且 间隔了一段时间, 同时采样器为true// 才进行 全局搜索窗口 的约束计算(对整体子图进行回环检测)else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {maybe_add_global_constraint = true;}

这里使用到的了一个全局定位的采样器，也就是说，并不是每个节点都会使用全局约束，而是以一定频率，或者说间隔几个节点才使用全局约束，后面可以了解到，这样是为了减少后端优化的计算量。

根据前面的分析，并不是 maybe_add_local_constraint 为 false，maybe_add_global_constraint 就一定为true，他们可能同时为 false。也就是说，一个节点与子图即不建立全局约束，也不建立局部约束。但是不会出现都为 true的情况。后面的代码就比较简单了：

// 获取节点信息数据与地图数据constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();submap = static_cast<const Submap2D*>(data_.submap_data.at(submap_id).submap.get());

六、建立local约束

如果 maybe_add_local_constraint 条件成立，其会执行如下代码：

// 计算约束的先验估计值// submap原点在global坐标系下的坐标的逆 * 节点在global坐标系下的坐标 = submap原点指向节点的坐标变换const transform::Rigid2d initial_relative_pose =optimization_problem_->submap_data().at(submap_id).global_pose.inverse() *optimization_problem_->node_data().at(node_id).global_pose_2d;// 进行局部搜索窗口 的约束计算 (对局部子图进行回环检测)constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, constant_data, initial_relative_pose);

这里记 optimization_problem_->submap_data().at(submap_id).global_pose 为 S u b m a p s u b m a p g l o b a l \mathbf {Submap}^{global}_{submap}Submapsubmapglobal​，记 optimization_problem_->node_data().at(node_id).global_pose_2d 为 R o b o t t r a c k i n g g l o b a l \mathbf {Robot}^{global}_{tracking}Robottrackingglobal​，那么上式对应得数学公式为：
R o b o t t r a c k i n g s u b m a p= [S u b m a p s u b m a p g l o b a l] − 1∗R o b o t t r a c k i n g g l o b a l (01) \color{Green} \tag{01} \mathbf {Robot}^{submap}_{tracking} = [\mathbf {Submap}^{global}_{submap}]^{-1} * \mathbf {Robot}^{global}_{tracking} Robottrackingsubmap​=[Submapsubmapglobal​]−1∗Robottrackingglobal​(01)也就是说，源码中的 initial_relative_pose 等价 R o b o t t r a c k i n g s u b m a p \mathbf {Robot}^{submap}_{tracking}Robottrackingsubmap​，表示 node_id 节点在 submap 系下的位姿。

随后会调用函数 constraint_builder_.MaybeAddConstraint()，完成约束的建立，关于 constraints::ConstraintBuilder2D constraint_builder_ 后续我们再进行细节分析。

七、建立global约束

如果 maybe_add_global_constraint 为 true，那么其会调用 constraint_builder_.MaybeAddGlobalConstraint() 函数添加全局约束，具体细节，也同样在后面进行分析。

八、结语

该篇博客熟悉了 PoseGraph2D::ComputeConstraint 函数，可以知道其主要是根据 node_id 与 submap_id 建立约束，约束分为 local_constraint 与 global_constraint 两种。其中建立约束的函数分别为 constraint_builder_.MaybeAddConstraint() 与 constraint_builder_.MaybeAddGlobalConstraint()。

该篇博客的篇幅似乎还比较短，那么就来分析一下 constraint_builder_ 是如何构建的，构建时又传递了那些参数。在 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 文件中，找到 PoseGraph2D 的构造函数如下所示：

/** * @brief 构造函数 ** @param[in] options 位姿图的参数配置 * @param[in] optimization_problem 优化问题 * @param[in] thread_pool map_builder中构造的线程池 */PoseGraph2D::PoseGraph2D(const proto::PoseGraphOptions& options,std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,common::ThreadPool* thread_pool): options_(options),optimization_problem_(std::move(optimization_problem)),constraint_builder_(options_.constraint_builder_options(), thread_pool),thread_pool_(thread_pool) {// overlapping_submaps_trimmer_2d 在配置文件中被注释掉了, 没有使用if (options.has_overlapping_submaps_trimmer_2d()) {const auto& trimmer_options = options.overlapping_submaps_trimmer_2d();AddTrimmer(absl::make_unique<OverlappingSubmapsTrimmer2D>(trimmer_options.fresh_submaps_count(),trimmer_options.min_covered_area(),trimmer_options.min_added_submaps_count()));}}

可以看到，构建 constraints::ConstraintBuilder2D 对象时，传递了一个线程池参数，且该线程池与 PoseGraph2D 中使用的是同一个。另外还传递了一些配置参数，该写配置参数可以在 src/cartographer/configuration_files/pose_graph.lua 中找到，如下所示：

-- 约束构建的相关参数constraint_builder = {sampling_ratio = 0.3, -- 对局部子图进行回环检测时的计算频率, 数值越大, 计算次数越多max_constraint_distance = 15.,-- 对局部子图进行回环检测时能成为约束的最大距离min_score = 0.55, -- 对局部子图进行回环检测时的最低分数阈值global_localization_min_score = 0.6,-- 对整体子图进行回环检测时的最低分数阈值loop_closure_translation_weight = 1.1e4,loop_closure_rotation_weight = 1e5,log_matches = true, -- 打印约束计算的log-- 基于分支定界算法的2d粗匹配器fast_correlative_scan_matcher = {linear_search_window = 7.,angular_search_window = math.rad(30.),branch_and_bound_depth = 7,},-- 基于ceres的2d精匹配器ceres_scan_matcher = {occupied_space_weight = 20.,translation_weight = 10.,rotation_weight = 1.,ceres_solver_options = {use_nonmonotonic_steps = true,max_num_iterations = 10,num_threads = 1,},},

该些参数的具体使用方式，在后面再进行详细介绍，该篇博客的字数目前也差不多了，所以就到这里为止。