//save compiler switches
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"

//Bad headers with problem goes here
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

//restore compiler switches
#pragma GCC diagnostic pop

cartographer::io::ProtoStreamWriter writer(filename);
map_builder_->SerializeState(&writer);
return writer.Close();

void MapBuilder::SerializeState(bool include_unfinished_submaps,
                                io::ProtoStreamWriterInterface* const writer)
{
  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer,
                    include_unfinished_submaps);
}

void WritePbStream(
    const mapping::PoseGraph& pose_graph,
    const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
        trajectory_builder_options,
    ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps)
{
  writer->WriteProto(CreateHeader());
  writer->WriteProto(
      SerializePoseGraph(pose_graph, include_unfinished_submaps));
  writer->WriteProto(SerializeTrajectoryBuilderOptions(
      trajectory_builder_options,
      GetValidTrajectoryIds(pose_graph.GetTrajectoryStates())));

  SerializeSubmaps(pose_graph.GetAllSubmapData(), include_unfinished_submaps,
                   writer);
  SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer);
  SerializeTrajectoryData(pose_graph.GetTrajectoryData(), writer);
  SerializeImuData(pose_graph.GetImuData(), writer);
  SerializeOdometryData(pose_graph.GetOdometryData(), writer);
  SerializeFixedFramePoseData(pose_graph.GetFixedFramePoseData(), writer);
  SerializeLandmarkNodes(pose_graph.GetLandmarkNodes(), writer);
}

cartographer::io::ProtoStreamReader stream(state_filename);
map_builder_->LoadState(&stream, load_frozen_state);

if (trajectory_options.has_pure_localization_trimmer()) 
{
  pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
      trajectory_id,
      trajectory_options.pure_localization_trimmer().max_submaps_to_keep() )  );
}

std::map<int, int> trajectory_remapping;
for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i)
{
  auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i);
  const auto& options_with_sensor_ids_proto =
      all_builder_options_proto.options_with_sensor_ids(i);
  // 新的轨迹id
  const int new_trajectory_id =
      AddTrajectoryForDeserialization(options_with_sensor_ids_proto);

  CHECK(trajectory_remapping
            .emplace(trajectory_proto.trajectory_id(), new_trajectory_id)
            .second)
      << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
  trajectory_proto.set_trajectory_id(new_trajectory_id);

  if (load_frozen_state) {
    pose_graph_->FreezeTrajectory(new_trajectory_id);
  }
}

io::ProtoStreamDeserializer deserializer(reader);

// Create a copy of the pose_graph_proto, such that we can re-write the
// trajectory ids.
proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
const auto& all_builder_options_proto =
//  子图位姿
MapById<SubmapId, transform::Rigid3d> submap_poses;
for (const proto::Trajectory& trajectory_proto :
     pose_graph_proto.trajectory())
{
  for (const proto::Trajectory::Submap& submap_proto :
       trajectory_proto.submap())
  {
    submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(),
                                 submap_proto.submap_index()},
                        transform::ToRigid3(submap_proto.pose()));
  }
}
MapById<SubmapId, mapping::proto::Submap> submap_id_to_submap;
// while 读子图数据
proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
    trajectory_remapping.at(
        proto.submap().submap_id().trajectory_id()));
submap_id_to_submap.Insert(
    SubmapId{proto.submap().submap_id().trajectory_id(),
             proto.submap().submap_id().submap_index()},
    proto.submap());
// 进入后端
for (const auto& submap_id_submap : submap_id_to_submap) {
  pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id_submap.id),
                                  submap_id_submap.data);
}

void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer)
{
    PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
    AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_)
    {
      absl::MutexLock locker(&mutex_);
      trimmers_.emplace_back(trimmer_ptr);
      return WorkItem::Result::kDoNotRunOptimization;
    });
}

// 纯定位时，子图的裁剪，如果没有裁剪器就不裁剪
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_)
{
	// 这里实际就是 PureLocalizationTrimmer::Trim
	trimmer->Trim(&trimming_handle);
}
// 裁剪器完成状态，删除裁剪器
trimmers_.erase(
    std::remove_if(trimmers_.begin(), trimmers_.end(),
                   [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                     return trimmer->IsFinished();
                   }),
    trimmers_.end());

void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph)
{ 
  // trajectory 是FINISHED状态，或者没有 trajectory
  if (pose_graph->IsFinished(trajectory_id_)) {
      num_submaps_to_keep_ = 0;
  }
  // 实际调用 TrimmingHandle::GetSubmapIds
  auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
  // 从序列号0开始裁剪子图，并删除相关的约束和节点
  // 剩下序列号大的子图
  for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i)
  {
      // 这是个大函数
      pose_graph->TrimSubmap(submap_ids.at(i)  );
  }
  if (num_submaps_to_keep_ == 0)
  {
      finished_ = true;
      pose_graph->SetTrajectoryState(
          trajectory_id_, 
          PoseGraphInterface::TrajectoryState::DELETED );
  }
}

std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
    int trajectory_id) const
{
  std::vector<SubmapId> submap_ids;
  // PoseGraph2D* const parent_;
  const auto& submap_data = parent_->optimization_problem_->submap_data();
  for (const auto& it : submap_data.trajectory(trajectory_id)  )
  {
      submap_ids.push_back(it.id);
  }
  return submap_ids;
}

// 分别根据 node_id 和 submap_id 获得 节点数据和子图
const TrajectoryNode::Data* constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
const Submap2D* submap = static_cast<const Submap2D*>(data_.submap_data.at(submap_id).submap.get() );

const transform::Rigid2d initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;

#include<ros/ros.h>
#include <boost/thread.hpp>
#include "boost/thread/mutex.hpp"
#include <boost/thread/condition.hpp>
using namespace std;

boost::mutex mut;
boost::condition cond;
boost::mutex::scoped_lock _lock(mut);

void thread_1(int n)
{
    while(true)
    {
    	// 线程1在这里阻塞，但先解开互斥锁，让其他线程能够得到这个互斥锁
        cond.wait(_lock);
        cout<< "thread 1: "<< rand()%50 <<endl;
        sleep(1);
    }
}

void thread_2(int n)
{
    int m;
    while(true)
    {
        m = 50 + rand()%50;
        cout<< "thread 2: "<< m <<endl;
        if(m > 90)
        {
        	// 发送一个信号给另外一个阻塞的线程，使其继续执行
            cond.notify_one();
            cout<<"thread 2 wake thread 1   ";
        }
        sleep(1);
    }
}

int main()
{
    unsigned int n = 2;
    boost::thread th_1 = boost::thread(boost::bind(&thread_1,n)); 
    boost::thread th_2 = boost::thread(boost::bind(&thread_2,n));

    th_1.join();
    th_2.join();
    return 0;
}

#include<ros/ros.h>
#include <boost/thread.hpp>
#include "boost/thread/mutex.hpp"
#include <boost/thread/condition.hpp>
using namespace std;

boost::mutex mut, mut_2;
boost::condition cond;

// 线程1和线程2 必须使用不同的 scoped_lock
boost::mutex::scoped_lock lock(mut);
boost::mutex::scoped_lock lock_2(mut_2);

void thread_1(int n)
{
    while(true)
    {
		cout << " ----- thread 1 is blocking ----- " << endl;
        cond.wait(lock);
        cout<< " ++++++++++++++ thread 1: "<< rand()%50 << " ++++++++++++++"  << endl;
        sleep(1);
    }
}

void thread_2(int n)
{
    while(true)
    {
		cout << " ----- thread 2 is blocking ----- " << endl;
        cond.wait(lock_2);
        cout<< " ************ thread 2: "<< rand()%50 << " ************" << endl;
        sleep(1);
    }
}

void thread_3(int n)
{
    int m;
    while(true)
    {
        m = 50 + rand()%50;
        cout<< "thread 3: "<< m <<endl;
        if(m > 88)
        {
        	// 唤醒一个
            cond.notify_one();
            cout<<" ------------------ thread 3 wake a thread ------------------ " <<endl;
        }
        sleep(1);
    }
}

int main()
{
    unsigned int n = 2;
    boost::thread th_1 = boost::thread(boost::bind(&thread_1,n));
    boost::thread th_2 = boost::thread(boost::bind(&thread_2,n));
    boost::thread th_3 = boost::thread(boost::bind(&thread_3,n));

	th_1.join();
    th_2.join();
    th_3.join();
    return 0;
}

#include <ros/ros.h>
#include <iostream>
#include <boost/ref.hpp>
#include <boost/thread/thread.hpp>
#include <string>
using namespace boost;
using namespace std;

int g_num = 10;      // 全局变量，写者改变该全局变量，读者读该全局变量
shared_mutex s_mu;   // 全局shared_mutex对象
 

void read_(std::string &str)
{
  const char* c = str.c_str();
  while (1)
  {
    {
      shared_lock<shared_mutex> m(s_mu);    //读锁定，shared_lock
      printf("线程 %s 进入临界区，global_num = %d\n", c, g_num);
      boost::this_thread::sleep(boost::posix_time::seconds(2));    //sleep 1秒，给足够的时间看其他线程是否能进入临界区
      printf("线程 %s 离开临界区...\n", c);
    }
    boost::this_thread::sleep(boost::posix_time::seconds(2));   //读线程离开后再sleep 1秒（改变这个值可以看到不一样的效果）
  }
  cout << std::endl;
}
 

void writer_(std::string &str)
{
  const char* c = str.c_str();
  while (1)
  {
    {
      unique_lock<shared_mutex> m(s_mu);    //写锁定，unique_lock
      ++g_num;
      printf("线程 %s 进入临界区，global_num = %d\n", c, g_num);
      boost::this_thread::sleep(boost::posix_time::seconds(1));    //sleep 1秒，让其他线程有时间进入临界区
      printf("线程 %s 离开临界区...\n", c);
    }
    //写线程离开后再sleep 2秒，如果这里也是1秒，那两个写线程一起请求锁时会让读线程饥饿
    boost::this_thread::sleep(boost::posix_time::seconds(2));   
  }
  cout << std::endl;
}

int main()
{
  std::string r1 = "read_1";
  std::string r2 = "read_2";
  std::string w1 = "writer_1";
  std::string w2 = "writer_2";
 
  boost::thread_group tg;
  tg.create_thread(bind(read_, boost::ref(r1)));   //两个读者
  tg.create_thread(bind(read_, boost::ref(r2)));
 
  tg.create_thread(bind(writer_, boost::ref(w1)));  //两个写者
  tg.create_thread(bind(writer_, boost::ref(w2)));
  tg.join_all();
 
  return 0;
}

// 第二阶段
// 如果栅格之前的 hit>0，那么就 ray 次数增加
if (voxels_.value(index).hits > 0)
{
  ++voxels_.mutable_value(index)->rays;
}

// 第三阶段
// 如果点的 rays > 3*hits 那么就认为是动态障碍物
if (!(voxel.rays < miss_per_hit_limit_ * voxel.hits))
{
    to_remove.insert(i);
}