cartographer_learn17节点，子图的添加及线程池

续接上篇

上一篇我们讨论到了如何将一个求一个节点在global坐标系中的位姿。总结一下大致分成两种情况。一种是这个节点所属轨迹存在子图s了，我们通过wTs * sTl来求得该轨迹得local坐标系在global坐标系中得位姿，再通过wTl *lTn来求得节点再global坐标系中得位姿。另一种是没有找到这个节点所属得轨迹存储得子图，cartographer得做法是先看看这个轨迹和其他轨迹有没有相对位姿关系，如果没有只能返回单位矩阵（这里是指wTl），如果有就通过别的local坐标系与这个local坐标系得位姿求得这个local坐标系得位姿。
接下来我们往下看看cartographer得后续处理过程。

节点和子图得添加

先上代码：

NodeId PoseGraph2D::AddNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  
  // 将节点在local坐标系下的坐标转成global坐标系下的坐标
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  // 向节点列表加入节点,并得到节点的id
  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);
......//其他操作
}

//调用函数1
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.";
  }

  // 向节点列表中添加一个新的节点
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});
  ++data_.num_trajectory_nodes;
......//其他操作
}

//调用函数2
void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
  // 如果不存在就添加，map的特性
  data_.trajectories_state[trajectory_id];

  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::FINISHED);
  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::DELETED);
  CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
        InternalTrajectoryState::DeletionState::NORMAL);

  // 为轨迹添加连接状态
  data_.trajectory_connectivity_state.Add(trajectory_id);

  // Make sure we have a sampler for this trajectory.
  // 添加采样器
  if (!global_localization_samplers_[trajectory_id]) {
    global_localization_samplers_[trajectory_id] =
        absl::make_unique<common::FixedRatioSampler>(
            options_.global_sampling_ratio());
  }
}

// 轨迹的状态
struct InternalTrajectoryState {
  enum class DeletionState {
    NORMAL,
    SCHEDULED_FOR_DELETION,
    WAIT_FOR_DELETION
  };

  PoseGraphInterface::TrajectoryState state =
      PoseGraphInterface::TrajectoryState::ACTIVE;
  DeletionState deletion_state = DeletionState::NORMAL;
};

先从调用函数1说起，首先一步是先看看这个轨迹是不是以前被记录过，如果没有要先设置一下这个轨迹一些初始状态（即调用函数2），这些轨迹的状态的存储的地方是上一篇最后一节我们提到的std::map<int, InternalTrajectoryState> PoseGraphData::trajectories_state。从上述代码最后一个结构可以看出轨迹的状态有两个变量表示state和deletion_state，它们的初始值分别是ACTIVE和NORMAL，至于其他的状态怎么表示即是什么意思，相信后面我们会讨论到。在设置完轨迹的初始状态后，cartographer还为每个轨迹生成一个采样器（调用函数2的后半部分），至于这个采样器有啥用，作者目前还没有发现。

节点的生成和存储

上面稍微提了一下轨迹状态及采样器的事情，接下来我们回到调用函数1中“向节点列表中添加一个新的节点”这一行。可以看到这里调用的是PoseGraphData::trajectory_nodes中的类方法实现的，trajectory_nodes也是我们上一篇最后一节中提到的后端用来存储节点的地方。我们去看看Append这个函数是如何实现的：

  IdType Append(const int trajectory_id, const DataType& data) {
    CHECK_GE(trajectory_id, 0);
    //先找到存储该轨迹数据的map
    auto& trajectory = trajectories_[trajectory_id];
    CHECK(trajectory.can_append_);
    //注意这里是rbegin，是最后一个元素的迭代器，这里是找到最后一个元素的索引
    const int index =
        trajectory.data_.empty() ? 0 : trajectory.data_.rbegin()->first + 1;
    //最后一个元素索引+1就是当前元素的索引。
    trajectory.data_.emplace(index, data);
    return IdType{trajectory_id, index};
  }

这个存储结构是我们前面几篇多次遇见的MapById，这里只不过是一个我们之前没有遇见过的类方法而已。具体的过程就略过了。我们来看看在这里都存储了节点的哪些信息，我们看到的是一个TrajectoryNode的结构体：

struct TrajectoryNode {
  common::Time time() const { return constant_data->time; }
  std::shared_ptr<const Data> constant_data;
  // 节点在global坐标系中的位姿
  transform::Rigid3d global_pose;
};

其中这个Data是我们第15篇提到过的Data。

子图的存储

回到调用函数1，下面是这个函数被上面省略掉的部分：

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) {
......//生成添加节点的一些操作
  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()'.
    //为即将添加的submap先预留空间
    const SubmapId submap_id =
        data_.submap_data.Append(trajectory_id, InternalSubmapData());
    // 将地图赋值 到新生成的 data_.submap_data.at(submap_id) 的 submap 中
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
  }
  return node_id;
}

struct InternalSubmapData {
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;
//与子图相关联的节点，这里的理解是这些个节点在前端时曾经被插入到submap中过
  std::set<NodeId> node_ids;
};

首先一点我们可以看到，submap的存储同样时上篇中最后一节提到的PoseGraphData::submap_data。并不是任何时候都会去存储submap，根据那个if判断我们可以发现当该轨迹中没有子图的存储时要存submao，或者是insertion_submaps中第二个submap没有被储存过的话就要存储它。和上一小节类似，同样的使用Append的方法实现。这里我们看看存储的内容是写什么？首先是一根指向该submap的指针，还有就是一个set，里面记录了和该submap有关联的节点的id。什么是有关联，备注中写了作者自己的理解。

线程池

在看完AddNode计算节点在global中的位姿，如何添加节点和子图及后端如何存储这些节点子图后，我们继续来看看这个函数的后续操作

NodeId PoseGraph2D::AddNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
......//计算节点在global中的位姿
......//创建节点存储节点，存储子图
//看看旧的submap是否完成插入
  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;
}

这里我们遇到了一个陌生的东西——线程池。本节我们来讨论线程池，作者也是初学，有错误请指出。先看看AddWorkItem这个函数：

void PoseGraph2D::AddWorkItem(
    const std::function<WorkItem::Result()>& work_item) {
  absl::MutexLock locker(&work_queue_mutex_);

  if (work_queue_ == nullptr) {
    // work_queue_是指向deque<WorkItem>的指针
    work_queue_ = absl::make_unique<WorkQueue>();
    //貌似是先往线程池中添加一个任务
    auto task = absl::make_unique<common::Task>();
    task->SetWorkItem([this]() { DrainWorkQueue(); });
    //thread_pool_的类型是指向ThreadPool的指针
    thread_pool_->Schedule(std::move(task));
  }
  const auto now = std::chrono::steady_clock::now();
  //将任务放入work_queue_队列中
  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());
}

可以看到这个函数的的参数有点点少见——std::function，这里作者推荐一篇博客：
https://blog.csdn.net/p942005405/article/details/84760715
在了解完function后我们跳过Task这个类，去直接看看线程池这个类，即ThreadPool。上代码：

class ThreadPool : public ThreadPoolInterface {
 public:
  explicit ThreadPool(int num_threads);

std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task)
      LOCKS_EXCLUDED(mutex_) override;
 private:
 void DoWork();
 void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override;
  absl::Mutex mutex_;

  // 结束线程的标志
  bool running_ GUARDED_BY(mutex_) = true;
  // 线程池
  std::vector<std::thread> pool_ GUARDED_BY(mutex_);  
  std::deque<std::shared_ptr<Task>> task_queue_ GUARDED_BY(mutex_);
  absl::flat_hash_map<Task*, std::shared_ptr<Task>> tasks_not_ready_
      GUARDED_BY(mutex_);
};

先看类成员一来就是一个互斥量，接下来是一个标志位，然后一个vector里面装着很多线程thread，再是一个双向数组deque，里面装着很多Task的指针（从这个命名来看，想必这里面装的就是线程池要执行的任务了），最后一个是一个类似哈希表的东西，目前还不知道作用。
再去看看构造函数：

ThreadPool::ThreadPool(int num_threads) {
  CHECK_GT(num_threads, 0) << "ThreadPool requires a positive num_threads!";
  absl::MutexLock locker(&mutex_);
  //依据传入线程的数量，在vector中创建对应数量的线程
  for (int i = 0; i != num_threads; ++i) {
    pool_.emplace_back([this]() { ThreadPool::DoWork(); });
  }
}

可以看到这里使用lambda表达式去创建线程，且线程执行的是这个类的私有方法ThreadPool::DoWork()，我们去看看这个方法：

void ThreadPool::DoWork() {
//这里比较高深，貌似是linux系统的话要做什么检查，不过不太影响代码的阅读
#ifdef __linux__
  // This changes the per-thread nice level of the current thread on Linux. We
  // do this so that the background work done by the thread pool is not taking
  // away CPU resources from more important foreground threads.
  CHECK_NE(nice(10), -1);
#endif
//一个lambda表达式
  const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
    return !task_queue_.empty() || !running_;
  };

  // 一直循环
  for (;;) {
    std::shared_ptr<Task> task;
    {
      absl::MutexLock locker(&mutex_);
      //作者对这里的理解是这个东西类似c++中的条件变量
      mutex_.Await(absl::Condition(&predicate));
      // 如果任务队列不为空, 那就取出第一个task
      if (!task_queue_.empty()) {
        task = std::move(task_queue_.front());
        task_queue_.pop_front();
      } else if (!running_) {
        return;
      }
    }
    CHECK(task);
    CHECK_EQ(task->GetState(), common::Task::DEPENDENCIES_COMPLETED);

    // 执行task
    Execute(task.get());
  }

先看看那个lambda表达式的意思，任务队列为空时返回false，标志位设置为停止时返回的时false。再看循环，那就容易理解了，当任务队列为空时无法上锁，线程被阻塞。当线程队列不为空时取出队列前面的Task。当标志位为false直接返回杀死线程。接下来我们先去看看Execute这个方法，再去看看Task这个类。这里值得注意的是Execute是ThreadPool这个类基类的类方法。

//Execute
void ThreadPoolInterface::Execute(Task* task) { task->Execute(); }

class Task {
 public:
  friend class ThreadPoolInterface;

  using WorkItem = std::function<void()>;
  enum State { NEW, DISPATCHED, DEPENDENCIES_COMPLETED, RUNNING, COMPLETED };

......//一堆类方法

 private:
......//一些类方法
void Execute() LOCKS_EXCLUDED(mutex_);
......//一堆类方法
//注意这是std::function<void()>
  WorkItem work_item_ GUARDED_BY(mutex_);
//应该是指向线程池的指针，基类指向派生类
  ThreadPoolInterface* thread_pool_to_notify_ GUARDED_BY(mutex_) = nullptr;
//当前任务的状态
  State state_ GUARDED_BY(mutex_) = NEW;
//未完成的依赖的数量
  unsigned int uncompleted_dependencies_ GUARDED_BY(mutex_) = 0;
 //本任务的依赖
  std::set<Task*> dependent_tasks_ GUARDED_BY(mutex_);
  absl::Mutex mutex_;
};

这个类没有一开始想象的那么简单，不仅仅只有一个function执行一下就好了，我们看到任务是带有依赖的，这些所谓的依赖也是一种Task，想必是要确保任务之间的相对顺序，必须执行完依赖的任务后才能执行本任务。同时也要注意任务的状态State。
我们再去看看Task的类方法Execute()

void Task::Execute() {
//检查依赖是否都完成，通过任务的转台
  {
    absl::MutexLock locker(&mutex_);
    CHECK_EQ(state_, DEPENDENCIES_COMPLETED);
    state_ = RUNNING;
  }

  // Execute the work item.
  if (work_item_) {
    work_item_();
  }

  absl::MutexLock locker(&mutex_);
  //把任务的状态设置为完成状态
  state_ = COMPLETED;

  // 通知依赖本任务的其他任务, 本任务执行完了
  for (Task* dependent_task : dependent_tasks_) {
    dependent_task->OnDependenyCompleted();
  }
}

这个类的其他函数就暂时不介绍了，不过它使用到了一种指针weak_ptr，这里作者提供一篇文章以供学习：
http://c.biancheng.net/view/7918.html
我们继续返回去看看线程池这个类的类方法——Schedule：

std::weak_ptr<Task> ThreadPool::Schedule(std::unique_ptr<Task> task) {
  std::shared_ptr<Task> shared_task;
  {
    absl::MutexLock locker(&mutex_);
    // map::insert() 会返回pair<map::iterator,bool> 类型, 
    // 第一个值为迭代器, 第二个值为插入操作是否成功
    auto insert_result =
        tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task)));
    CHECK(insert_result.second) << "Schedule called twice";
    shared_task = insert_result.first->second;
  }
  //设置Task类中线程池的指针
  SetThreadPool(shared_task.get());
  return shared_task;
}

可见任务被加入线程池中时并不是直接加入task_queue_中等待执行，可能时因为我们之前所说的每个任务都可能有自己的依赖任务。它们先是被加入tasks_not_ready_中。再来看看另一个类方法NotifyDependenciesCompleted：

void ThreadPool::NotifyDependenciesCompleted(Task* task) {
  absl::MutexLock locker(&mutex_);

  // 找到task的索引
  auto it = tasks_not_ready_.find(task);
  CHECK(it != tasks_not_ready_.end());

  // 加入到任务队列中
  task_queue_.push_back(it->second);
  // 从tasks_not_ready_中删除task
  tasks_not_ready_.erase(it);
}

可以看到NotifyDependenciesCompleted才会把任务从tasks_not_ready_转移到task_queue_中等待线程的执行。想必时等待任务的依赖执行完后才会调用这个函数。
好了本篇就到这里了。