1-首先是结束建图路径
void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
/*
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFinished(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
//L