cartographer-ros阅读梳理（五）后端部分-关于pose_graph节点的建立与维护

一、前言

        局部定位的local_trajectory_builder构建完成后，另一个在map_builder私有类里面构建的std::unique_ptr<PoseGraph> pose_graph_完成了另一部分工作，包含全局位姿优化与回环检测等，这篇文章将对pose_graph的构成和功能进行分析，他主要完成了以下几个功能：一是接收local_trajectory_builder建立的子图，放到全局位姿图里，根据子图之间的关系建立节点和图的约束；二是利用传感器信息、里程计、回环检测等实现对后端的全局优化，并利用这些优化结果来修正位姿图中的节点信息

        本文主要对PoseGraph2D的构建过程进行梳理

二、pose_graph节点的建立

        类型PoseGraph2D是从类PoseGraph派生而来的，而PoseGraph又继承了接口类PoseGraphInterface，后两者都是抽象类，纯虚函数的使用继承自PoseGraph2D

class PoseGraph2D : public PoseGraph {
 public:
  //构造函数 
  //输入参数:构造参数，当前优化问题的对象指针，线程池
  PoseGraph2D(
      const proto::PoseGraphOptions& options,
      std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
      common::ThreadPool* thread_pool);
  ~PoseGraph2D() override;
  
  //屏蔽
  PoseGraph2D(const PoseGraph2D&) = delete;
  PoseGraph2D& operator=(const PoseGraph2D&) = delete;

  //添加因子图节点
  NodeId AddNode(
      //constant_data为轨迹节点数据，内含重力向量、重力矫正后的点云数据以及局部位姿信息
      std::shared_ptr<const TrajectoryNode::Data> constant_data,
      int trajectory_id,
      const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
      LOCKS_EXCLUDED(mutex_);

 private:
  //位姿图配置选项
  const proto::PoseGraphOptions options_;
  //全局优化回调函数
  GlobalSlamOptimizationCallback global_slam_optimization_callback_;
  //线程锁
  mutable absl::Mutex mutex_;
  absl::Mutex work_queue_mutex_;

  // If it exists, further work items must be added to this queue, and will be
  // considered later.
  //任务队列，把还没有优化的东西存进去，任何需要被执行的任务都需要添加到这个队列当中，不会直接执行，会被调度执行
  std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(work_queue_mutex_);

  // We globally localize a fraction of the nodes from each trajectory.
  //一个哈希表，用于对各个轨迹上的部分节点进行全局定位
  absl::flat_hash_map<int, std::unique_ptr<common::FixedRatioSampler>>
      global_localization_samplers_ GUARDED_BY(mutex_);

  // Number of nodes added since last loop closure.
  //上次回环检测之后新加的节点数量
  int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0;

  // Current optimization problem.
  //当前优化问题的对象指针
  std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
  //约束构造器，用于异步的计算约束
  constraints::ConstraintBuilder2D constraint_builder_;

  // Thread pool used for handling the work queue.
  //线程池
  common::ThreadPool* const thread_pool_;

  // List of all trimmers to consult when optimizations finish.
  //用于指导地图修饰的修饰器
  std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);

  PoseGraphData data_ GUARDED_BY(mutex_);

  ValueConversionTables conversion_tables_;
};

        pose_graph_2d.h文件，这里只拿了一些和位姿图节点建立有关的东西，顺便到.cc文件里看了一眼，东西也太多了，一千多行，只能顺着.h文件的函数慢慢梳理了

//构造函数
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) {
  //选项，用于根据子图之间重叠的部分修饰地图
  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()));
  }
}

PoseGraph2D::~PoseGraph2D() {
  WaitForAllComputations();
  absl::MutexLock locker(&work_queue_mutex_);
  CHECK(work_queue_ == nullptr);
}

        构造函数与析构函数，主要是写入配置文件options、建立待优化问题optimization_problem_、线程池thread_pool，构造函数里有一块关于overlapping_submaps的选项，是用来根据子图之间的重叠部分修饰子图的，这个选项好像默认是不开启的，里面调用了AddTrimmer这个函数

//将一个拉姆达表达式添加到工作队列中
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
  // C++11 does not allow us to move a unique_ptr into a lambda.
  PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
  //通过函数AddWorkItem将一个lambda表达式添加到工作队列中
  AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    trimmers_.emplace_back(trimmer_ptr);
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

//添加任务至任务队列等待执行
void PoseGraph2D::AddWorkItem(
    const std::function<WorkItem::Result()>& work_item) {
  absl::MutexLock locker(&work_queue_mutex_);
  //如果任务队列为空，初始化
  if (work_queue_ == nullptr) {
    work_queue_ = absl::make_unique<WorkQueue>();
    //将 执行一次DrainWorkQueue()的任务 放入线程池，等待计算
    auto task = absl::make_unique<common::Task>();
    task->SetWorkItem([this]() { DrainWorkQueue(); });
    //线程池等待调度
    thread_pool_->Schedule(std::move(task));
  }
  const auto now = std::chrono::steady_clock::now();
  //添加任务至队里
  work_queue_->push_back({now, work_item});
  kWorkQueueSizeMetric->Set(work_queue_->size());
  kWorkQueueDelayMetric->Set(
      std::chrono::duration_cast<std::chrono::duration<double>>(
          now - work_queue_->front().time)
          .count());
}

       pose_graph的任务队列管理主要运用了一个

std::unique_ptr<WorkQueue> work_queue_指针，任何需要被执行的任务都需要添加到这个队列当中，不会直接执行，会被调度执行 其中调用了DrainWorkQueue函数，用来给队列添加任务放入线程池

//在调用线程上执行工作队列中的待处理任务, 直到队列为空或需要优化时退出循环
void PoseGraph2D::DrainWorkQueue() {
  bool process_work_queue = true;
  size_t work_queue_size;
  while (process_work_queue) {
    std::function<WorkItem::Result()> work_item;
    {
      //函数一直执行，知道队列为空
      absl::MutexLock locker(&work_queue_mutex_);
      if (work_queue_->empty()) {
        work_queue_.reset();
        return;
      }
      //pop第一个任务
      work_item = work_queue_->front().task;
      work_queue_->pop_front();
      work_queue_size = work_queue_->size();
      kWorkQueueSizeMetric->Set(work_queue_size);
    }
    //执行任务
    //同时也改变了循环条件,推出新欢
    process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
  }
  LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
  // We have to optimize again.
  //有时还没有完全处理完队列中的所有任务，就因为状态run_loop_closure_再次为true开启新的闭环检测而退出。此时需要重新注册一下回调函数
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
        HandleWorkQueue(result);
      });
}

//处理任务队列回调函数
//当约束构建器在后台完成了一些工作之后，就会调用这个函数HandleWorkQueue，来调度队列里的任务
void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result) {
  {
    absl::MutexLock locker(&mutex_);
    data_.constraints.insert(data_.constraints.end(), result.begin(),
                             result.end());
  }
  RunOptimization();
  
  //如果提供了全局优化之后的回调函数，则调用回调函数
  if (global_slam_optimization_callback_) {
    std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
    std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
    {
      absl::MutexLock locker(&mutex_);
      const auto& submap_data = optimization_problem_->submap_data();
      const auto& node_data = optimization_problem_->node_data();
      for (const int trajectory_id : node_data.trajectory_ids()) {
        if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
            submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
          continue;
        }
        trajectory_id_to_last_optimized_node_id.emplace(
            trajectory_id,
            std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
        trajectory_id_to_last_optimized_submap_id.emplace(
            trajectory_id,
            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
      }
    }
    global_slam_optimization_callback_(
        trajectory_id_to_last_optimized_submap_id,
        trajectory_id_to_last_optimized_node_id);
  }

  {
    //然后，更新轨迹之间的连接关系，并调用修饰器完成地图的修饰
    absl::MutexLock locker(&mutex_);
    for (const Constraint& constraint : result) {
      UpdateTrajectoryConnectivity(constraint);
    }
    DeleteTrajectoriesIfNeeded();
    TrimmingHandle trimming_handle(this);
    for (auto& trimmer : trimmers_) {
      trimmer->Trim(&trimming_handle);
    }
    trimmers_.erase(
        std::remove_if(trimmers_.begin(), trimmers_.end(),
                       [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                         return trimmer->IsFinished();
                       }),
        trimmers_.end());
    //已经完成了一次地图的后端优化,将计数器num_nodes_since_last_loop_closure_清零
    num_nodes_since_last_loop_closure_ = 0;

    // Update the gauges that count the current number of constraints.
    double inter_constraints_same_trajectory = 0;
    double inter_constraints_different_trajectory = 0;
    for (const auto& constraint : data_.constraints) {
      if (constraint.tag ==
          cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
        continue;
      }
      if (constraint.node_id.trajectory_id ==
          constraint.submap_id.trajectory_id) {
        ++inter_constraints_same_trajectory;
      } else {
        ++inter_constraints_different_trajectory;
      }
    }
    kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
    kConstraintsDifferentTrajectoryMetric->Set(
        inter_constraints_different_trajectory);
  }

  DrainWorkQueue();
}

        DrainWorkQueue函数会一直取双端队列workqueue 的第一个函数，然后执行，一直执行到函数返回内容是kRunOptimization为止，同时也调用HandleWorkQueue函数，来调度队列里的任务

//添加因子图节点
NodeId PoseGraph2D::AddNode(
    //更新子图时的点云信息以及相对位姿
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    //更新子图时的轨迹id    
    const int trajectory_id,
    //更新子图时插入的子图
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  //将local_pose转化为世界坐标系下的位姿，通过trajectory中提取LocalToGlobal转化
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);
  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  //通过拉姆达表达式注册一个为新增节点添加约束的任务ComputeConstraintsForNode
  const bool newly_finished_submap =
      insertion_submaps.front()->insertion_finished();
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    //添加约束
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });
  return node_id;
}

NodeId PoseGraph2D::AppendNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
    const transform::Rigid3d& optimized_pose) {
  absl::MutexLock locker(&mutex_);
  //维护各个轨迹之间的连接关系
  AddTrajectoryIfNeeded(trajectory_id);
  if (!CanAddWorkItemModifying(trajectory_id)) {
    LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
  }

  //新建一个node，输入点云信息、位姿及轨迹id
  //加入到trajectory_nodes中
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});
  ++data_.num_trajectory_nodes;
  // Test if the 'insertion_submap.back()' is one we never saw before.
  //如果submap_data中有名为trajectory_id的轨迹，或submap_data的最后一条轨迹是否是新生成的
  if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
              ->data.submap != insertion_submaps.back()) {
    // We grow 'data_.submap_data' as needed. This code assumes that the first
    // time we see a new submap is as 'insertion_submaps.back()'.
    //如果是的话，将trajectory_id赋给submap_data
    const SubmapId submap_id =
        data_.submap_data.Append(trajectory_id, InternalSubmapData());
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
  }
  return node_id;
}

        添加节点主要有两步，在获取传感器信息、轨迹信息、子图信息后，送入ComputeConstraintsForNode函数，添加约束。

//为节点添加约束
WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {
  std::vector<SubmapId> submap_ids;
  std::vector<SubmapId> finished_submap_ids;
  std::set<NodeId> newly_finished_submap_node_ids;
  {
    absl::MutexLock locker(&mutex_);
    //先取节点索引和传感器数据
    const auto& constant_data =
        data_.trajectory_nodes.at(node_id).constant_data;
    //获取新旧子图的索引
    submap_ids = InitializeGlobalSubmapPoses(
        node_id.trajectory_id, constant_data->time, insertion_submaps);
    CHECK_EQ(submap_ids.size(), insertion_submaps.size());
    //以旧图为参考，计算节点相对于子图的局部位姿
    const SubmapId matching_id = submap_ids.front();
    const transform::Rigid2d local_pose_2d =
        transform::Project2D(constant_data->local_pose *
                             transform::Rigid3d::Rotation(
                                 constant_data->gravity_alignment.inverse()));
    //计算节点在世界坐标系下的位姿                           
    const transform::Rigid2d global_pose_2d =
        optimization_problem_->submap_data().at(matching_id).global_pose *
        constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
        local_pose_2d;
    //待优化问题装入上述几个量
    optimization_problem_->AddTrajectoryNode(
        matching_id.trajectory_id,
        optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                                 global_pose_2d,
                                 constant_data->gravity_alignment});
    //新增的节点和新旧子图之间添加INTRA_SUBMAP类型的约束                             
    for (size_t i = 0; i < insertion_submaps.size(); ++i) {
      const SubmapId submap_id = submap_ids[i];
      // Even if this was the last node added to 'submap_id', the submap will
      // only be marked as finished in 'data_.submap_data' further below.
      CHECK(data_.submap_data.at(submap_id).state ==
            SubmapState::kNoConstraintSearch);
      data_.submap_data.at(submap_id).node_ids.emplace(node_id);
      const transform::Rigid2d constraint_transform =
          constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
          local_pose_2d;
      data_.constraints.push_back(
          Constraint{submap_id,
                     node_id,
                     {transform::Embed3D(constraint_transform),
                      options_.matcher_translation_weight(),
                      options_.matcher_rotation_weight()},
                     Constraint::INTRA_SUBMAP});
    }

    // TODO(gaschler): Consider not searching for constraints against
    // trajectories scheduled for deletion.
    // TODO(danielsievers): Add a member variable and avoid having to copy
    // them out here.
    //找到所有完成建立的子图
    for (const auto& submap_id_data : data_.submap_data) {
      if (submap_id_data.data.state == SubmapState::kFinished) {
        CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
        finished_submap_ids.emplace_back(submap_id_data.id);
      }
    }
    //标记最新一张完成的子图
    if (newly_finished_submap) {
      const SubmapId newly_finished_submap_id = submap_ids.front();
      InternalSubmapData& finished_submap_data =
          data_.submap_data.at(newly_finished_submap_id);
      CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch);
      finished_submap_data.state = SubmapState::kFinished;
      newly_finished_submap_node_ids = finished_submap_data.node_ids;
    }
  }
  //遍历所有finished_submap_ids子图，建立它们与新增节点之间可能的约束
  for (const auto& submap_id : finished_submap_ids) {
    ComputeConstraint(node_id, submap_id);
  }
  //最新一张的子图，建立它与旧节点之间可能存在的约束
  if (newly_finished_submap) {
    const SubmapId newly_finished_submap_id = submap_ids.front();
    // We have a new completed submap, so we look into adding constraints for
    // old nodes.
    for (const auto& node_id_data : optimization_problem_->node_data()) {
      const NodeId& node_id = node_id_data.id;
      if (newly_finished_submap_node_ids.count(node_id) == 0) {
        ComputeConstraint(node_id, newly_finished_submap_id);
      }
    }
  }
  //通知约束构建器新增节点的操作结束
  constraint_builder_.NotifyEndOfNode();
  absl::MutexLock locker(&mutex_);
  ++num_nodes_since_last_loop_closure_;
  if (options_.optimize_every_n_nodes() > 0 &&
      num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
    return WorkItem::Result::kRunOptimization;
  }
  return WorkItem::Result::kDoNotRunOptimization;
}

        ComputeConstraintsForNode函数的逻辑也比较简单，获取到局部位姿信息转化后的全局位姿信息后，装入待优化问题optimization_problem_指针，然后建立新节点与旧子图、旧节点与新子图的约束关系（通过ComputeConstraint函数构建）

        至此整个pose_graph节点的添加就已经建立完成，但是约束建立的过程说的比较笼统，将在下面的内容进行分析

三、constraint_builder_的建立与维护

        PoseGraph2D使用对象constraint_builder_在后台进行闭环检测， 建立约束，主体内容在constraint_builder_2d.h里

class ConstraintBuilder2D {
 public:
  using Constraint = PoseGraphInterface::Constraint;
  using Result = std::vector<Constraint>;

  //构造函数
  ConstraintBuilder2D(const proto::ConstraintBuilderOptions& options,
                      common::ThreadPoolInterface* thread_pool);
  //析构函数
  ~ConstraintBuilder2D();
  
  //禁用
  ConstraintBuilder2D(const ConstraintBuilder2D&) = delete;
  ConstraintBuilder2D& operator=(const ConstraintBuilder2D&) = delete;

  // Schedules exploring a new constraint between 'submap' identified by
  // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
  // 'initial_relative_pose' is relative to the 'submap'.
  //
  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
  // all computations are finished.
  void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap,
                          const NodeId& node_id,
                          const TrajectoryNode::Data* const constant_data,
                          const transform::Rigid2d& initial_relative_pose);

  // Schedules exploring a new constraint between 'submap' identified by
  // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
  // This performs full-submap matching.
  //
  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
  // all computations are finished.
  void MaybeAddGlobalConstraint(
      const SubmapId& submap_id, const Submap2D* submap, const NodeId& node_id,
      const TrajectoryNode::Data* const constant_data);

  // Must be called after all computations related to one node have been added.
  void NotifyEndOfNode();

  // Registers the 'callback' to be called with the results, after all
  // computations triggered by 'MaybeAdd*Constraint' have finished.
  // 'callback' is executed in the 'ThreadPool'.
  void WhenDone(const std::function<void(const Result&)>& callback);

  // Returns the number of consecutive finished nodes.
  int GetNumFinishedNodes();

  // Delete data related to 'submap_id'.
  void DeleteScanMatcher(const SubmapId& submap_id);

  static void RegisterMetrics(metrics::FamilyFactory* family_factory);
 private:
  //构建约束器的配置
  const constraints::proto::ConstraintBuilderOptions options_;
  //线程池
  common::ThreadPoolInterface* thread_pool_;
  //线程锁，保护公共资源的互斥量
  absl::Mutex mutex_;

  // 'callback' set by WhenDone().
  //公共接口WhenDone回调函数对象
  std::unique_ptr<std::function<void(const Result&)>> when_done_
      GUARDED_BY(mutex_);

  // TODO(gaschler): Use atomics instead of mutex to access these counters.
  // Number of the node in reaction to which computations are currently
  // added. This is always the number of nodes seen so far, even when older
  // nodes are matched against a new submap.
  //当前需要考虑的轨迹节点数量
  int num_started_nodes_ GUARDED_BY(mutex_) = 0;
  //已经完成计算的轨迹节点数量
  int num_finished_nodes_ GUARDED_BY(mutex_) = 0;
  //已经完成轨迹节点约束计算的task
  std::unique_ptr<common::Task> finish_node_task_ GUARDED_BY(mutex_);
  //WhenDone的任务状态机

  std::unique_ptr<common::Task> when_done_task_ GUARDED_BY(mutex_);

  // Constraints currently being computed in the background. A deque is used to
  // keep pointers valid when adding more entries. Constraint search results
  // with below-threshold scores are also 'nullptr'.
  //用于保存后台计算的约束的双端队列
  std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);

  // Map of dispatched or constructed scan matchers by 'submap_id'.
  //记录各个子图的扫描匹配器
  std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
      GUARDED_BY(mutex_);
  //每个单独子图的采样器
  std::map<SubmapId, common::FixedRatioSampler> per_submap_sampler_;
  //ceres扫描匹配器
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;

  // Histogram of scan matcher scores.
  //扫描匹配得分的直方图
  common::Histogram score_histogram_ GUARDED_BY(mutex_);
};

//构造函数
ConstraintBuilder2D::ConstraintBuilder2D(
    //配置项
    const constraints::proto::ConstraintBuilderOptions& options,
    //线程池
    common::ThreadPoolInterface* const thread_pool)
    : options_(options),
      thread_pool_(thread_pool),
      finish_node_task_(absl::make_unique<common::Task>()),
      when_done_task_(absl::make_unique<common::Task>()),
      ceres_scan_matcher_(options.ceres_scan_matcher_options()) {}

//析构函数
ConstraintBuilder2D::~ConstraintBuilder2D() {
  absl::MutexLock locker(&mutex_);
  CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW);
  CHECK_EQ(when_done_task_->GetState(), common::Task::NEW);
  CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
  CHECK_EQ(num_started_nodes_, num_finished_nodes_);
  CHECK(when_done_ == nullptr);
}

        上面是约束器的.h文件，构造函数与析构函数，构造函数加载了配置和线程池，然后构建了几个默认项，析构函数在保证任务都完成之后释放函数内存，没有特别运行什么东西

        约束器的运行逻辑实在是没有理明白，去pose_graph_2d搜Constraint也没看出来个大概，这里参考约束构建器——constraint_builder_的说法：

         Global SLAM的核心PoseGraph2D通过MaybeAdd-WhenDone调用循环来组织后台的闭环检测。 所谓的MaybeAdd-WhenDone调用循环是指，在任意调用MaybeAddConstraint、MaybeAddGlobalConstraint、NotifyEndOfNode之后，调用一次WhenDone，如此循环往复。

        按照这个逻辑，分别取参看这三个函数

void ConstraintBuilder2D::MaybeAddConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose) {
  //如果初始位姿很远，不再建立约束
  if (initial_relative_pose.translation().norm() >
      options_.max_constraint_distance()) {
    return;
  }
  //如果子图采样器暂停
  if (!per_submap_sampler_
           .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id),
                    std::forward_as_tuple(options_.sampling_ratio()))
           .first->second.Pulse()) {
    return;
  }

  absl::MutexLock locker(&mutex_);
  //如果when_done_存在，说明上一次闭环检测迭代还没有结束
  if (when_done_) {
    LOG(WARNING)
        << "MaybeAddConstraint was called while WhenDone was scheduled.";
  }
  //添加新的约束
  constraints_.emplace_back();
  kQueueLengthMetric->Set(constraints_.size());
  auto* const constraint = &constraints_.back();
  //初始化扫描匹配器
  const auto* scan_matcher =
      DispatchScanMatcherConstruction(submap_id, submap->grid());
  //构建线程池任务的对象constraint_task
  auto constraint_task = absl::make_unique<common::Task>();
  //constraint_task写入具体任务内容
  constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
                      constant_data, initial_relative_pose, *scan_matcher,
                      constraint);
  });
  //由于具体的约束计算过程需要用到刚刚构建的扫描匹配器对象scan_matcher，而该对象的核心也是在线程池中的一个Task完成构建的
  //所以这里通过给Task添加依赖关系来保证，在进行约束计算之前扫描匹配器就已经完成构建和初始化了
  constraint_task->AddDependency(scan_matcher->creation_task_handle);
  auto constraint_task_handle =
      thread_pool_->Schedule(std::move(constraint_task));
  finish_node_task_->AddDependency(constraint_task_handle);
}

//用于为新增的子图创建扫描匹配器
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
                                                     const Grid2D* const grid) {
  CHECK(grid);
  //如果全局匹配器里已经存在，则直接返回对应id的匹配器
  if (submap_scan_matchers_.count(submap_id) != 0) {
    return &submap_scan_matchers_.at(submap_id);
  }
  //扩展容器submap_scan_matchers_，并将输入的子图占用栅格填充到新扩展的扫描匹配器中
  auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
  kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
  //赋值grid
  submap_scan_matcher.grid = grid;
  //从配置项中获取扫描匹配器的配置
  auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();
  auto scan_matcher_task = absl::make_unique<common::Task>();
  scan_matcher_task->SetWorkItem(
      [&submap_scan_matcher, &scan_matcher_options]() {
        submap_scan_matcher.fast_correlative_scan_matcher =
            absl::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
                *submap_scan_matcher.grid, scan_matcher_options);
      });
  //放入一个线程进行闭环匹配
  submap_scan_matcher.creation_task_handle =
      thread_pool_->Schedule(std::move(scan_matcher_task));
  return &submap_scan_matchers_.at(submap_id);
}

void ConstraintBuilder2D::MaybeAddGlobalConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, const TrajectoryNode::Data* const constant_data) {
  absl::MutexLock locker(&mutex_);
  //如果when_done_存在，说明上一次闭环检测迭代还没有结束
  if (when_done_) {
    LOG(WARNING)
        << "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
  }
  //添加新的约束
  constraints_.emplace_back();
  kQueueLengthMetric->Set(constraints_.size());
  auto* const constraint = &constraints_.back();
  //初始化扫描匹配器
  const auto* scan_matcher =
      DispatchScanMatcherConstruction(submap_id, submap->grid());
  //构建线程池任务的对象constraint_task
  auto constraint_task = absl::make_unique<common::Task>();
 constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
                      constant_data, transform::Rigid2d::Identity(),
                      *scan_matcher, constraint);
  });
  constraint_task->AddDependency(scan_matcher->creation_task_handle);
  auto constraint_task_handle =
      thread_pool_->Schedule(std::move(constraint_task));
  finish_node_task_->AddDependency(constraint_task_handle);
}

void ConstraintBuilder2D::NotifyEndOfNode() {
  absl::MutexLock locker(&mutex_);
  //已经完成了全部约束计算的节点
  CHECK(finish_node_task_ != nullptr);
  //一个新的路径节点被添加到后端
  finish_node_task_->SetWorkItem([this] {
    absl::MutexLock locker(&mutex_);
    ++num_finished_nodes_;
  });
  //将任务对象finish_node_task_添加到线程池的调度队列中
  auto finish_node_task_handle =
      thread_pool_->Schedule(std::move(finish_node_task_));
  //重新构建一个finish_node_task_对象    
  finish_node_task_ = absl::make_unique<common::Task>();
  //将该对象添加到WhenDone任务的依赖列表中
  when_done_task_->AddDependency(finish_node_task_handle);
  ++num_started_nodes_;
}


//后台线程中完成子图与路径节点之间可能的约束计算
void ConstraintBuilder2D::ComputeConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, bool match_full_submap,
    const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose,
    const SubmapScanMatcher& submap_scan_matcher,
    std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
  CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
  //转换为绝对位置
  const transform::Rigid2d initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;

  // The 'constraint_transform' (submap i <- node j) is computed from:
  // - a 'filtered_gravity_aligned_point_cloud' in node j,
  // - the initial guess 'initial_pose' for (map <- node j),
  // - the result 'pose_estimate' of Match() (map <- node j).
  // - the ComputeSubmapPose() (map <- submap i)
  float score = 0.;
  //初始化转换矩阵
  transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();

  // Compute 'pose_estimate' in three stages:
  // 1. Fast estimate using the fast correlative scan matcher.
  // 2. Prune if the score is too low.
  // 3. Refine.
  //选择不同的扫描匹配方式
  //全局所有submap进行匹配
  if (match_full_submap) {
    kGlobalConstraintsSearchedMetric->Increment();
    //匹配分数达到未一定值时则直接抛弃，获取优化值pose_estimate
    if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
            constant_data->filtered_gravity_aligned_point_cloud,
            options_.global_localization_min_score(), &score, &pose_estimate)) {
      CHECK_GT(score, options_.global_localization_min_score());
      CHECK_GE(node_id.trajectory_id, 0);
      CHECK_GE(submap_id.trajectory_id, 0);
      kGlobalConstraintsFoundMetric->Increment();
      kGlobalConstraintScoresMetric->Observe(score);
    } else {
      return;
    }
  } else {
    kConstraintsSearchedMetric->Increment();
    if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
            initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
            options_.min_score(), &score, &pose_estimate)) {
      // We've reported a successful local match.
      CHECK_GT(score, options_.min_score());
      kConstraintsFoundMetric->Increment();
      kConstraintScoresMetric->Observe(score);
    } else {
      return;
    }
  }
  {
    absl::MutexLock locker(&mutex_);
    //接着在一个局部的locker的保护下，将新获得的约束得分统计到一个直方图中
    score_histogram_.Add(score);
  }

  // Use the CSM estimate as both the initial and previous pose. This has the
  // effect that, in the absence of better information, we prefer the original
  // CSM estimate.
  //采用ceres 库进行一次优化匹配
  ceres::Solver::Summary unused_summary;
  ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                            constant_data->filtered_gravity_aligned_point_cloud,
                            *submap_scan_matcher.grid, &pose_estimate,
                            &unused_summary);
  //转换回node相对于submap的位置
  const transform::Rigid2d constraint_transform =
      ComputeSubmapPose(*submap).inverse() * pose_estimate;
  //输出约束结果
  constraint->reset(new Constraint{submap_id,
                                   node_id,
                                   {transform::Embed3D(constraint_transform),
                                    options_.loop_closure_translation_weight(),
                                    options_.loop_closure_rotation_weight()},
                                   Constraint::INTER_SUBMAP});

  if (options_.log_matches()) {
    std::ostringstream info;
    info << "Node " << node_id << " with "
         << constant_data->filtered_gravity_aligned_point_cloud.size()
         << " points on submap " << submap_id << std::fixed;
    if (match_full_submap) {
      info << " matches";
    } else {
      const transform::Rigid2d difference =
          initial_pose.inverse() * pose_estimate;
      info << " differs by translation " << std::setprecision(2)
           << difference.translation().norm() << " rotation "
           << std::setprecision(3) << std::abs(difference.normalized_angle());
    }
    info << " with score " << std::setprecision(1) << 100. * score << "%.";
    LOG(INFO) << info.str();
  }
}

        MaybeAddConstraint用于检查子图和路径节点之间是否存在可能的约束，MaybeAddGlobalConstraint与其构建过程类似，不过在ComputeConstraint函数中，MaybeAddGlobalConstraint匹配的书全部的子图，也就是全局匹配，而NotifyEndOfNode是在有节点完成全部匹配后才调用的

        完成了这些操作之后， 它就会调用接口NotifyEndOfNode， 通知对象constraint_builder_完成了一个路径节点的插入工作，下面的代码描述了该接口的实现。

void ConstraintBuilder2D::WhenDone(
    //当所有的闭环检测任务结束之后的回调函数
    const std::function<void(const ConstraintBuilder2D::Result&)>& callback) {
  absl::MutexLock locker(&mutex_);
  CHECK(when_done_ == nullptr);
  // TODO(gaschler): Consider using just std::function, it can also be empty.
  //指针when_done_记录为回调函数
  when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback);
  CHECK(when_done_task_ != nullptr);
  //指定when_done_task_的具体工作内容,加入线程池
  when_done_task_->SetWorkItem([this] { RunWhenDoneCallback(); });
  thread_pool_->Schedule(std::move(when_done_task_));
  when_done_task_ = absl::make_unique<common::Task>();
}

void ConstraintBuilder2D::RunWhenDoneCallback() {
  Result result;
  std::unique_ptr<std::function<void(const Result&)>> callback;
  {
    absl::MutexLock locker(&mutex_);
    CHECK(when_done_ != nullptr);
    //检查约束器是否都已经完成计算
    for (const std::unique_ptr<Constraint>& constraint : constraints_) {
      if (constraint == nullptr) continue;
      //将约束结果结果放入result中
      result.push_back(*constraint);
    }
    if (options_.log_matches()) {
      LOG(INFO) << constraints_.size() << " computations resulted in "
                << result.size() << " additional constraints.";
      LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
    }
    //清空
    constraints_.clear();
    callback = std::move(when_done_);
    when_done_.reset();
    kQueueLengthMetric->Set(constraints_.size());
  }
  (*callback)(result);
}

        该函数只有一个输入参数，记录了当所有的闭环检测任务结束之后的回调函数，具体内容主要是检查所有的约束器是否已经完成计算

        至此整个约束器的构建分析就大致结束了

四、后话

        这部分的逻辑还是比较清晰的，没有前面那么多类的调用，位姿图的建立比较简单，就是构建节点和子图之间的关系，然后送到约束器，构建可能存在的约束，后面的话应该只剩送到全局优化器进行优化了