// 模板参数依次为： number of residuals,  first parameter的维度
class MyCostFun : public SizedCostFunction<1, 1, 1, 1>
{
    public:
		MyCostFun(double x, double y):
			m_x(x), m_y(y){}
   virtual bool Evaluate(double const* const* parameters,
		      	 double* residuals,
			 double** jacobians) const
   {
       double a = parameters[0][0];
       double b = parameters[0][1];
       double c = parameters[0][2];
       residuals[0] = m_y - exp(a*m_x*m_x + b*m_x + c);

		if (jacobians != NULL && jacobians[0] != NULL)
		{
		    jacobians[0][0] = -exp(a*m_x*m_x + b*m_x + c)* m_x * m_x;
		    jacobians[0][1] = -exp(a*m_x*m_x + b*m_x + c)* m_x;
		    jacobians[0][2] = -exp(a*m_x*m_x + b*m_x + c);

		}
		return true;
   }
   protected:
	double m_x;
    double m_y;
};

int main(int argv, char** argc)
{
    ros::init(argv, argc, "test_ceres");
    ros::NodeHandle nh;
    // 真实值
	double a = 1.0, b = 2.0, c = 1.0;

	// 生成带有噪声的模拟数据
	vector<double> x_data, y_data;
	int N = 240;  // 数据数量
	double w_sigma = 1.0;  // 高斯标准差
	cv::RNG  rng;
	for(unsigned int i =0; i<N; i++)
	{
		double x = i/100.0;
		x_data.push_back(x);
		y_data.push_back(exp(a*x*x + b*x + c) + rng.gaussian(w_sigma)  );
	}
	// 作为初值
	a = 0.0;
	b = 0.0;
	c = 0.0;
	Problem problem;
	for(unsigned int i=0; i<N; i++)
	{
		CostFunction* cost_func = new MyCostFun(x_data[i], y_data[i]);
		problem.AddResidualBlock(cost_func, nullptr, &a, &b, &c);
	}
	Solver::Options option;
	option.minimizer_progress_to_stdout = true;

	Solver::Summary summary;
	Solve(option, &problem, &summary);
	cout << summary.BriefReport() << endl <<endl;
	cout << "a: "<< a <<"  b: "<< b << "  c: "<< c <<endl;

    return 0;
}

set_num_residuals(kNumResiduals);
*mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};

class CERES_EXPORT QuaternionParameterization : public LocalParameterization
{
 public:
  virtual ~QuaternionParameterization() {}

  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;

  // 表示参数的自由度，也就是 Manifold 空间（四元数）是 4 维的
  // 即四元数本身的实际维数，如果是旋转矩阵，则为 9
  virtual int GlobalSize() const { return 4; }
  // 表示迭代增量所在的切空间(tangent space)是 3 维的。即旋转矢量的实际维数
  virtual int LocalSize() const { return 3; }
};

bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const
{
	/* -------- 将旋转矢量转换为四元数形式-------- */
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0)
  {
  	// 这里的 除以norm_delta 相当于归一化
  	const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
  	double q_delta[4];
  	q_delta[0] = cos(norm_delta);
  	q_delta[1] = sin_delta_by_delta * delta[0];
  	q_delta[2] = sin_delta_by_delta * delta[1];
  	q_delta[3] = sin_delta_by_delta * delta[2];

  	/* --------2.采用四元数乘法更新-------- */
  	// 注意这个乘法的顺序关乎求雅克比矩阵，具体说明参照 ComputeJacobian
  	x_plus_delta = q_delta * x;
  }
  else
  {
    for (int i = 0; i < 4; ++i)
        x_plus_delta[i] = x[i];
  }
  return true;
}

// x[0] 对应 w
bool QuaternionParameterization::ComputeJacobian(const double* x,
                                                 double* jacobian) const
{
  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
  return true;
}

ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param) //重构参数，优化时实际使用的是3维的等效旋转矢量

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