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

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

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

一、前言

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

$\color{red}疑问1:$ global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。
$\color{red}疑问2:$ 为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。
$\color{red}疑问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 为 $\mathbf {Submap}^{global}_{submap}$ ，记 optimization_problem_->node_data().at(node_id).global_pose_2d 为 $\mathbf {Robot}^{global}_{tracking}$ ，那么上式对应得数学公式为：
$\color{Green} \tag{01} \mathbf {Robot}^{submap}_{tracking} = [\mathbf {Submap}^{global}_{submap}]^{-1} * \mathbf {Robot}^{global}_{tracking}$ 也就是说，源码中的 initial_relative_pose 等价 $\mathbf {Robot}^{submap}_{tracking}$ ，表示 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,
      },
    },

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