代码修改记录

node_main.cc无修改。 node.cc只有细节不同，可以忽略

map_builder_bridge.cc

MapBuilderBridge类增加变量std::atomic<int> optimizations_performed_;，

MapBuilderBridge::MapBuilderBridge(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer)
    : node_options_(node_options),
      map_builder_(std::move(map_builder)),
      tf_buffer_(tf_buffer),
      // 增加
      optimizations_performed_(0),
      frontier_detector_(static_cast<cartographer::mapping::PoseGraph*>(
          map_builder_->pose_graph())  )
{
	// 增加
  map_builder_->pose_graph()->SetGlobalSlamOptimizationCallback(
      std::bind(&MapBuilderBridge::OnGlobalSlamResult, this));
}
// 这个回调函数其实是把后端的那个同名变量传过来，因为优化次数统计是在后端
void MapBuilderBridge::OnGlobalSlamResult() { optimizations_performed_++; }

~MapBuilderBridge() { frontier_detector_.NotifyEnd();  }

MapBuilderBridge::GetSubmapList()中添加一行： submap_list.optimizations_performed = optimizations_performed_;

void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id,   const ::cartographer::common::Time time,
    const Rigid3d  local_pose,
    ::cartographer::sensor::RangeData  range_data_in_local,
    // 增加 InsertionResult
    const std::unique_ptr<const ::cartographer::mapping::
                          TrajectoryBuilderInterface::InsertionResult>&  insertion_result )
{
  std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
      std::make_shared<LocalTrajectoryData::LocalSlamData>(
          LocalTrajectoryData::LocalSlamData{time, local_pose,
                                             std::move(range_data_in_local)} );
  absl::MutexLock lock(&mutex_);
  local_slam_data_[trajectory_id] = std::move(local_slam_data);
  // 最关键，这里是唯一调用了 frontier_detector_ 函数的地方
  if (insertion_result)
    frontier_detector_.HandleSubmapUpdates( insertion_result->insertion_submap_ids);
  // frontier_detector_.publishUpdatedFrontiers();
}

struct InsertionResult增加了一个成员：std::vector<SubmapId> insertion_submap_ids;

offline_node.cc

去掉了async_spinner.start();

增加3个变量

extern int total_submap_updates;
extern int optimization_events;
extern int skipped_updates;

pose_graph_2d.h

PoseGraph2D类增加变量std::atomic<int> optimizations_performed_;

int optimizations_performed() const override {
	return optimizations_performed_;
}

MapById<SubmapId, InternalSubmapData> submap_data;


struct InternalSubmapData {
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;

// IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
  std::set<NodeId> node_ids;
};


PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
    const SubmapId& submap_id) const
{
  const auto it = data_.submap_data.find(submap_id);
  if (it == data_.submap_data.end()) {
    return {};
  }
  auto submap = it->data.submap;
  if (data_.global_submap_poses_2d.Contains(submap_id)) {
    // We already have an optimized pose.
    return {submap,
            transform::Embed3D(
                data_.global_submap_poses_2d.at(submap_id).global_pose)};
  }
  // We have to extrapolate.
  return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
                                                submap_id.trajectory_id) *
                      submap->local_pose()};
}

struct SubmapData {
  std::shared_ptr<const Submap> submap;
  transform::Rigid3d pose;
};

pose_graph_2d.cc

PoseGraph2D构造函数增加optimizations_performed_(0)

PoseGraph2D::AppendNode的返回类型改为std::pair<NodeId, vector<SubmapId>，原来是NodeId， 最后增加

std::vector<SubmapId> submap_ids;
auto submap_data_iter =
  std::prev(data_.submap_data.EndOfTrajectory(trajectory_id),
            insertion_submaps.size()  );
for (int i = 0; i < static_cast<int>(insertion_submaps.size()); ++i)
{
	CHECK_EQ(submap_data_iter->data.submap, insertion_submaps.at(i));
	submap_ids.push_back(submap_data_iter->id);
	++submap_data_iter;
}
return {node_id, submap_ids};

PoseGraph2D::RunOptimization的最后增加optimizations_performed_++;，用于统计优化次数。