(02)Cartographer源码无死角解析-(82) 轨迹结束处理Node::FinishTrajectoryUnderLock()、最后一次优化RunFinalOptimization()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过前面几篇博客，对于 ROS 订阅服务，发布消息，最后通过 Rviz 进行可视化的过程应该算是比较熟悉了，特别是关于各个坐标系之间的关系，有了比较深的理解。下面来讲解一些收尾工作，比如建图完成之后，需要做什么处理，轨迹与地图是如何保存的，保存好之后应该如何加载，以及重定位模式等等。

该篇博客先首先来看看建图完成之后做了那些处理，先回到 src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 这个文件，可以看到如下代码：

	......
	// 使用默认topic 开始轨迹
	if (FLAGS_start_trajectory_with_default_topics) {
    
    
  		node.StartTrajectoryWithDefaultTopics(trajectory_options);
	}
	
	::ros::spin();
	
	// 结束所有处于活动状态的轨迹
	node.FinishAllTrajectories();
	
	// 当所有的轨迹结束时, 再执行一次全局优化
	node.RunFinalOptimization();
	
	// 如果save_state_filename非空, 就保存pbstream文件
	if (!FLAGS_save_state_filename.empty()) {
    
    
  		node.SerializeState(FLAGS_save_state_filename,
	                        true /* include_unfinished_submaps */);
	}

再轨迹开始之后会阻塞在 ::ros::spin(); 这个位姿，期间会等待接收数据，收到数据执行回调函数利用数据进行建图与定位。只有 ctrl + c 结束进程或者通过 ROS 内置的其他方式，比如建图完成，才会跳过阻塞执行 ::ros::spin() 之后的函数，如 node.FinishAllTrajectories();、node.RunFinalOptimization() 等。

首先的疑问是，除 ctrl + c 结束进程以外，还有什么方式让 ::ros::spin() 阻塞停止，接着往下运行。据本人观察，即使数据集播放完毕，其也依旧阻塞，不过其存在一个服务，通过指令的方式，告诉 cartographer 目前已经完成建图了，可以停止了，该服务依旧位于 node.cc 这个文件，构造函数的如下代码：

      kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
  service_servers_.push_back(node_handle_.advertiseService(

当通过服务 “finish_trajectory” 接收到客户端指令时，会回调 Node::HandleFinishTrajectory() 这个函数，该函数会根据传入的参数结束轨迹：

/**
 * @brief 结束一条轨迹
 *
 * @param[in] request 轨迹的id
 * @param[out]] response 返回StatusResponse格式的处理状态
 * @return true
 */
bool Node::HandleFinishTrajectory(
    ::cartographer_ros_msgs::FinishTrajectory::Request& request,
    ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
    
    
  absl::MutexLock lock(&mutex_);
  response.status = FinishTrajectoryUnderLock(request.trajectory_id);
  return true;
}

可以看到，服务传递的参数就是一个 trajectory_id，还是十分简单的，上锁之后会进一步调用 Node::FinishTrajectoryUnderLock() 这个函数。

二、Node::FinishTrajectoryUnderLock()

$\color{blue}(1):$ 其首先在 Node::trajectories_scheduled_for_finish_ 这个成员变量中查询一下 trajectory_id 是否已经准备结束了，如果是则打印一下信息，且返回 OK 的状态码，主要目的是放置客户端发送多次取消同一轨迹的情况。

$\color{blue}(2):$ 接着调用 Node::TrajectoryStateToStatus 函数，首先查询一下轨迹是否存在，如果不存在则返回且告诉知客户，其指定的轨迹不存在。如果存在且为 TrajectoryState::ACTIVE 状态，则返回 StatusCode::OK 否则返回 StatusCode::INVALID_ARGUMENT，且告知客户端无法完成该轨迹，其是如何具体查询状态的，后续我们再进行分析。

$\color{blue}(3):$ 如果为 TrajectoryState::ACTIVE 状态，则判断一下该轨迹是否书否存在订阅者，如有，则关闭，且从 subscribers_ 中擦除该轨迹相关信息。

$\color{blue}(4):$ 调用cartographer中map_builder的FinishTrajectory()进行轨迹的结束，且结束之后把 trajectory_id 放置在 trajectories_scheduled_for_finish_ 之中，避免后续被重复结束。然后返回 StatusCode::OK 标识，标识正常结束该轨迹。

/**
 * @brief 结束指定id的轨迹
 *
 * @param[in] trajectory_id 要结束的轨迹的id
 * @return cartographer_ros_msgs::StatusResponse
 */
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
    const int trajectory_id) {
    
    
  cartographer_ros_msgs::StatusResponse status_response;
  // Step: 1 检查 trajectory_id 是否在 正在结束的轨迹集合中
  if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
    
    
    status_response.message = absl::StrCat("Trajectory ", trajectory_id,
                                           " already pending to finish.");
    status_response.code = cartographer_ros_msgs::StatusCode::OK;
    LOG(INFO) << status_response.message;
    return status_response;
  }

  // First, check if we can actually finish the trajectory.
  // Step: 2 检查这个轨迹是否存在, 如果存在则检查这个轨迹是否是ACTIVE状态
  status_response = TrajectoryStateToStatus(
      trajectory_id, {
    
    TrajectoryState::ACTIVE} /* valid states */);
  // 如果不是OK状态就返回ERROR
  if (status_response.code != cartographer_ros_msgs::StatusCode::OK) {
    
    
    LOG(ERROR) << "Can't finish trajectory: " << status_response.message;
    return status_response;
  }

  // Shutdown the subscribers of this trajectory.
  // A valid case with no subscribers is e.g. if we just visualize states.
  // Step: 3 如果这个轨迹存在subscribers, 则先关闭subscriber
  if (subscribers_.count(trajectory_id)) {
    
    
    for (auto& entry : subscribers_[trajectory_id]) {
    
    
      entry.subscriber.shutdown();
      subscribed_topics_.erase(entry.topic);
      LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
    }
    // 在subscribers_中将这条轨迹的信息删除
    CHECK_EQ(subscribers_.erase(trajectory_id), 1);
  }

  // Step: 4 调用cartographer中的map_builder的FinishTrajectory()进行轨迹的结束
  map_builder_bridge_.FinishTrajectory(trajectory_id);
  // 将这个轨迹id放进正在结束的轨迹集合中
  trajectories_scheduled_for_finish_.emplace(trajectory_id);
  status_response.message =
      absl::StrCat("Finished trajectory ", trajectory_id, ".");
  status_response.code = cartographer_ros_msgs::StatusCode::OK;
  return status_response;
}

根据该函数，可以知道，通过服务的方式，可以控制轨迹的结束，所以可以再终端运行指令 rosservice call /finish_trajectory 0 结束轨迹。

三、 Node::TrajectoryStateToStatus()

现在回过头来看看，这个函数是如何获取轨迹状态的，其核心在于调用了

const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();

再深入之前，先来看看 trajectory_states 的类型，如下：

std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>

其用于标记每条轨迹目前的状态，轨迹的状态共有如下几种：

  enum class TrajectoryState {
    
     ACTIVE, FINISHED, FROZEN, DELETED };

分别表示活跃，完成、冻结以及删除。MapBuilderBridge::GetTrajectoryStates() 这个函数函数也不会复杂，其主要从 map_builder_ 的后端获取轨迹的状态：

	auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();

通过查看 PoseGraph2D::GetTrajectoryStates() 可以知道，Node::map_builder_bridge_::map_builder_::pose_graph_::data_.trajectories_state中存储了所有轨迹的状态，在 pose_graph_2d.cc 文件中搜索可以 data_.trajectories_state 可知其在这些函数中被引用：

PoseGraph2D::AddTrajectoryIfNeeded() //把轨迹添加到连接状态里，同时为轨迹创建采样器
PoseGraph2D::DeleteTrajectoriesIfNeeded() //如果轨迹处于等待删除状态则会对其进行删除
PoseGraph2D::DeleteTrajectory(const int trajectory_id) //根据 trajectory_id 删除轨迹，主要更改 data_.trajectories_state 中的轨迹状态
PoseGraph2D::FinishTrajectory(const int trajectory_id) //轨迹完成时，会把轨迹状态标记为完成
PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) // 根据 data_.trajectories_state 获取轨迹的状态
PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) //判断轨迹是否为冻结状态
PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) //判断是否可以往该轨迹添加数据，只有活跃的轨迹才能添加数据
PoseGraph2D::GetTrajectoryStates() //获取所有轨迹状态

总的来说，上面这这些函数都是通过 PoseGraph2D:: data_.trajectories_state 获取轨迹状态的，另外 PoseGraph2D::DeleteTrajectoriesIfNeeded() 函数在 PoseGraph2D::HandleWorkQueue() 中被调用，也就是前端数据到来时都会进行一次轨迹的删除判断，外部通过 PoseGraph2D::DeleteTrajectory() 函数如下代码：

    data_.trajectories_state.at(trajectory_id).deletion_state =InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION;

把轨迹设置为等待删除，不用多说，被 PoseGraph2D::DeleteTrajectoriesIfNeeded() 检测到后就会被标记为删除。

$\color{red} 注意$ 你没有看错，是标记为删除，仅仅是标记而已，实际上并没有真的删除轨迹以及相关数据。并且貌似 PoseGraph3D::TrimmingHandle::SetTrajectoryState() 这个函数还能把轨迹的状态恢复到正常，这个据具体再做细节分析了。

另外，轨迹的完成最终肯定要调用到 PoseGraph2D::FinishTrajectory() 其是如何传递的呢？代码如下：

//Node::FinishTrajectoryUnderLock() 
Node::HandleFinishTrajectory()
	Node::FinishTrajectoryUnderLock()
	 	MapBuilderBridge::FinishTrajectory()
	 		MapBuilder->FinishTrajectory()
	 			PoseGraph::FinishTrajectory()
	 				PoseGraph2D::FinishTrajectory()

这样轨迹就被标记为完成了。文件 node_main.cc 中的 Run() 函数 ::ros::spin() 后面会调用 FinishAllTrajectories 也就是Node::FinishAllTrajectories() ，其会遍历所有轨迹，判断轨迹是否活跃，如果处于活跃状态则也会调用 Node::FinishTrajectoryUnderLock() ，所以上面调用流程中 Node::FinishTrajectoryUnderLock() 与 Node::HandleFinishTrajectory() 是并列的关系。

四、 PoseGraph2D::RunFinalOptimization()

node_main.cc 中的 Run() 函数 ::ros::spin() 后，接着 node.FinishAllTrajectories(); 又调用了 node.RunFinalOptimization()，从命名可以知道，该函数是执行最后一次优化，最终调用到的函数为:

map_builder_bridge_.map_builder_->pose_graph()->RunFinalOptimization()

也就是 PoseGraph2D::RunFinalOptimization() 这个函数：

// 执行最后一次的优化, 等待所有的计算任务结束
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();
}

其代码还是比较简单的，就是重新设置了后端问题优化中 ceres 最大的迭代次数然后加到工作队列中去，不过需要注意，紧接着又添加了一个任务，其把参数复原了。也就是说，设置所有轨迹为完成状态时，将不会往轨迹中添加数据，所以该函数添加的两个任务就是任务队列中排到最后的任务。最后再调用了 PoseGraph2D::WaitForAllComputations() 函数，等待所有任务计算完成：

// 等待所有的计算任务执行完成
void PoseGraph2D::WaitForAllComputations() {
    
    
  int num_trajectory_nodes;
  {
    
    
    absl::MutexLock locker(&mutex_);
    num_trajectory_nodes = data_.num_trajectory_nodes;
  }

  const int num_finished_nodes_at_start =
      constraint_builder_.GetNumFinishedNodes();

  // 报告节点计算的进度
  auto report_progress = [this, num_trajectory_nodes,
                          num_finished_nodes_at_start]() {
    
    
    // Log progress on nodes only when we are actually processing nodes.
    if (num_trajectory_nodes != num_finished_nodes_at_start) {
    
    
      std::ostringstream progress_info;
      progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
                    << 100. *
                           (constraint_builder_.GetNumFinishedNodes() -
                            num_finished_nodes_at_start) /
                           (num_trajectory_nodes - num_finished_nodes_at_start)
                    << "%...";
      std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
    }
  };

  // First wait for the work queue to drain so that it's safe to schedule
  // a WhenDone() callback.
  {
    
    
    const auto predicate = [this]()
                               EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) {
    
    
                                 return work_queue_ == nullptr;
                               };
    absl::MutexLock locker(&work_queue_mutex_);
    // 等待工作队列为空
    while (!work_queue_mutex_.AwaitWithTimeout(
        absl::Condition(&predicate),
        absl::FromChrono(common::FromSeconds(1.)))) {
    
     // 1秒打印一次进度
      report_progress();
    }
  }

  // Now wait for any pending constraint computations to finish.
  // 现在等待任何挂起的约束计算完成
  absl::MutexLock locker(&mutex_);
  bool notification = false;
  constraint_builder_.WhenDone(
      [this,
       &notification](const constraints::ConstraintBuilder2D::Result& result)
          LOCKS_EXCLUDED(mutex_) {
    
    
            absl::MutexLock locker(&mutex_);
            // 保存新计算的约束
            data_.constraints.insert(data_.constraints.end(), result.begin(),
                                     result.end());
            notification = true;
          });

  const auto predicate = [&notification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
    
    
    return notification;
  };

  // 等待直到notification为true
  while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
                                  absl::FromChrono(common::FromSeconds(1.)))) {
    
    
    report_progress();
  }
  CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes);
  std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
}

$\color{blue}(1):$ 首先要注意到的是，该函数存在两个循环，第一个循环退出条件是队列为空，且会以1秒的间隔打印目前约束约束计算的进度，num_trajectory_nodes - num_finished_nodes_at_start 表示距离上一次优化完成，目前完成节点约束计算的数量，num_trajectory_nodes - num_finished_nodes_at_start 表示距离上一次优化完成，目前添加了多少个节点到队列中，他们的比值就是计算约束的进度，直到队列为空，才停止打印。

$\color{blue}(2):$ 值得注意的是，队列为空，只能表示子图内的约束计算完成了，不能代表子图间约束也完成了，因为子图间约束是在处理 ConstraintBuilder2D 中完成计算的，其被直接分配给线程池，而非 PoseGraph2D 的工作队列。且是一定间隔节点的频率添加子图间约束，该部分在PoseGraph2D::ComputeConstraint()中有体现，如 global_localization_samplers_[node_id.trajectory_id]->Pulse() 就是对全局约束采样频率的控制。

$\color{blue}(3):$ 那么不多说，需要等待所有约束计算以及相应的优化完成。回顾一下前面的代码 PoseGraph2D::DrainWorkQueue()，单计算约束的节点累计到一定数量时，则会执行如下函数：

  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
    
    
        HandleWorkQueue(result);
      });

通过 HandleWorkQueue() 取调用 RunOptimization()，PoseGraph2D::optimization_problem_ 之中，的约束进以及计前端位姿、里程计odom数据、landmark数据、等数据进行与 global 系相关位姿的优化。但是，这里却没有必要，必要了，因为最后一次优化，我们希望使用前面
PoseGraph2D::RunFinalOptimization() 函数添加到任务队列中中的配置(迭代次数更多)进行优化。所以原本调用 HandleWorkQueue()→RunOptimization()优化的方式要取消，所以所以把回调函数修改成如下：

  constraint_builder_.WhenDone(
      [this,
       &notification](const constraints::ConstraintBuilder2D::Result& result)
          LOCKS_EXCLUDED(mutex_) {
    
    
            absl::MutexLock locker(&mutex_);
            // 保存新计算的约束
            data_.constraints.insert(data_.constraints.end(), result.begin(),
                                     result.end());
            notification = true;
          });

其就是把最后一个计算出来的约束，保存到 data_.constraint 之中，且把 notification 设置为 true。

$\color{blue}(4):$ 其设置好新的 constraint_builder_.WhenDone 之后，就进入到第二个循环进行等待了，等待 notification 被设置为 true，才退出，也就是新的 constraint_builder_.WhenDone 被执行的时候。

$\color{blue}(5):$ PoseGraph2D::RunFinalOptimization() 函数再添加到队列的第一个任务返回值为 WorkItem::Result::kRunOptimization，也就是说该函数被 PoseGraph2D::DrainWorkQueue() 处理之后，会调用constraint_builder_.WhenDone，而此时其已经被设置为新的代码形式。会把 notification 被设置为 true。且新形式不会再调用 PoseGraph2D::HandleWorkQueue() 进入循环，线程结束。PoseGraph2D::WaitForAllComputations() 感应到 notification 被设置为 true 之后，循环结束。检查完成约束计算的节点数 constraint_builder_.GetNumFinishedNodes() 是否与总节点 num_trajectory_nodes 相等，然后输出告知优化完成

$\color{blue}注意:$ 调用 PoseGraph2D::RunFinalOptimization() 与 PoseGraph2D::HandleWorkQueue() 的并非同一个线程，PoseGraph2D::RunFinalOptimization() 来自于执行 node_main.cc 的主线程，其添加好两个任务(修改优化参数、复原修改参数) 之后，然后调用 WaitForAllComputations() 函数等待队列为空，之后重设 constraint_builder_.WhenDone() 函数，该函数需要工作队列任务被执行之后，返回 WorkItem::Result::kRunOptimization 才会被执行，RunFinalOptimization() 中有设置，所以其会执行，但是执行的时被重设之后的。

五、总结

通过该篇博客，首先了解到轨迹是如何结束的，知道有两种方式，通过服务或者 ctrl + c 结束进程时，都可完成轨迹的结束，轨迹结束最总都是调用 PoseGraph2D::FinishTrajectory() 函数变换轨迹的状态。如果使用 ctrl + c 结束，其还会执行最后一次优化，即最总调用到 PoseGraph2D::RunFinalOptimization() 函数。除了停止轨迹的服务，源码中还创建了很多服务，在 Node::Node() 这个构造函数中可以看到如下代码：

  // Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
  service_servers_.push_back(node_handle_.advertiseService(
      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kWriteStateServiceName, &Node::HandleWriteState, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kReadMetricsServiceName, &Node::HandleReadMetrics, this));

如子图，轨迹的查询，开始一条新的轨迹，获取轨迹等。另外还可以看到 Node::HandleWriteState 这个服务，服务名称为 write_state，其作用就是把整个 cartographer 系统的状态以 proto 流文件的形式保存下来。下一篇博客，我们就来分析一下。