#include <iostream>
#include <ros/ros.h>
#include "hdl_backend.hpp"
// #include <pcl_ros/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <hdl_graph_slam/ros_utils.hpp>
#include <functional>  // std::bind 和 std::placeholders

#include "hdl_graph_slam/keyframe_updater.hpp"
#include <visualization_msgs/MarkerArray.h>

#include <glog/logging.h>

// #include <vector.h>

#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

hdl_backend* instance = nullptr;  // 初始化静态单例(分配内存, 调用hdl_backend 的默认构造函数)
// hdl_backend& hdl_opt = hdl_backend::getInstance();  // 因为 静态成员变量 instance 是私有的,需要 静态成员函数 getInstance() 拿到 instance, 并且起个别名(引用)

typedef pcl::PointXYZI PointT;

int main(int argc, char* argv[]) {
  // 执行 ros 节点初始化
  ros::init(argc, argv, "hdl_backend");
  std::cout << "hdl backend get in ...===============" << std::endl;
  instance = new hdl_backend();
  // hdl_backend& hdl_opt = instance->getInstance();
  instance->init();
  return 0;
}

void hdl_backend::init() {
  map_frame_id = "map";
  odom_frame_id = "odom";
  map_cloud_resolution = 0.01;   // 地图点云分辨率
  trans_odom2map.setIdentity();  // 设置为单位矩阵
  max_keyframes_per_update = 10;
  keyframe_updater.reset(new hdl_graph_slam::KeyframeUpdater(nh));
  points_topic = "/velodyne_points";
  graph_slam.reset(new hdl_graph_slam::GraphSLAM("lm_var_cholmod"));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
  // graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象

  auto voxelgrid = new pcl::VoxelGrid<PointT>();
  voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
  downsample_filter.reset(voxelgrid);

  inf_calclator.reset(new hdl_graph_slam::InformationMatrixCalculator(nh));
  loop_detector.reset(new hdl_graph_slam::LoopDetector(nh));

  anchor_node = nullptr;
  anchor_edge = nullptr;
  odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(nh, "/laser_odom", 256));  // 创建 message_filters::Subscriber 对象，用于订阅nav_msgs::Odometry类型的消息
  // cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/keyframe_points", 32));        // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
  // cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/velodyne_points", 32));  // 三维
  cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/filtered_points", 32));  // 二维

  // 现在的 /filtered_points 是原始点云数据,没有做坐标转换, 滤波, 将采样等
  // keyframe_points 在前端做了降采样

  sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));  // 创建 message_filters::Synchronizer 对象，用于同步上面创建的两个订阅者（odom_sub和cloud_sub）的消息。
  sync->registerCallback(boost::bind(&hdl_backend::cloud_callback, this, _1, _2));                               // 为同步器注册了一个回调函数，当同步条件满足时，会调用HdlGraphSlamNodelet类的cloud_callback成员函数

  markers_pub = nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);        // 发布可视化数据（具体是什么还不知道）   这种类型的消息通常用于在RViz等可视化工具中显示标记（markers）
  odom2map_pub = nh.advertise<geometry_msgs::TransformStamped>("/hdl_graph_slam/odom2pub", 16);      // 发布从里程计到地图的变换
  map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points", 1, true);    // 这个话题会保留以前扫描过的点云！！ 扫过的地方的点云就是
  map_points_pub2 = nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points2", 1, true);  // 这个话题会保留以前扫描过的点云！！ 扫过的地方的点云就是

  read_until_pub = nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);  // 这种类型的消息通常用于指示读取操作应该持续到何时
  odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/opt_odom", 32);

  save_map_service_server = nh.advertiseService("/hdl_graph_slam/save_map", &hdl_backend::save_map_service, this);  // 保存地图

  graph_updated = false;

  double graph_update_interval = 3;      // 表示图优化更新的时间间隔，默认值为3.0秒    interval：间隔
  double map_cloud_update_interval = 5;  // 地图点云更新的时间间隔，默认值为10.0秒

  // optimization_timer = nh.createWallTimer(ros::WallDuration(graph_update_interval), &hdl_backend::optimization_timer_callback, this);  // 创建一个定时器，定时器会在每个 graph_update_interval 秒后触发，然后调用 optimization_timer_callback 函数
  map_publish_timer = nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &hdl_backend::map_points_publish_timer_callback, this);  // 创建一个定时器，定时器会在每个 map_cloud_update_interval 秒后触发，然后调用 map_points_publish_timer_callback 函数

  pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
  rad->setRadiusSearch(0.8);
  rad->setMinNeighborsInRadius(1);
  outlier_removal_filter = rad;

  std::thread proc_thread(&hdl_backend::optimization_timer_callback, this);
  proc_thread.detach();

  ros::spin();
}




/**
 * @brief 这个函数将队列中的所有数据添加到姿势图中，然后优化姿势图
 * @param event   // 是ROS提供的定时器时间类，包含信息有：计时器出发的时间戳、上一次出发的时间戳、计时器的周期信息等
 */
// void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event) {
void hdl_backend::optimization_timer_callback() {
  LOG(INFO) << "begin()";
  while(ros::ok()) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);  // 创建了一个锁，用于保护对主线程的访问，确保线程安全
    // add keyframes and floor coeffs in the queues to the pose graph  将队列中的关键帧和楼层系数添加到姿势图中
    bool keyframe_updated = flush_keyframe_queue();  // 调用 flush_keyframe_queue 函数，将关键帧队列中的数据添加到位姿图中，并返回是否有关键帧被更新

    if(!keyframe_updated) {  // 如果没有关键帧被更新，则执行大括号内的代码
      std_msgs::Header read_until;
      read_until.stamp = ros::Time::now() + ros::Duration(30, 0);  // 时间戳为什么要加30秒？
      read_until.frame_id = points_topic;
      read_until_pub.publish(read_until);
      read_until.frame_id = "/filtered_points";
      read_until_pub.publish(read_until);  // 不同的 frame_id 和 话题 发布两次， 是干嘛用的？
    }
    if(!keyframe_updated) {
      // 检查地板、GPS、IMU等队列，分别调用相应的 flush 函数（例如 flush_floor_queue、flush_gps_queue、flush_imu_queue），如果这些数据没有更新，则函数直接返回，不进行后续操作。
      // LOG(INFO) << "关键帧没有更新,usleep(30000);  & continue;";
      // return;
      usleep(30000);
      continue;
    }

    // loop detection   闭环检测
    std::vector<hdl_graph_slam::Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);

    // 如果检测到闭环，则为闭环关系添加一个新的边到位姿图中，计算相对位姿并生成信息矩阵，确保闭环边的优化权重。

    for(const auto& loop : loops) {  // 遍历闭环检测结果
      Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
      Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
      auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);  // 添加边，位姿之间的关系构成边(edge)
      graph_slam->add_robust_kernel(edge, "Huber", 2);
      // graph_slam->add_robust_kernel(edge, "NONE", 0.01);   // Huber  Cauchy   Tukey  None
    }
    std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));  // 将新关键帧 new_keyframes 合并到已有的关键帧列表 keyframes 中
    new_keyframes.clear();                                                                 // 清空 new_keyframes

    // move the first node anchor position to the current estimate of the first node pose   将第一节点锚点位置移动到第一节点姿态的当前估计值
    // so the first node moves freely while trying to stay around the origin                因此 ， 第一个节点在试图停留在原点附近的同时可以自由移动

    if(anchor_node && true) {  // launch文件中，fix_first_node_adaptive 设置为 true   // 这个能不能设置为false
      Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
      anchor_node->setEstimate(anchor_target);
      // 如果启用了自适应固定第一帧的功能（参数 "fix_first_node_adaptive"），则将第一个关键帧的锚点位置更新为当前估计的位置，以允许它自由移动但仍然保持在原点附近。
    }
    // optimize the pose graph
    int num_iterations = 5000;  // launch文件中都是设置成512

    // for(int i = 0; i < keyframes.size(); i++){
    //   // std::cout << "before optimize, odom[" << i << "] = \n" << keyframes[i]->odom.matrix() << std::endl;
    //   std::cout << "before optimize, odom[" << i << "] = \n" << keyframes[i]->node->estimate().matrix() << std::endl;
    // }

    // if(num_added_keyframe % 5 == 0){
    graph_slam->optimize(num_iterations);
    // }
    num_added_keyframe++;

    LOG(INFO) << "完成g2o的图优化==============";

    // for(int i = 0; i < keyframes.size(); i++){
    //   // std::cout << "after optimize, odom[" << i << "] = \n" << keyframes[i]->odom.matrix() << std::endl;
    //   std::cout << "after optimize, odom[" << i << "] = \n" << keyframes[i]->node->estimate().matrix() << std::endl;
    // }

    std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
    std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const hdl_graph_slam::KeyFrame::Ptr& k) { return std::make_shared<hdl_graph_slam::KeyFrameSnapshot>(k); });

    keyframes_snapshot_mutex.lock();
    keyframes_snapshot.swap(snapshot);
    keyframes_snapshot_mutex.unlock();
    graph_updated = true;

    // publish tf     发布位姿变换
    const auto& keyframe = keyframes.back();  // 获取最新的关键帧估计
    Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse();
    trans_odom2map_mutex.lock();
    trans_odom2map = trans.matrix().cast<float>();
    trans_odom2map_mutex.unlock();

    // if(odom2map_pub.getNumSubscribers()) {
    geometry_msgs::TransformStamped ts = hdl_graph_slam::matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
    // ts.header.stamp = ros::Time::now();   // ?
    // odom2map_pub.publish(ts);  // 发布odom2map_pub话题中/
    // LOG(INFO) << "odom2map TF变换发布完成";
    // }

    // if(markers_pub.getNumSubscribers()) {
    auto markers = create_marker_array(ros::Time::now());
    markers_pub.publish(markers);
    // LOG(INFO) << "markers_pub 发布完成";
    // }
    publishMatrixAsPose(odom_pub, keyframes[keyframes.size() - 1]->node->estimate().matrix().cast<float>());
    // nav_msgs::Odometry
    usleep(30000);
  }

  LOG(INFO) << "end()";
}

void hdl_backend::publishMatrixAsPose(ros::Publisher& publisher, const Eigen::Matrix4f& matrix) {
  // 提取平移部分
  Eigen::Vector3f translation = matrix.block<3, 1>(0, 3);

  // 提取旋转部分
  Eigen::Matrix3f rotation_matrix = matrix.block<3, 3>(0, 0);
  Eigen::Quaternionf quaternion(rotation_matrix);

  // 创建并填充 PoseStamped 消息
  geometry_msgs::PoseStamped pose_msg;
  pose_msg.header.stamp = ros::Time ::now();
  pose_msg.header.frame_id = "odom";  // 设置坐标系

  pose_msg.pose.position.x = translation.x();
  pose_msg.pose.position.y = translation.y();
  pose_msg.pose.position.z = translation.z();
  pose_msg.pose.orientation.x = quaternion.x();
  pose_msg.pose.orientation.y = quaternion.y();
  pose_msg.pose.orientation.z = quaternion.z();
  pose_msg.pose.orientation.w = quaternion.w();

  // 发布消息
  publisher.publish(pose_msg);
}

/**
 * @brief generate map point cloud and publish it
 * @param event
 */
void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
  // 源码中，发布是图优化完之后发布才有内容的
  // LOG(INFO) << graph_updated;
  if(!graph_updated) {
    // if(!map_points_pub.getNumSubscribers() || !graph_updated) {
    return;
  }

  // 直接发keyframe试一下?

  pcl::PointCloud<PointT>::Ptr trans_cloud;
  trans_cloud.reset(new pcl::PointCloud<PointT>());
  for(int i = 0; i < keyframes.size(); i++) {
    pcl::PointCloud<PointT>::Ptr cloud_i(new pcl::PointCloud<PointT>());
    pcl::transformPointCloud(*keyframes[i]->cloud, *cloud_i, keyframes[i]->node->estimate().matrix());
    *trans_cloud += *cloud_i;
  }
  trans_cloud->header.frame_id = map_frame_id;
  trans_cloud->header.stamp = keyframes_snapshot.back()->cloud->header.stamp;

  // 发布
  sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
  pcl::toROSMsg(*trans_cloud, *cloud2_msg);
  cloud2_msg->header.frame_id = "map";
  // keyframe_points_pub.publish(*cloud2_msg);  // 话题名:
  map_points_pub2.publish(*cloud2_msg);

  // 发布点云数据
  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
  snapshot = keyframes_snapshot;

  // LOG(INFO) << "keyframes_snapshot.size() = " << keyframes_snapshot.size();
  auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
  if(!cloud) {
    return;
  }
  cloud->header.frame_id = map_frame_id;
  cloud->header.stamp = snapshot.back()->cloud->header.stamp;

  sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
  pcl::toROSMsg(*cloud, *cloud_msg);

  map_points_pub.publish(*cloud_msg);
  // LOG(INFO) << "点云数据发布完成";

  // LOG(INFO) << "map_points_publish_timer_callback 函数执行完毕";
}

/**
 * @brief downsample a point cloud
 * @param cloud  input cloud
 * @return downsampled point cloud
 */
pcl::PointCloud<PointT>::ConstPtr hdl_backend::downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 对点云数据进行向下采样，减少点的数量以提高处理速度
  if(!downsample_filter) {
    return cloud;
  }
  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象，用来存储下采样后的点云数据
  downsample_filter->setInputCloud(cloud);
  downsample_filter->filter(*filtered);
  return filtered;
}

pcl::PointCloud<PointT>::ConstPtr hdl_backend::outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) {
  if(!outlier_removal_filter) {
    return cloud;
  }

  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
  outlier_removal_filter->setInputCloud(cloud);
  outlier_removal_filter->filter(*filtered);
  filtered->header = cloud->header;

  return filtered;
}

/**
 * @brief 接收点云数据并放到 keyframe_queue 中
 * @param odom_msg  前端的激光里程计数据
 * @param cloud_msg 前端滤波后的点云数据
 */
void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
  const ros::Time& stamp = cloud_msg->header.stamp;                  // 获取点云数据的时间戳
  Eigen::Isometry3d odom = hdl_graph_slam::odom2isometry(odom_msg);  // 将里程计消息转换为Eigen库中的Isometry3d类型，它表示一个3D刚体变换（包括旋转和平移）

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  // 创建一个点云对象的指针
  pcl::fromROSMsg(*cloud_msg, *cloud);                                // 将ROS的点云消息 cloud_msg 转换为PCL（Point Cloud Library）的点云格式
  if(base_frame_id.empty()) {                                         // 用类的单例实现应该就ok?
    base_frame_id = cloud_msg->header.frame_id;                       // 将当前点云消息的坐标系 frame_id 设置为基础坐标系      base_frame_id 是整个系统中关键帧数据的参考坐标系
  }

  auto filtered = downsample(cloud);
  filtered = outlier_removal(filtered);

  // 更新关键帧判断
  double delta_time = 1000;
  if(keyframe_queue.size() != 0) delta_time = (stamp - keyframe_queue.back()->stamp).toSec();
  if(!keyframe_updater->update(odom, delta_time)) {  // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
    // std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列

    if(keyframe_queue.empty()) {  // 如果关键帧队列是空的，发布一个 ROS 消息通知系统继续读取点云数据，直到指定时间点（stamp + ros::Duration(10, 0)）。这段代码确保在没有关键帧生成时，系统能够继续读取点云数据
      // LOG(INFO) << "keyframe_queue.empty()";
      std_msgs::Header read_until;
      read_until.stamp = stamp + ros::Duration(10, 0);
      read_until.frame_id = points_topic;
      read_until_pub.publish(read_until);
      read_until.frame_id = "/filtered_points";
      read_until_pub.publish(read_until);
    }
    return;  // 　如果没有关键帧更新，就　return
  }

  double accum_d = keyframe_updater->get_accum_distance();  // 获取累计的运动距离，用于判断关键帧生成的条件
  // std::cout << "============ accum_d ============" << accum_d << std::endl;
  hdl_graph_slam::KeyFrame::Ptr keyframe(new hdl_graph_slam::KeyFrame(stamp, odom, accum_d, filtered));  // 创建一个新的关键帧 keyframe，其中包含当前的时间戳 stamp，里程计信息 odom，累计距离 accum_d，以及处理后的点云数据 cloud（这里的处理就是做了个消息类型的转换）

  // LOG(INFO) << "get new keyframe!!!";

  // std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 使用 std::lock_guard 加锁，确保对关键帧队列 keyframe_queue 的操作是线程安全的
  keyframe_queue.push_back(keyframe);  // 将新生成的关键帧 keyframe 推入 keyframe_queue 队列，供后续的处理使用
  // LOG(INFO) << "keyframe_queue.push_back(keyframe)";
}

/**
 * @brief this method adds all the keyframes in #keyframe_queue to the pose graph (odometry edges)    此方法将#keyframe_queue中的所有关键帧添加到姿势图中（里程计边）
 * @return if true, at least one keyframe was added to the pose graph   如果为真，则至少有一个关键帧被添加到姿势图中
 * 将队列中的关键帧添加到位姿图中，并返回是否至少有一个关键帧被添加
 */
bool hdl_backend::flush_keyframe_queue() {
  std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 多线程锁

  // LOG(INFO) << "flush_keyframe_queue";

  // keyframe_queue: 存放关键帧队列

  if(keyframe_queue.empty()) {  // 没有关键帧就返回
    return false;
  }

  trans_odom2map_mutex.lock();
  Eigen::Isometry3d odom2map(trans_odom2map.cast<double>());  // odom 到　map　的齐次变换矩阵
  trans_odom2map_mutex.unlock();

  // std::cout << "flush_keyframe_queue - keyframes len:" << keyframes.size() << std::endl;
  int num_processed = 0;
  // for(int i = 0; i < std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) { // max_keyframes_per_update = 10
  // 这里有问题吧？　keyframe_queue　是　push_back　的，是加在后面的呀，上面这个for循环不是只处理前１０个keyframe？
  for(int i = 0; i < keyframe_queue.size(); i++) {  // max_keyframes_per_update = 10
    num_processed = i;

    const auto& keyframe = keyframe_queue[i];
    // new_keyframes will be tested later for loop closure
    new_keyframes.push_back(keyframe);

    // add pose node
    Eigen::Isometry3d odom = odom2map * keyframe->odom;  // 一个4*4的矩阵，齐次变换矩阵
    keyframe->node = graph_slam->add_se3_node(odom);     // 添加节点，在graph-based SLAM中，机器人的位姿是一个节点（node）

    // fix the first node  固定第一个节点,整段代码只运行一次
    if(keyframes.empty() && new_keyframes.size() == 1) {
      // if(nh.param<bool>("fix_first_node", false)) {  // launch 中是 true
      Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
      // std::stringstream sst(nh.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));  // 这个是什么意思?
      std::stringstream sst("10 10 10 1 1 1");  // 这个是什么意思?

      for(int i = 0; i < 6; i++) {
        double stddev = 1.0;
        sst >> stddev;
        inf(i, i) = 1.0 / stddev;
      }
      anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());  // 添加节点，在graph-based SLAM中，机器人的位姿是一个节点（node）
      anchor_node->setFixed(true);
      anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);  // 添加边，位姿之间的关系构成边(edge)
      // }
    }

    if(i == 0 && keyframes.empty()) {
      continue;
    }

    // add edge between consecutive keyframes
    const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];

    Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
    Eigen::MatrixXd information = inf_calclator->calc_information_matrix(keyframe->cloud, prev_keyframe->cloud, relative_pose);
    auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);  // 添加边，位姿之间的关系构成边(edge)
    graph_slam->add_robust_kernel(edge, "NONE", 1.0);
    // graph_slam->add_robust_kernel(edge, "Tukey", 0.1);

    // std::cout << "keyframe->node = \n" << keyframe->node->estimate().matrix() << std::endl;
    // std::cout << "prev_keyframe->node = \n" << prev_keyframe->node->estimate().matrix() << std::endl;
  }

  std_msgs::Header read_until;
  read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(10, 0);
  read_until.frame_id = points_topic;
  read_until_pub.publish(read_until);
  read_until.frame_id = "/filtered_points";
  read_until_pub.publish(read_until);

  keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);  // 这里把keyframe都删了
  return true;
}

  /**
   * @brief save map data as pcd
   * @param req
   * @param res
   * @return
   */
  bool hdl_backend::save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res) {
    std::cout << "get in save_map_service !!!" << std::endl;
    std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot1;

    // keyframes_snapshot_mutex.lock();
    snapshot1 = keyframes_snapshot;
    // keyframes_snapshot_mutex.unlock();

    auto cloud = map_cloud_generator->generate(snapshot1, req.resolution);
    if(!cloud) {
      res.success = false;
      return true;
    }

    if(zero_utm && req.utm) {
      for(auto& pt : cloud->points) {
        pt.getVector3fMap() += (*zero_utm).cast<float>();
      }
    }

    cloud->header.frame_id = "map";
    cloud->header.stamp = snapshot1.back()->cloud->header.stamp;

    if(zero_utm) {
      std::ofstream ofs(req.destination + ".utm");
      ofs << boost::format("%.6f %.6f %.6f") % zero_utm->x() % zero_utm->y() % zero_utm->z() << std::endl;
    }

    int ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
    res.success = ret == 0;

    return true;
  }


/**
 * @brief create visualization marker
 * @param stamp
 * @return
 */
visualization_msgs::MarkerArray hdl_backend::create_marker_array(const ros::Time& stamp) const {
  visualization_msgs::MarkerArray markers;
  markers.markers.resize(4);

  // node markers
  visualization_msgs::Marker& traj_marker = markers.markers[0];
  traj_marker.header.frame_id = "map";
  traj_marker.header.stamp = stamp;
  traj_marker.ns = "nodes";
  traj_marker.id = 0;
  traj_marker.type = visualization_msgs::Marker::SPHERE_LIST;

  traj_marker.pose.orientation.w = 1.0;
  traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 0.5;

  visualization_msgs::Marker& imu_marker = markers.markers[1];
  imu_marker.header = traj_marker.header;
  imu_marker.ns = "imu";
  imu_marker.id = 1;
  imu_marker.type = visualization_msgs::Marker::SPHERE_LIST;

  imu_marker.pose.orientation.w = 1.0;
  imu_marker.scale.x = imu_marker.scale.y = imu_marker.scale.z = 0.75;

  traj_marker.points.resize(keyframes.size());
  traj_marker.colors.resize(keyframes.size());
  for(int i = 0; i < keyframes.size(); i++) {
    Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
    traj_marker.points[i].x = pos.x();
    traj_marker.points[i].y = pos.y();
    traj_marker.points[i].z = pos.z();

    double p = static_cast<double>(i) / keyframes.size();
    traj_marker.colors[i].r = 1.0 - p;
    traj_marker.colors[i].g = p;
    traj_marker.colors[i].b = 0.0;
    traj_marker.colors[i].a = 1.0;

    if(keyframes[i]->acceleration) {
      Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
      geometry_msgs::Point point;
      point.x = pos.x();
      point.y = pos.y();
      point.z = pos.z();

      std_msgs::ColorRGBA color;
      color.r = 0.0;
      color.g = 0.0;
      color.b = 1.0;
      color.a = 0.1;

      imu_marker.points.push_back(point);
      imu_marker.colors.push_back(color);
    }
  }

  // edge markers
  visualization_msgs::Marker& edge_marker = markers.markers[2];
  edge_marker.header.frame_id = "map";
  edge_marker.header.stamp = stamp;
  edge_marker.ns = "edges";
  edge_marker.id = 2;
  edge_marker.type = visualization_msgs::Marker::LINE_LIST;

  edge_marker.pose.orientation.w = 1.0;
  edge_marker.scale.x = 0.05;

  edge_marker.points.resize(graph_slam->graph->edges().size() * 2);
  edge_marker.colors.resize(graph_slam->graph->edges().size() * 2);

  auto edge_itr = graph_slam->graph->edges().begin();
  for(int i = 0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) {
    g2o::HyperGraph::Edge* edge = *edge_itr;
    g2o::EdgeSE3* edge_se3 = dynamic_cast<g2o::EdgeSE3*>(edge);
    if(edge_se3) {
      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[0]);
      g2o::VertexSE3* v2 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[1]);
      Eigen::Vector3d pt1 = v1->estimate().translation();
      Eigen::Vector3d pt2 = v2->estimate().translation();

      edge_marker.points[i * 2].x = pt1.x();
      edge_marker.points[i * 2].y = pt1.y();
      edge_marker.points[i * 2].z = pt1.z();
      edge_marker.points[i * 2 + 1].x = pt2.x();
      edge_marker.points[i * 2 + 1].y = pt2.y();
      edge_marker.points[i * 2 + 1].z = pt2.z();

      double p1 = static_cast<double>(v1->id()) / graph_slam->graph->vertices().size();
      double p2 = static_cast<double>(v2->id()) / graph_slam->graph->vertices().size();
      edge_marker.colors[i * 2].r = 1.0 - p1;
      edge_marker.colors[i * 2].g = p1;
      edge_marker.colors[i * 2].a = 1.0;
      edge_marker.colors[i * 2 + 1].r = 1.0 - p2;
      edge_marker.colors[i * 2 + 1].g = p2;
      edge_marker.colors[i * 2 + 1].a = 1.0;

      if(std::abs(v1->id() - v2->id()) > 2) {
        edge_marker.points[i * 2].z += 0.5;
        edge_marker.points[i * 2 + 1].z += 0.5;
      }

      continue;
    }

    g2o::EdgeSE3Plane* edge_plane = dynamic_cast<g2o::EdgeSE3Plane*>(edge);
    if(edge_plane) {
      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_plane->vertices()[0]);
      Eigen::Vector3d pt1 = v1->estimate().translation();
      Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0);

      edge_marker.points[i * 2].x = pt1.x();
      edge_marker.points[i * 2].y = pt1.y();
      edge_marker.points[i * 2].z = pt1.z();
      edge_marker.points[i * 2 + 1].x = pt2.x();
      edge_marker.points[i * 2 + 1].y = pt2.y();
      edge_marker.points[i * 2 + 1].z = pt2.z();

      edge_marker.colors[i * 2].b = 1.0;
      edge_marker.colors[i * 2].a = 1.0;
      edge_marker.colors[i * 2 + 1].b = 1.0;
      edge_marker.colors[i * 2 + 1].a = 1.0;

      continue;
    }

    g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast<g2o::EdgeSE3PriorXY*>(edge);
    if(edge_priori_xy) {
      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xy->vertices()[0]);
      Eigen::Vector3d pt1 = v1->estimate().translation();
      Eigen::Vector3d pt2 = Eigen::Vector3d::Zero();
      pt2.head<2>() = edge_priori_xy->measurement();

      edge_marker.points[i * 2].x = pt1.x();
      edge_marker.points[i * 2].y = pt1.y();
      edge_marker.points[i * 2].z = pt1.z() + 0.5;
      edge_marker.points[i * 2 + 1].x = pt2.x();
      edge_marker.points[i * 2 + 1].y = pt2.y();
      edge_marker.points[i * 2 + 1].z = pt2.z() + 0.5;

      edge_marker.colors[i * 2].r = 1.0;
      edge_marker.colors[i * 2].a = 1.0;
      edge_marker.colors[i * 2 + 1].r = 1.0;
      edge_marker.colors[i * 2 + 1].a = 1.0;

      continue;
    }

    g2o::EdgeSE3PriorXYZ* edge_priori_xyz = dynamic_cast<g2o::EdgeSE3PriorXYZ*>(edge);
    if(edge_priori_xyz) {
      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xyz->vertices()[0]);
      Eigen::Vector3d pt1 = v1->estimate().translation();
      Eigen::Vector3d pt2 = edge_priori_xyz->measurement();

      edge_marker.points[i * 2].x = pt1.x();
      edge_marker.points[i * 2].y = pt1.y();
      edge_marker.points[i * 2].z = pt1.z() + 0.5;
      edge_marker.points[i * 2 + 1].x = pt2.x();
      edge_marker.points[i * 2 + 1].y = pt2.y();
      edge_marker.points[i * 2 + 1].z = pt2.z();

      edge_marker.colors[i * 2].r = 1.0;
      edge_marker.colors[i * 2].a = 1.0;
      edge_marker.colors[i * 2 + 1].r = 1.0;
      edge_marker.colors[i * 2 + 1].a = 1.0;

      continue;
    }
  }

  // sphere
  visualization_msgs::Marker& sphere_marker = markers.markers[3];
  sphere_marker.header.frame_id = "map";
  sphere_marker.header.stamp = stamp;
  sphere_marker.ns = "loop_close_radius";
  sphere_marker.id = 3;
  sphere_marker.type = visualization_msgs::Marker::SPHERE;

  if(!keyframes.empty()) {
    Eigen::Vector3d pos = keyframes.back()->node->estimate().translation();
    sphere_marker.pose.position.x = pos.x();
    sphere_marker.pose.position.y = pos.y();
    sphere_marker.pose.position.z = pos.z();
  }
  sphere_marker.pose.orientation.w = 1.0;
  sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0;

  sphere_marker.color.r = 1.0;
  sphere_marker.color.a = 0.3;

  return markers;
}