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

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

vector<int> v;
for(int i=0; i<4; i++)
{
    v.emplace_back();
    auto& num = v.back();
    num = i;
    qDebug()<< "size: "<<v.size() <<"  v.back: "<<v.back();
}

#include <iostream>
#include <map>
#include <string>
#include <vector>
using namespace std;

class A{
public:
    A(const string& s) {cout << "A construct " <<endl;}
    A(const A& a) {cout <<"A copy"<<endl; }
private:
    string s;
};

int main()
{
    std::map<int, string> m;
    vector<A> v;
    A a1("test");  // a1 构造函数
    cout << "------------------" << endl;
    v.push_back(a1);   // a1 拷贝构造函数
    v.emplace_back(a1);
   
    //先运行构造函数，后拷贝构造函数
    v.push_back(A("test") );
    v.emplace_back(A("test") );

    // v.push_back("test");  报错
    // 这个才是真正的用途
    v.emplace_back("test");   // 隐式转换，只有拷贝构造函数
    cout << "******************" << endl;
    return 0;
}

// 无法区分哪个参数用来构造 key 哪些用来构造 value
// 有可能是 std::string s("hello", 1), std::complex<double> cpx(2) 
// 也有可能是 std::string s("hello"), std::complex<double> cpx(1, 2)
std::map<std::string, std::complex<double>> scp;
scp.emplace("hello", 1, 2); 

// 想对于 map 避免临时变量的构造的话，就需要构建两个 tuple
std::map<std::string, std::complex<double>> scp;
scp.emplace(std::piecewise_construct, // 此常量值作为构造 pair 对象的第一个参数传递，以选择构造函数形式，通过将两个元组对象的元素转发给它们各自的构造函数来构造其成员。
    std::forward_as_tuple("hello"), // 该函数会构造一个 tuple 并转发给 pair 构造，并存储在 first 字段
    std::forward_as_tuple(1, 2)); //该函数会构造一个 tuple 并转发给 pair 构造，存储在 second 字段

// std::map<int, ::cartographer::mapping::PoseExtrapolator>  extrapolators_;
  // PoseExtrapolator的构造函数有2个参数：common::Duration   pose_queue_duration,
                                  //      double   imu_gravity_time_constant
  extrapolators_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant)  );

    vector<std::unique_ptr<A> > dd;
    dd.emplace_back();
    // 注意这里的 size 也是1
    cout <<"capacity: "<< dd.capacity() <<" size: "<< dd.size() <<endl;
//    cout << dd.at(0)->getValue() <<endl;  这里运行会崩溃
    
    auto*  a1 = &dd.back();  // 这个是 unique_ptr
    a1->reset(new A(5) );  // 不能再用=，unique_ptr不能复制
    cout <<"capacity: "<< dd.capacity() <<" size: "<< dd.size() <<endl;
    cout << dd.at(0)->getValue() <<endl;

// 省略部分不重要的成员
class Task
{
 public:
  friend class ThreadPoolInterface;
  State GetState() LOCKS_EXCLUDED(mutex_);     //返回本Task当前状态
  // 设置Task 执行的任务 （函数）
  void SetWorkItem(const WorkItem& work_item); 
  // 给当前任务添加 依赖任务， 如当前任务为b，AddDependency(a)表示任务a依赖b 
  // 把当前任务b，加入到依赖任务 a 的 dependent_tasks_ 列表
  void AddDependency(std::weak_ptr<Task> dependency) LOCKS_EXCLUDED(mutex_);

 private:
  // 执行当前任务，比如当前任务为a，并更新依赖a的任务dependent_tasks_中所有任务状态
  void Execute() LOCKS_EXCLUDED(mutex_);

  // 当前任务进入线程待执行队列
  void SetThreadPool(ThreadPoolInterface* thread_pool) LOCKS_EXCLUDED(mutex_);

  // 当前任务的依赖任务完成时候，当前任务状态随之改变
  void OnDependenyCompleted();

  
  using  WorkItem = std::function<void()>;
  enum  State { NEW, DISPATCHED, DEPENDENCIES_COMPLETED, RUNNING, COMPLETED };
  // 任务具体执行过程
  WorkItem   work_item_ ;
  // 执行当前任务的线程池
  ThreadPoolInterface*  thread_pool_to_notify_ = nullptr;
  State   state_  GUARDED_BY(mutex_) = NEW; // 初始化状态为 NEW
  // 依赖当前任务的任务列表
  std::set<Task*>  dependent_tasks_  GUARDED_BY(mutex_);
};

// 省略部分不重要的成员
// 不明白为什么要先构造个抽象类，只有这一个派生类
class ThreadPool : public ThreadPoolInterface
{
 public:
 	//初始化一个线程数量固定的线程池。
  explicit ThreadPool(int num_threads);
  // 添加想要ThreadPool执行的task，插入 tasks_not_ready_
  // 如果任务满足执行要求，直接插入task_queue_准备执行
  std::weak_ptr<Task>  Schedule(std::unique_ptr<Task> task);
 private:
 	//每个线程初始化时, 执行DoWork()函数. 与线程绑定
  void DoWork();
  void NotifyDependenciesCompleted(Task* task);
  //running_只是一个监视哨, 只有线程池在running_状态时, 才能往work_queue_加入函数
  bool running_  = true;
  // pool_就是一系列线程
  std::vector<std::thread>  pool_ ;
  // 十分重要的任务队列
  std::deque<std::shared_ptr<Task>>  task_queue_ ; 
  //未准备好的 task，task可能有依赖还未完成
  absl::flat_hash_map<Task*, std::shared_ptr<Task>>  tasks_not_ready_;  
};

ThreadPool::ThreadPool(int num_threads)
{
  absl::MutexLock  locker(&mutex_);
  // 原来还是用了std::thread，还以为google连线程也是自己实现的
  for (int i = 0; i != num_threads; ++i)
  {
    // std::vector<std::thread>   pool_;
    pool_.emplace_back([this]() { ThreadPool::DoWork(); });
  }
}