cartographer全局优化

全局优化

SLAM过程全局优化基本原理，参考：https://zhuanlan.zhihu.com/p/50055546

：第i个Submap相对于世界坐标系的位姿，其中上角标 表示这是Submap的位姿, ;

：第j个Scan相对于世界坐标系的位姿，其中上角标 表示这是Scan的位姿， ;

:第j个Scan插入第i个Submap的相对位姿。

: 第j个Scan插入第i个Submap的质量。可以理解为对ScanMatching效果的一个评分值。

那么，我们可以把 看做是约束(Constraints)，我们的总体目标就是优化 和 这两类量，使总体的误差最小。其中，误差函数的定义过程如下：

首先，考虑其中一对儿约束，对于 、和，定义误差为：

ceres使用说明：https://blog.csdn.net/u011178262/article/details/88774577

简单理解，node-submap-odom等都有相互关系，全局优化optimization_problem，要综合计算这些约束之间关系，最终通过调整node和submap的位置，让这些约束相互妥协，实现综合最小误差。这个原理，就像下面通过最小二乘法，调节直线参数位置，拟合直线，让每个约束互相妥协，达到最小误差。

因此，OptimizationProblem2D类主要有两种函数：

AddResidualBlock（）加入约束信息，solve（）计算优化结果。

AddResidualBlock(cost_funcfution, loss_function, &x)

其中：cost_funcfution,误差函数，loss_function（可以为NULL） x，为cost_function 待优化变量的初始值。比如，在优化submap和node（scan）位置的过程中，x为submap和scan的初始值，cost_function与下面e有关。

因此，关于node和submap，AddResidualBlock相关程序如下。

// Add cost functions for intra- and inter-submap constraints.
  for (const Constraint& constraint : constraints) {
    problem.AddResidualBlock(
        CreateAutoDiffSpaCostFunction(constraint.pose),
        // Loop closure constraints should have a loss function.
        constraint.tag == Constraint::INTER_SUBMAP
            ? new ceres::HuberLoss(options_.huber_scale())
            : nullptr,
        C_submaps.at(constraint.submap_id).data(),
        C_nodes.at(constraint.node_id).data());
  }

cartographer/mapping/internal/optimization/optimization_problem_2d.cc

namespace cartographer {
namespace mapping {
namespace optimization {

struct NodeSpec2D {
  common::Time time;
  transform::Rigid2d local_pose_2d;
  transform::Rigid2d global_pose_2d;
  Eigen::Quaterniond gravity_alignment; //重力校准
};

struct SubmapSpec2D {
  transform::Rigid2d global_pose;
};

class OptimizationProblem2D
    : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
                                          transform::Rigid2d> {
 public:
  explicit OptimizationProblem2D(
      const optimization::proto::OptimizationProblemOptions& options);
  //  add data OptimizationProblem2D needed 把与优化相关的数据加入OptimizationProblem2D类中
  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override;
  void AddOdometryData(int trajectory_id,
                       const sensor::OdometryData& odometry_data) override;
  void AddTrajectoryNode(int trajectory_id,
                         const NodeSpec2D& node_data) override;
  void InsertTrajectoryNode(const NodeId& node_id,
                            const NodeSpec2D& node_data) override;
  void TrimTrajectoryNode(const NodeId& node_id) override;
  void AddSubmap(int trajectory_id,
                 const transform::Rigid2d& global_submap_pose) override;
  void InsertSubmap(const SubmapId& submap_id,
                    const transform::Rigid2d& global_submap_pose) override;
  void TrimSubmap(const SubmapId& submap_id) override;
  void SetMaxNumIterations(int32 max_num_iterations) override;
  // from constrains of the data ,AddResidualBlock()  and solve
  // 根据数据之间的约束关系，加入costfunction函数，并优化                                       
  void Solve(
      const std::vector<Constraint>& constraints,
      const std::map<int, PoseGraphInterface::TrajectoryState>&
          trajectories_state,
      const std::map<std::string, LandmarkNode>& landmark_nodes) override;

  const MapById<NodeId, NodeSpec2D>& node_data() const override {
    return node_data_;
  }
  const MapById<SubmapId, SubmapSpec2D>& submap_data() const override {
    return submap_data_;
  }
  const std::map<std::string, transform::Rigid3d>& landmark_data()
      const override {
    return landmark_data_;
  }
  const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
    return imu_data_;
  }
  const sensor::MapByTime<sensor::OdometryData>& odometry_data()
      const override {
    return odometry_data_;
  }

 private:
  std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
      int trajectory_id, common::Time time) const;
  // Computes the relative pose between two nodes based on odometry data.
  std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
      int trajectory_id, const NodeSpec2D& first_node_data,
      const NodeSpec2D& second_node_data) const;

  optimization::proto::OptimizationProblemOptions options_;
  MapById<NodeId, NodeSpec2D> node_data_;
  MapById<SubmapId, SubmapSpec2D> submap_data_;
  std::map<std::string, transform::Rigid3d> landmark_data_;
  sensor::MapByTime<sensor::ImuData> imu_data_;
  sensor::MapByTime<sensor::OdometryData> odometry_data_;
};

}  // namespace optimization
}  // namespace mapping
}  // namespace cartographer

/cartographer/mapping/internal/2d/pose_graph.cc

主要函数如下：

总结：通过addNode等主要函数，计算node和submap之间constraint并传给optimization_problem_，optimization_problem_根据要求阶段性的进行优化。当然optimization_problem_优化还可能用到imu和odom数据（addImuData，addOdomData），通过相关函数加入到optimization_problem。

其中ComputeConstraintsForNode以及runOptimization等工作通过压入work_queue，通过thread_pool处理。

// AddNode的主要工作是把一个TrajectoryNode的数据加入到PoseGraph维护的trajectory_nodes_这个容器中。并返回加入的节点的Node.
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) {
  // 生成一个新的Pose，这个Pose是由trajectory相对于世界坐标系的Pose乘以节点数据中包含的该节点相对于该trajectory的LocalTrajectoryBuilder Pose
  // 所以生成的optimized_pose就是该节点的绝对位姿
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  common::MutexLocker locker(&mutex_);
  // 必要时新增一条trajectory。怎么判断是否必要，查看AddTrajectoryIfNeeded的代码
  AddTrajectoryIfNeeded(trajectory_id);
  // TrajectoryNode定义在/mapping/id.h.解读见：https://zhuanlan.zhihu.com/p/48790984
  // 把该节点加入到PoseGraph2D维护的trajectory_nodes_这个容器中
  const NodeId node_id = trajectory_nodes_.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});
  // 节点数加1.
  ++num_trajectory_nodes_;

  // 跟上一个情况类似，如果submap_data_还没有元素或最后一个元素不等于insertion_submaps
  // 的第二个元素
  // 说明insertion_submaps的第二个元素还不被PoseGraph所知。
  if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
          insertion_submaps.back()) {
    // We grow 'submap_data_' as needed. This code assumes that the first
    // time we see a new submap is as 'insertion_submaps.back()'.
    // 生成一个SubmapId,把insertion_submaps的第二个元素加到submap_data_中
    const SubmapId submap_id =
        submap_data_.Append(trajectory_id, InternalSubmapData());
    submap_data_.at(submap_id).submap = insertion_submaps.back();
  }

  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  // 检查insertion_submaps的第一个submap是否已经被finished了。
  // 如果被finished，那么我们需要计算一下约束并且进行一下全局优化了。
  // 这里是把这个工作交到了workItem中等待处理
  const bool newly_finished_submap = insertion_submaps.front()->finished();
  AddWorkItem([=]() REQUIRES(mutex_) {
    ComputeConstraintsForNode(node_id, insertion_submaps,
                              newly_finished_submap);
  });
  // 上述都做完了，返回node_id.
  return node_id;
}

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>();
    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});
  kWorkQueueDelayMetric->Set(
      std::chrono::duration_cast<std::chrono::duration<double>>(
          now - work_queue_->front().time)
          .count());
}


// imu 数据加入到 optimization_problem_
void PoseGraph2D::AddImuData(const int trajectory_id,
                             const sensor::ImuData& imu_data) {
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    if (CanAddWorkItemModifying(trajectory_id)) {
      optimization_problem_->AddImuData(trajectory_id, imu_data);
    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
}
// odometry_data 数据加入到 optimization_problem_
void PoseGraph2D::AddOdometryData(const int trajectory_id,
                                  const sensor::OdometryData& odometry_data) {
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    if (CanAddWorkItemModifying(trajectory_id)) {
      optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
}
//AddFixedFramePoseData是处理GPS等能够测量机器人完整位姿的传感器输入，
//目前2D情况下并没有用到该函数。
void PoseGraph2D::AddFixedFramePoseData(
    const int trajectory_id,
    const sensor::FixedFramePoseData& fixed_frame_pose_data) {
  LOG(FATAL) << "Not yet implemented for 2D.";
}

void PoseGraph2D::AddLandmarkData(int trajectory_id,
                                  const sensor::LandmarkData& landmark_data) {
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    if (CanAddWorkItemModifying(trajectory_id)) {
      //根据数据生成一个LandmarkNode::LandmarkObservation，压入landmark_nodes_这个容器中
      for (const auto& observation : landmark_data.landmark_observations) {
        data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
            PoseGraphInterface::LandmarkNode::LandmarkObservation{
                trajectory_id, landmark_data.time,
                observation.landmark_to_tracking_transform,
                observation.translation_weight, observation.rotation_weight});
      }
    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
}
// 计算node和submap之间的约束
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
                                    const SubmapId& submap_id) {
  ...
}
// 三个参数，一个是刚加入的节点ID；一个是Local Slam返回的insertion_submaps；
// 一个是是否有新finished的submap的判断标志
// 四步：
//1.把该节点的信息加入到OptimizationProblem中，方便进行优化
//2.计算节点和新加入的submap之间的约束
//3.计算其它submap与节点之间约束
//4.计算新的submap和旧的节点的约束
void PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {
  // 获取节点数据
  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
  // 根据节点数据的时间获取最新的submap的id
  const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
      node_id.trajectory_id, constant_data->time, insertion_submaps);
  CHECK_EQ(submap_ids.size(), insertion_submaps.size());// 检查两者大小是否相等
  // 获取这两个submap中前一个的id
  const SubmapId matching_id = submap_ids.front();
  // 计算该Node经过重力align后的相对位姿，即在submap中的位姿
  const transform::Rigid2d local_pose_2d = transform::Project2D(
      constant_data->local_pose *
      transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
  // 计算该Node在世界坐标系中的绝对位姿；但中间为啥要乘一个constraints::ComputeSubmapPose(*insertion_submaps.front()).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;//该submap的绝对位姿乘以在相应submap的相对位姿不就可以了吗？中间那一项是啥呢？
  // 把该节点的信息加入到OptimizationProblem中，方便进行优化
  optimization_problem_->AddTrajectoryNode(
      matching_id.trajectory_id,
      optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                               global_pose_2d,
                               constant_data->gravity_alignment});
  //遍历处理每一个insertion_submaps
  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 'submap_data_' further below.
    CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);//检查指定id是否是kActive
    // 加入到PoseGraph维护的容器中
    submap_data_.at(submap_id).node_ids.emplace(node_id);
    // 计算相对位姿
    const transform::Rigid2d constraint_transform =
        constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
        local_pose_2d;
    // 把约束压入约束集合中
    constraints_.push_back(Constraint{submap_id,
                                      node_id,
                                      {transform::Embed3D(constraint_transform),
                                       options_.matcher_translation_weight(),
                                       options_.matcher_rotation_weight()},
                                      Constraint::INTRA_SUBMAP});
  }

  // 遍历历史中的submap，计算新的Node与每个submap的约束
  for (const auto& submap_id_data : submap_data_) {
    if (submap_id_data.data.state == SubmapState::kFinished) {//确认submap已经被finished了
      CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);//检查该submap中还没有跟该节点产生约束
      ComputeConstraint(node_id, submap_id_data.id);//计算该节点与submap的约束
    }
  }

  // 如果有新的刚被finished的submap
  if (newly_finished_submap) {
    //insertion_maps中的第一个是Old的那个，如果有刚被finished的submap，那一定是他
    const SubmapId finished_submap_id = submap_ids.front();
    // 获取该submap的数据
    InternalSubmapData& finished_submap_data =
        submap_data_.at(finished_submap_id);
    // 检查它还是不是kActive。
    CHECK(finished_submap_data.state == SubmapState::kActive);
    // 把它设置成finished
    finished_submap_data.state = SubmapState::kFinished;
    // We have a new completed submap, so we look into adding constraints for
    // old nodes.
    //计算新的submap和旧的节点的约束
    ComputeConstraintsForOldNodes(finished_submap_id);
  }
  // 结束构建约束
  constraint_builder_.NotifyEndOfNode();
  // 计数器加1
  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;
}
// 获取该node和该submap中的node中较新的时间
common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
                                            const SubmapId& submap_id) const {
  // 获取指定id的时间
  common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time;
  // 获取指定id的submap的数据
  const InternalSubmapData& submap_data = data_.submap_data.at(submap_id);
  if (!submap_data.node_ids.empty()) {//如果该submap相关的node_ids不为空。
    // 获取指定id的submap相关的node_ids列表中的最后一个元素
    const NodeId last_submap_node_id =
        *data_.submap_data.at(submap_id).node_ids.rbegin(); //c.rbegin() 返回一个逆序迭代器，它指向容器c的最后一个元素
    // 把时间更新为节点建立时间与submap中最后一个节点时间中较晚的那个
    time = std::max(
        time,
        data_.trajectory_nodes.at(last_submap_node_id).constant_data->time);
  }
  return time;
}

// 在每有一条新的约束constraint加入时，用来更新不同trajectory之间的connectivity的关系。
void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
  //检查constraint的标签类型是否是INTER_SUBMAP类型.
  CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
  //根据constraint中产生约束的一对儿node_id和submap_id获取两者上次建立约束的最晚时间
  const common::Time time =
      GetLatestNodeTime(constraint.node_id, constraint.submap_id);
  //对两者所在trajectory_id建立connectivity
  trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
                                         constraint.submap_id.trajectory_id,
                                         time);
}


// 输入是由ConstraintBuilder2D建立起来的约束向量
void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result) {
  {
    // 在处理数据时加上互斥锁，防止出现数据访问错误
    absl::MutexLock locker(&mutex_);
    // 把result中的所有约束加入到constraints_向量的末尾处
    data_.constraints.insert(data_.constraints.end(), result.begin(),
                             result.end());
    //data_.constraints 是RunOptimization() 依据
  }
  // 执行优化。这里调用了PoseGraph2D的另一个成员函数RunOptimization()来处理
  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();
      // 把optimization_problem_中的node和submap的数据拷贝到两个参数中
      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);
  }

  {
    // 更新trajectory之间的connectivity信息
    absl::MutexLock locker(&mutex_);
    for (const Constraint& constraint : result) {
      UpdateTrajectoryConnectivity(constraint);
    }
    DeleteTrajectoriesIfNeeded();
     // 调用trimmers_中每一个trimmer的Trim函数进行处理。但还是不清楚这个trimming的含义是什么。
    TrimmingHandle trimming_handle(this);
    for (auto& trimmer : trimmers_) {
      trimmer->Trim(&trimming_handle);
    }
    // Trim之后把他们从trimmers_这个向量中清除，trimmers_将重新记录等待新一轮的Loop Closure过程中产生的数据
    trimmers_.erase(
        std::remove_if(trimmers_.begin(), trimmers_.end(),
                       [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                         return trimmer->IsFinished();
                       }),
        trimmers_.end());
    // 重新把“上次Loop Closure之后新加入的节点数”置为0，run_loop_closure置为false
    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();
}

// 处理workqueue中的task
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;
      }
      work_item = work_queue_->front().task;
      work_queue_->pop_front();
      work_queue_size = 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.
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
        HandleWorkQueue(result);
      });
}

//建图后最终优化
void PoseGraph2D::RunFinalOptimization() {
  {
    //该任务仅仅起到参数设置作用
    AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {  
      absl::MutexLock locker(&mutex_);
      optimization_problem_->SetMaxNumIterations(
          options_.max_num_final_iterations());
      return WorkItem::Result::kRunOptimization;
    });
    AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
      absl::MutexLock locker(&mutex_);
      optimization_problem_->SetMaxNumIterations(
          options_.optimization_problem_options()
              .ceres_solver_options()
              .max_num_iterations());
      return WorkItem::Result::kDoNotRunOptimization;
    });
  }
  WaitForAllComputations();
}

void PoseGraph2D::RunOptimization() {
  if (optimization_problem_->submap_data().empty()) {
    return;
  }

  // No other thread is accessing the optimization_problem_,
  // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
  // when executing the Solve. Solve is time consuming, so not taking the mutex
  // before Solve to avoid blocking foreground processing.
  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);
  absl::MutexLock locker(&mutex_);
  ...
}

// 该函数的主要工作就是指定一个trajectory_id的情况下，返回当前正处于活跃状态下的submap的id，
// 也就是系统正在维护的insertion_submaps的两个submap的id。insertion_submaps可能为空，
// 也可能当前只有一个元素，也可能已经有两个元素了。
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());
  // 返回OptimizationProblem2D的成员变量： MapById<SubmapId, SubmapSpec2D> submap_data_;
  // submap_data_中存的就是以SubmapId为key values值管理的所有Submap的全局位姿
  const auto& submap_data = optimization_problem_->submap_data();
  if (insertion_submaps.size() == 1) {
    // If we don't already have an entry for the first submap, add one.
    // SizeOfTrajectoryOrZero: 返回一个指定Id的元素（这里的元素是SubmapSpec2D）的数据的size，如果该id不存在，则返回0
    // 如果判断指定id的submap_data的size为0，说明该trajectory_id上还没有submap数据，那么就需要建立一个submap
    // If we don't already have an entry for the first submap, add one.
    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
      if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
        // TrajectoryConnectivityState 定义在/mapping/internal/trajectory_connectivity_state.h中
        // initial_trajectory_poses_中存的是一系列带有int型id的InitialTrajectoryPose，
        // 一个InitialTrajectoryPose定义了与该trajectory相关联的trajectory_id的值。
        // 因此，下面这句是把该trajectory_id与其应该关联的id(存在InitialTrajectoryPose的to_trajectory_id中)关联起来
        trajectory_connectivity_state_.Connect( //把这个表里数据与指定id关联
            trajectory_id,
            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
            time);
      }
      // 将该submap的global pose加入到optimization_problem_中，方便以后优化
      optimization_problem_->AddSubmap(
          trajectory_id,
          transform::Project2D(ComputeLocalToGlobalTransform(
                                   global_submap_poses_, trajectory_id) *
                               insertion_submaps[0]->local_pose()));
    }
    // 看一下存submap的这个容器的size是否确实等于1
    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
    // 因为是第一个submap，所以该submap的ID是trajectory_id+0，其中0是submap的index.
    const SubmapId submap_id{trajectory_id, 0};
    // 检查这个SubmapId下的submap是否等于insertion_submaps的第一个元素。因为我们初始化第一个submap肯定是要被插入的那个submap
    CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
    // 如果是第一个submap，那么把刚刚建立的submap的id返回
    return {submap_id};
  }
  CHECK_EQ(2, insertion_submaps.size());//检查insertion_submaps的size等于2.
  // 获取submap_data的末尾trajectory_id
  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
  // 检查开头是否不等于末尾。如果等于，说明这个容器里没有一个元素
  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
  // end_it并没有实际的元素，所以它之前的一个submap的id就是submap_data中的最后一个元素
  const SubmapId last_submap_id = std::prev(end_it)->id;
  // 如果是等于insertion_submaps的第一个元素，说明insertion_submaps的第二个元素还没有分配了id
  if (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.
    // 这种情况下，要给新的submap分配id，并把它加到OptimizationProblem的submap_data_这个容器中
    const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
    optimization_problem_->AddSubmap(//解算新的submap的pose插入到OptimizationProblem2D::submap_data_中
        trajectory_id,
        //然后把该pose乘以insertion_submaps中第一个pose的逆再乘第二个pose
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1]));
    // 返回一个包含两个Submap的向量。
    return {last_submap_id,
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
  }
  // 如果是等于insertion_submaps的第二个元素，
  // 说明insertion_submaps的第二个元素也已经分配了id并加入到了OptimizationProblem的submap_data_中
  CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back());
  // 生成一个submap的Id, 这个id是submap_data_的倒数第二个。
  const SubmapId front_submap_id{trajectory_id,
                                 last_submap_id.submap_index - 1};
  // 检查倒数第二个是否等于insertion_submaps的第一个。
  CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front());
  // 把这两个submap的id返回 AddNode
  return {front_submap_id, last_submap_id};
} namespace cartographer