cartographer_learn_18OptimizationProblem2D介绍及轨迹间的连接关系的管理

续接上篇

上一篇我们讨论到了后端如何存储节点和子图，及节点和子图的存储过程中都是存储的那些东西。其中子图的存储时存储了一根指向submap的指针及一个set用以记录那些与子图有关联的节点，节点存储的是Data这个结构体和节点再global中的位姿。同时我们还学习了cartographer线程池，主要有两点。一点是如何保证在任务队列中没有任务时使得线程阻塞，作者认为这是通过条件变量实现的。一点的如何使得各个任务保持逻辑上的先后顺序，而不是仅仅依靠任务添加的时间顺序执行任务，cartographer的实现是每个任务都有自己的依赖任务，只有依赖任务被执行完毕后才会执行自己的这个任务。
让我们从线程池中回来，继续学习AddNode这个函数的剩余部分。

子图加入优化问题（OptimizationProblem2D）

看看AddNode剩余部分的代码：

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;
}

可以看到仍给线程池的任务是一个叫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没有加入优化问题则加入，并返回insertion_submaps中submap的submap的id
    submap_ids = InitializeGlobalSubmapPoses(
        node_id.trajectory_id, constant_data->time, insertion_submaps);
   ......//一堆其他操作
  }

这个函数一开始没有什么特别的，直到InitializeGlobalSubmapPoses这个函数，我们再进去看看：

std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  CHECK(!insertion_submaps.empty());

  // 获取优化问题存储的所有submap，submap_data类型是MapById
  const auto& submap_data = optimization_problem_->submap_data();
  ......//一堆其他的操作
}

这个函数一来就遇到了一个我们陌生的类optimization_problem_，它的类型是OptimizationProblem2D，从名字看想必它就是做优化的类了。我们先来学习一下这个类。

OptimizationProblem2D

这个类在cartographer/cartographer/mapping/internal/optimization/optimization_2d.h中，上代码：

class OptimizationProblem2D
    : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
                                          transform::Rigid2d> {
......//一堆类方法
private:
......//一堆类方法
  optimization::proto::OptimizationProblemOptions options_; // 配置项
  MapById<NodeId, NodeSpec2D> node_data_;                   // 节点的各种姿态，包括local坐标系下的，global坐标系下的
  MapById<SubmapId, SubmapSpec2D> submap_data_;             // submap在global下姿态
  std::map<std::string, transform::Rigid3d> landmark_data_; // landmark数据
  sensor::MapByTime<sensor::ImuData> empty_imu_data_;       //imu数据
  sensor::MapByTime<sensor::OdometryData> odometry_data_;   // 里程计数据
  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_; // gps数据
  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;   // 轨迹数据
}

我们先看看这个类的类成员，发现是一堆MapById，map和MapByTime。虽然这个MapByTime我们之前没有讨论过，但是作者稍微去看了看发现它和MapById差不多，我们到后面有机会想必也能讨论到它，这里我们就先这么认为，它是MapById的变种。这些结构都是用来存储数据的，想必是为优化问题存储数据。那我们先去看看那些数据是如何被加进来的。这里我们稍微列举几个，其他的大致类似。
子图的加入：

void OptimizationProblem2D::AddSubmap(
    const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
  submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose});
}

这里使用的Append这个方法在上一篇的第二节有介绍过，注意这里存储的只有子图的位姿。有趣的是二维的建图优化是似乎不使用imu数据，所有这里加入imu数据的这个函数的空的。
节点的加入：

void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
                                              const NodeSpec2D& node_data) {
  node_data_.Append(trajectory_id, node_data);
  //如果没有储存过这个轨迹的数据，要在轨迹数据那为这个轨迹申请空间
  trajectory_data_[trajectory_id];
}

里程计数据的加入：

void OptimizationProblem2D::AddOdometryData(
    const int trajectory_id, const sensor::OdometryData& odometry_data) {
  odometry_data_.Append(trajectory_id, odometry_data);
}

本次对OptimizationProblem2D的介绍先到这，这个类很重要，想必我们往后还有机会继续学习它。

子图的加入

我们继续回到InitializeGlobalSubmapPoses这个函数中，看看它剩余的部分：

td::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  CHECK(!insertion_submaps.empty());

  // 获取优化问题存储的所有submap，submap_data类型是MapById
  const auto& submap_data = optimization_problem_->submap_data();
  //插入的submap只有一个的情况，即刚刚启动那会
  if (insertion_submaps.size() == 1) {
    //判断这个子图是不是被加过了，如果没有就加入
    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
      // 如果没设置初始位姿就是0, 设置了就是1
      if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
        //这里可以先跳过，我们下一节讨论
        data_.trajectory_connectivity_state.Connect(
            trajectory_id,
            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
            time);
      }
      //加入
      optimization_problem_->AddSubmap(
          trajectory_id, transform::Project2D(
                             ComputeLocalToGlobalTransform(
                                 data_.global_submap_poses_2d, trajectory_id) *
                             insertion_submaps[0]->local_pose()));
    }
    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));

    const SubmapId submap_id{trajectory_id, 0};
    CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
    return {submap_id};
  }
  //插入的子图有两个的情况
  CHECK_EQ(2, insertion_submaps.size());

  // EndOfTrajectory有介绍过，就是获取该轨迹最后一个数据的后一个数据
  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
  // prev也介绍过，结合前一句就是获取该轨迹最后一个数据
  const SubmapId last_submap_id = std::prev(end_it)->id;
  // 如果是等于insertion_submaps的第一个元素, 说明insertion_submaps的第二个元素还没有在加入到optimization_problem_中
  if (data_.submap_data.at(last_submap_id).submap ==
      insertion_submaps.front()) {
    // In this case, 'last_submap_id' is the ID of
    // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
    //取第一个子图在global中的位姿
    const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
    //在优化问题中插入第二个子图
    optimization_problem_->AddSubmap(
        trajectory_id,
        //wT2 = wT1 * 1Tl * lT2
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1]));
    return {last_submap_id,
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
  }
  CHECK(data_.submap_data.at(last_submap_id).submap ==
        insertion_submaps.back());
  const SubmapId front_submap_id{trajectory_id,
                                 last_submap_id.submap_index - 1};
  CHECK(data_.submap_data.at(front_submap_id).submap ==
        insertion_submaps.front());
  return {front_submap_id, last_submap_id};
}

这个函数分成两种情况，一种是slam过程刚刚开始，插入的子图只有一个的时候和插入的子图是两个的时候。大致的流程都写在备注中了。其中有两个子图时，如何求第二个子图在global中的位姿呢？做法是先找到第一个子图在global中的位姿，再通过第一个子图和第二个子图的关系（在local坐标系下做的转换）求得第二个子图的位姿。同时注意看返回的结果。

轨迹间的连接关系的管理

我们回头看看上面跳过的那个东西——trajectory_connectivity_state，这又是一个新的类，在第16篇时我们也看到过它，但是当时我们不知道这是什么东西。我们现在来看看这是什么。

class TrajectoryConnectivityState {
 public:
 ......//一堆类方法
 private:
//mutable时c++的保留字符
  mutable ConnectedComponents connected_components_;
//记录轨迹间的连接关系即这些连接关系建立的时间
  std::map<std::pair<int, int>, common::Time> last_connection_time_map_;
};

先看这个类的类成员，一来就是一个没见过的类型。这里介绍一下mutable，它的作用时标记这个标量，这个被标记的变量在const的类函数中是可以被改变的。第二个是一个map，记录轨迹间的联系。我们在去看看这个不太熟悉的类ConnectedComponents

class ConnectedComponents {
 public:
......//一堆类方法

 private:
......//一堆类方法

  absl::Mutex lock_;
  // Tracks transitive connectivity using a disjoint set forest, i.e. each
  // entry points towards the representative for the given trajectory.
  std::map<int, int> forest_ GUARDED_BY(lock_);
  // Tracks the number of direct connections between a pair of trajectories.
  std::map<std::pair<int, int>, int> connection_map_ GUARDED_BY(lock_);
};

先看这个类的的两个类成员，两个map，第一个是记录哪些轨迹间有联系，同时它也记录了每条轨迹自己与自己间的联系。比如有1，2，3条轨迹，其中1和2有联系，1和3有联系。则forest_中状态如下图：

第二个map记录的是这两个轨迹被记录连接的次数。由于学习还不够深入，作者目前还不能完全把握这个类的作用，只值直到这个类的部分作用。所以作者在这里介绍这个类的作用时可能还不够全。如同我们上面举得栗子，有1，2，3这3条轨迹，1和2有联系，1和3有联系。那请问2和3有没有联系呢？是有的，所以仅仅依靠TrajectoryConnectivityState这个类我们无法得知各个轨迹间准确的联系关系，ConnectedComponents这个类就是解决这一问题的。我们下面将把这种联系称之为间接联系。由于对这两个类的把握还不够全面，我们对它们类函数的介绍是遇到一个介绍一个，逐步加深理解。
回到InitializeGlobalSubmapPoses看看我们跳过的代码：

td::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {

.......//一堆其他操作
if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
        //为两条轨迹间添加联系
        data_.trajectory_connectivity_state.Connect(
            trajectory_id,
            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
            time);
      }
 ......//一堆其他操作
 }

去看看Connect这个函数：

void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
                                          const int trajectory_id_b,
                                          const common::Time time) {
  //通过ConnectedComponents这个类去查看这两条轨迹之前是否有联系，有的话直接添加接好了
  if (TransitivelyConnected(trajectory_id_a, trajectory_id_b)) {
    // The trajectories are transitively connected, i.e. they belong to the same
    // connected component. In this case we only update the last connection time
    // of those two trajectories.
    auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
    if (last_connection_time_map_[sorted_pair] < time) {
      last_connection_time_map_[sorted_pair] = time;
    }
  } else {
    // The connection between these two trajectories is about to join to
    // connected components. Here we update all bipartite trajectory pairs for
    // the two connected components with the connection time. This is to quickly
    // change to a more efficient loop closure search (by constraining the
    // search window) when connected components are joined.
    //如果之间没有联系，除了要添加当前的联系外，还要添加所有间接的联系
    std::vector<int> component_a =
        connected_components_.GetComponent(trajectory_id_a);
    //找到所有与trajectory_id_b有联系的其他轨迹
    std::vector<int> component_b =
        connected_components_.GetComponent(trajectory_id_b);
    for (const auto id_a : component_a) {
      for (const auto id_b : component_b) {
        auto id_pair = std::minmax(id_a, id_b);
        last_connection_time_map_[id_pair] = time;
      }
    }
  }
  connected_components_.Connect(trajectory_id_a, trajectory_id_b);
}

我们先去看看如何判断两条轨迹间之前有无联系的

bool TrajectoryConnectivityState::TransitivelyConnected(
    const int trajectory_id_a, const int trajectory_id_b) const {
  return connected_components_.TransitivelyConnected(trajectory_id_a,
                                                     trajectory_id_b);
}
//调用函数1
bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
                                                const int trajectory_id_b) {
  if (trajectory_id_a == trajectory_id_b) {
    return true;
  }

  absl::MutexLock locker(&lock_);

  if (forest_.count(trajectory_id_a) == 0 ||
      forest_.count(trajectory_id_b) == 0) {
    return false;
  }
  return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
}
//调用函数2
int ConnectedComponents::FindSet(const int trajectory_id) {
  auto it = forest_.find(trajectory_id);
  CHECK(it != forest_.end());
  if (it->first != it->second) {
    // Path compression for efficiency.
    it->second = FindSet(it->second);
  }
  return it->second;
}

调用函数2实现的过程有点绕，我们使用之前那个栗子说明调用函数2实现的过程。比如说调用函数1的两个参数是2，3。则这个函数会调用FindSet两次，即FindSet（2）和FindSet（3）。这里我们说明FindSet（2），另外一个类似。这里我们追踪迭代器it的变化，如下图：

所以它的返回是1，FindSet（3）同理。我们理解了FindSet这个函数的实现过程后，我们再去看看如何找到所有与某条轨迹有联系的其他轨迹的呢？即GetComponent这个函数，作者这里偷个懒，把代码贴出来，读者自己理解一下。。。

std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) {
  absl::MutexLock locker(&lock_);
  const int set_id = FindSet(trajectory_id);
  std::vector<int> trajectory_ids;
  for (const auto& entry : forest_) {
    if (FindSet(entry.first) == set_id) {
      trajectory_ids.push_back(entry.first);
    }
  }
  return trajectory_ids;
}