namespace velodyne_pointcloud
{
  struct PointXYZIR
  {
  PCL_ADD_POINT4D;                    // quad-word XYZ
  float    intensity;                 
  uint16_t ring;                      
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW     // ensure proper alignment
  } EIGEN_ALIGN16;
}; // namespace velodyne_pointcloud

void PoseGraph2D::RunFinalOptimization()
{
  {
  	// 参数 max_num_final_iterations 默认 200
    // 优化标志
    AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
      absl::MutexLock locker(&mutex_);
      optimization_problem_->SetMaxNumIterations(
          options_.max_num_final_iterations());
      return WorkItem::Result::kRunOptimization;
    });
    // 参数 max_num_iterations， 默认 50
    // 不优化标志
    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;
    });
  }
  // 另一个调用地方在 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.)))) {
      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;
  };
  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;
}

void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result&  result)
{
  {   // 将新的约束添加到全局约束队列中
    absl::MutexLock locker(&mutex_);
    data_.constraints.insert(data_.constraints.end(), result.begin(),
                             result.end() );
  }
  // ceres 在这里
  RunOptimization();
   			// 以下是 省略 的内容
  //如果设置了全局优化回调函数，则进行调用
  //根据约束结果，更新轨迹间的链接关系
  // 优化完成后，累计节点清零
  // 计算相同轨迹的 inter_constraints_same_trajectory  
  // 计算不同轨迹的 inter_constraints_different_trajectory

  // 优化结束后，重新开启任务队列，即继续执行
  // work_queue_里的 work_item
  DrainWorkQueue();
}

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

void PoseGraph2D::RunOptimization()
{
  if (optimization_problem_->submap_data().empty() )
    return;
  // std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
  
  // No other thread is accessing the optimization_problem_,
  // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
  // when executing Solve. 
  // 调用优化，Ceres在这里面，由于耗时间，故没加锁，防止阻塞其他线程
  optimization_problem_->Solve(data_.constraints, 
    GetTrajectoryStates(),  data_.landmark_nodes);

  absl::MutexLock locker(&mutex_);
   // 优化后所有的submap和node数据
  const auto& submap_data = optimization_problem_->submap_data();
  const auto& node_data = optimization_problem_->node_data();
  /*  遍历所有优化后的轨迹的所有节点位姿  */
  for (const int trajectory_id : node_data.trajectory_ids())
  {
    for (const auto& node : node_data.trajectory(trajectory_id) )
    {
      auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
      // 更新所有节点的 全局位姿
      mutable_trajectory_node.global_pose =
          transform::Embed3D(node.data.global_pose_2d) *
          transform::Rigid3d::Rotation(
              mutable_trajectory_node.constant_data->gravity_alignment);
    }

    // Extrapolate all point cloud poses that were not included in the
    // 'optimization_problem_' yet. 注意是已经加入tarjectory但是还没有进行优化的新Node
    // 由于使用采样器建立约束，有的node没有建立约束，其pose就不会被优化
    // 因此，要通过已经优化的位姿转换关系来修正所有的node

    // 子图的local到global的新的转移矩阵
    const auto local_to_new_global =
        ComputeLocalToGlobalTransform(submap_data, trajectory_id);
        // 旧的转移矩阵
    const auto local_to_old_global = ComputeLocalToGlobalTransform(
        data_.global_submap_poses_2d, trajectory_id);
    const transform::Rigid3d old_global_to_new_global =
        local_to_new_global * local_to_old_global.inverse();
      // 上次最后一个优化的节点
    const NodeId last_optimized_node_id =
        std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
    auto node_it =
        std::next(data_.trajectory_nodes.find(last_optimized_node_id));
     // 后续未优化的节点的全局pose进行转移
    for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
         ++node_it)
    {
      // TrajectoryNode类型， 指针
      auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
      修正 global_pose
      mutable_trajectory_node.global_pose =
          old_global_to_new_global * mutable_trajectory_node.global_pose;
    }
  }
  // 更新所有路标位姿
  for (const auto& landmark : optimization_problem_->landmark_data()) {
    data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
  }
  data_.global_submap_poses_2d = submap_data;
}

void ConstraintBuilder2D::ComputeConstraint(
    const SubmapId& submap_id,  const Submap2D* const submap,
    const NodeId& node_id,  bool match_full_submap,
    const TrajectoryNode::Data* const  constant_data,
    const transform::Rigid2d&  initial_relative_pose,
    const SubmapScanMatcher&   submap_scan_matcher,
    std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint)
{
  CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
  /*  node在local坐标系的位姿 = 子图在local坐标系中的位姿 * node在子图的位姿*/
  const transform::Rigid2d  initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;

  float score = 0.;
  transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();

// 匹配所有子图，对应 MaybeAddGlobalConstraint
if (match_full_submap)
{
  kGlobalConstraintsSearchedMetric->Increment();
  if( submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
          constant_data->filtered_gravity_aligned_point_cloud,
          // 参数为 global_localization_min_score
          options_.global_localization_min_score(), 
          &score,  &pose_estimate)  )
  {
    CHECK_GT(score, options_.global_localization_min_score());
    CHECK_GE(node_id.trajectory_id, 0);
    CHECK_GE(submap_id.trajectory_id, 0);
    // 最后记录下全局约束的次数和统计置信度
    kGlobalConstraintsFoundMetric->Increment();
    kGlobalConstraintScoresMetric->Observe(score);
  }
  else   return;
}
// 匹配局部子图
else
{
  kConstraintsSearchedMetric->Increment();
  if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
          initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
          // 参数为 min_score
          options_.min_score(), 
          &score, &pose_estimate)  )
  {
    CHECK_GT(score, options_.min_score());
    kConstraintsFoundMetric->Increment();
    kConstraintScoresMetric->Observe(score);
  }
  else    return;
}
{
  absl::MutexLock locker(&mutex_);
  score_histogram_.Add(score);
}

// Use the CSM estimate as both the initial and previous pose. 
// This has the effect that, in the absence of better information, 
// we prefer the original  CSM estimate.
ceres::Solver::Summary  unused_summary;
// ceres更新pose_estimate，获得节点在local map中的最优位姿
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                 constant_data->filtered_gravity_aligned_point_cloud,
                *submap_scan_matcher.grid,  &pose_estimate,
                &unused_summary);
// 计算得到node相对子图的位姿
const transform::Rigid2d constraint_transform = 
    ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset(new Constraint{submap_id,
       node_id,
       { transform::Embed3D(constraint_transform),
        options_.loop_closure_translation_weight(),
        options_.loop_closure_rotation_weight() },
       Constraint::INTER_SUBMAP} );

if (options_.log_matches() )
{
    std::ostringstream info;
    info << "Node " << node_id << " with "
         << constant_data->filtered_gravity_aligned_point_cloud.size()
         << " points on submap " << submap_id << std::fixed;
    if (match_full_submap)
         info << " matches";
    else
    {
      const transform::Rigid2d difference =
          initial_pose.inverse() * pose_estimate;
      info << " differs by translation " << std::setprecision(2)
           << difference.translation().norm() << " rotation "
           << std::setprecision(3) << std::abs(difference.normalized_angle());
    }
    info << " with score " << std::setprecision(1) << 100. * score << "%.";
    LOG(INFO) << info.str();
}