ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
tf_ = tf;
costmap_ros_ = costmap_ros;
// 在局部代价地图中的位姿
costmap_ros_->getRobotPose(current_pose_);

// 局部代价地图
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
// base_local_planner::LocalPlannerUtil
// 其实就是把三个参数赋值给 LocalPlannerUtil 的三个成员变量
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());

//这里创建了dwa的共享指针，也就是  boost::shared_ptr<DWAPlanner> dp_;
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));

if( private_nh.getParam( "odom_topic", odom_topic_ ))
{
  odom_helper_.setOdomTopic( odom_topic_ );
}

initialized_ = true;

// Warn about deprecated parameters -- remove this block in N-turtle
// 这些不必关注
nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel");
nav_core::warnRenamedParameter(private_nh, "max_vel_theta", "max_rot_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_theta", "min_rot_vel");
nav_core::warnRenamedParameter(private_nh, "acc_lim_trans", "acc_limit_trans");
nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel");
// 动态调整
dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);

// 开始规划时再次获取位姿
if ( !costmap_ros_->getRobotPose(current_pose_))
{
  ROS_ERROR("Could not get robot pose");
  return false;
}

// TEB的部分全局路径也叫 transformed_plan，应该是因为下面的getLocalPlan参数也是这个名字
std::vector<geometry_msgs::PoseStamped> transformed_plan;
// 获取全局路径
if ( !planner_util_.getLocalPlan(current_pose_, transformed_plan) )
{
  ROS_ERROR("Could not get local plan");
  return false;
}

if(transformed_plan.empty())
{
  ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
  return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());

base_local_planner::transformGlobalPlan(
      *tf_,
      global_plan_,
      global_pose,
      *costmap_,
      global_frame_,
      transformed_plan)

if(limits_.prune_plan)   
{
    base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
}

// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
dp_->updatePlanAndLocalCosts(current_pose_,  transformed_plan, 
     costmap_ros_->getRobotFootprint()  );

void DWAPlanner::updatePlanAndLocalCosts(
      const geometry_msgs::PoseStamped&  global_pose,
      const std::vector<geometry_msgs::PoseStamped>&  new_plan,
      const std::vector<geometry_msgs::Point>& footprint_spec)
{
    global_plan_.resize(new_plan.size());
    for (unsigned int i = 0; i < new_plan.size(); ++i)
    {
      global_plan_[i] = new_plan[i];
    }

    obstacle_costs_.setFootprint(footprint_spec);

    // costs for going away from path
    path_costs_.setTargetPoses(global_plan_);

    // costs for not going towards the local goal as much as possible
    goal_costs_.setTargetPoses(global_plan_);

    // alignment costs
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();

    Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
    double sq_dist =
        (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) 
                                             +
        (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);

    // we want the robot nose to be drawn to its final position
    // (before robot turns towards goal orientation), not the end of the
    // path for the robot center. Choosing the final position after
    // turning towards goal orientation causes instability when the
    // robot needs to make a 180 degree turn at the end
    std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
    double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);

    front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
      forward_point_distance_ * cos(angle_to_goal);
    front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
      sin(angle_to_goal);

    goal_front_costs_.setTargetPoses(front_global_plan);
    
   // keeping the nose on the path
  if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_)
  {
    alignment_costs_.setScale(path_distance_bias_);
    // costs for robot being aligned with path (nose on path, not ju
    alignment_costs_.setTargetPoses(global_plan_);
  }
  else
  {
    // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
    alignment_costs_.setScale(0.0);
  }
}

Vector3f v1 = Vector3f::UnitX();

AngleAxisf aa(45*3.1415/180, v1);
// 返回弧度制表示的旋转角度 和 旋转轴(列向量)
cout << aa.angle() <<"  "<<endl<< aa.axis() <<endl; 
cout << aa.toRotationMatrix()<<endl;   // 转换为3 × 3的旋转矩阵

// fromRotationMatrix():从一个3×3的旋转矩阵构建AngleAxis对象。也就是提取旋转矩阵所表示的旋转角
//  inverse(): 返回以AngleAxis表示的当前旋转的逆旋转

Eigen::Quaterniond q1(x, y, z, w);
 
Eigen::Quaterniond q2(Vector4d(x, y, z, w));
 
Eigen::Quaterniond q2(Matrix3d(R));

// 参数顺序和 static_transform_publisher 的一致
Eigen::Vector3f angle(roll, pitch, yaw);
Eigen::Matrix3d rotation;
rotation = Eigen::AngleAxisd(angle[0], Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(angle[1], Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(angle[2], Eigen::Vector3d::UnitX()) ;

Eigen::Quaterniond quaternion(transform.transform.rotation.w,
                              transform.transform.rotation.x,
                              transform.transform.rotation.y,
                              transform.transform.rotation.z
                              );
Eigen::Matrix3d rotation = quaternion.matrix();

Translation<float,2>(tx, ty)
Translation<float,3>(tx, ty, tz)
Translation<float,N>(s)
Translation<float,N>(vecN)

Translation2d t1(1,4);
Translation2d t2(2,7);
Translation2d t3;
t3=t1*t2;
cout<<t3.translation()<<endl;

#include <Eigen/Eigenvalues>

A << 2,-2,0, -2,1,-2, 0,-2,0;
EigenSolver<Matrix3d> eigensolver(A);
if (eigensolver.info() == Success)
{
    cout << eigensolver.eigenvalues() << endl<<endl;
    cout << eigensolver.eigenvectors() << endl;
}

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