LIVMapper.cpp

/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.

Developer: Chunran Zheng <zhengcr@connect.hku.hk>

For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.

This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/

#include "LIVMapper.h"

#include <filesystem>
#include <sensor_msgs/image_encodings.hpp>

LIVMapper::LIVMapper(const rclcpp::NodeOptions &options)
    : rclcpp::Node("laserMapping", options),
      extT(0, 0, 0),
      extR(M3D::Identity())
{
  extrinT.assign(3, 0.0);
  extrinR.assign(9, 0.0);
  cameraextrinT.assign(3, 0.0);
  cameraextrinR.assign(9, 0.0);

  p_pre.reset(new Preprocess());
  p_imu.reset(new ImuProcess());

  readParameters();
  VoxelMapConfig voxel_config;
  loadVoxelConfig(this, voxel_config);

  visual_sub_map.reset(new PointCloudXYZI());
  feats_undistort.reset(new PointCloudXYZI());
  feats_down_body.reset(new PointCloudXYZI());
  feats_down_world.reset(new PointCloudXYZI());
  pcl_w_wait_pub.reset(new PointCloudXYZI());
  pcl_wait_pub.reset(new PointCloudXYZI());
  pcl_wait_save.reset(new PointCloudXYZRGB());
  pcl_wait_save_intensity.reset(new PointCloudXYZI());
  octomap_latest_cloud.reset(new PointCloudXYZI());
  voxelmap_manager.reset(new VoxelMapManager(voxel_config, voxel_map));
  vio_manager.reset(new VIOManager());
  root_dir = ROOT_DIR;
  initializeFiles();
  initializeComponents();
  path.header.stamp = this->now();
  path.header.frame_id = "camera_init";
  initializeSubscribersAndPublishers();
}

rclcpp::Time LIVMapper::currentStamp()
{
  double t = LidarMeasures.last_lio_update_time;
  if (LidarMeasures.lio_vio_flg == VIO && !LidarMeasures.measures.empty()) { t = LidarMeasures.measures.back().vio_time; }
  if (t <= 0.0) return this->now();
  return get_ros_time(t);
}

LIVMapper::~LIVMapper() {}

void LIVMapper::readParameters()
{
  ros_params::get(this, "common.lid_topic", lid_topic, "/livox/lidar");
  ros_params::get(this, "common.imu_topic", imu_topic, "/livox/imu");
  ros_params::get(this, "common.ros_driver_bug_fix", ros_driver_fix_en, false);
  ros_params::get(this, "common.img_en", img_en, 1);
  ros_params::get(this, "common.lidar_en", lidar_en, 1);
  ros_params::get(this, "common.img_topic", img_topic, "/left_camera/image");

  ros_params::get(this, "vio.normal_en", normal_en, true);
  ros_params::get(this, "vio.inverse_composition_en", inverse_composition_en, false);
  ros_params::get(this, "vio.max_iterations", max_iterations, 5);
  ros_params::get(this, "vio.img_point_cov", IMG_POINT_COV, 100.0);
  ros_params::get(this, "vio.raycast_en", raycast_en, false);
  ros_params::get(this, "vio.exposure_estimate_en", exposure_estimate_en, true);
  ros_params::get(this, "vio.inv_expo_cov", inv_expo_cov, 0.2);
  ros_params::get(this, "vio.grid_size", grid_size, 5);
  ros_params::get(this, "vio.grid_n_height", grid_n_height, 17);
  ros_params::get(this, "vio.patch_pyrimid_level", patch_pyrimid_level, 3);
  ros_params::get(this, "vio.patch_size", patch_size, 8);
  ros_params::get(this, "vio.outlier_threshold", outlier_threshold, 1000.0);

  ros_params::get(this, "time_offset.exposure_time_init", exposure_time_init, 0.0);
  ros_params::get(this, "time_offset.img_time_offset", img_time_offset, 0.0);
  ros_params::get(this, "time_offset.imu_time_offset", imu_time_offset, 0.0);
  ros_params::get(this, "time_offset.lidar_time_offset", lidar_time_offset, 0.0);
  ros_params::get(this, "uav.imu_rate_odom", imu_prop_enable, false);
  ros_params::get(this, "uav.gravity_align_en", gravity_align_en, false);

  ros_params::get(this, "evo.seq_name", seq_name, "01");
  ros_params::get(this, "evo.pose_output_en", pose_output_en, false);
  ros_params::get(this, "imu.gyr_cov", gyr_cov, 1.0);
  ros_params::get(this, "imu.acc_cov", acc_cov, 1.0);
  ros_params::get(this, "imu.imu_int_frame", imu_int_frame, 3);
  ros_params::get(this, "imu.imu_en", imu_en, false);
  ros_params::get(this, "imu.gravity_est_en", gravity_est_en, true);
  ros_params::get(this, "imu.ba_bg_est_en", ba_bg_est_en, true);

  ros_params::get(this, "preprocess.blind", p_pre->blind, 0.01);
  ros_params::get(this, "preprocess.filter_size_surf", filter_size_surf_min, 0.5);
  ros_params::get(this, "preprocess.hilti_en", hilti_en, false);
  ros_params::get(this, "preprocess.lidar_type", p_pre->lidar_type, AVIA);
  ros_params::get(this, "preprocess.scan_line", p_pre->N_SCANS, 6);
  ros_params::get(this, "preprocess.point_filter_num", p_pre->point_filter_num, 3);
  ros_params::get(this, "preprocess.feature_extract_enabled", p_pre->feature_enabled, false);

  ros_params::get(this, "pcd_save.interval", pcd_save_interval, -1);
  ros_params::get(this, "pcd_save.pcd_save_en", pcd_save_en, false);
  ros_params::get(this, "pcd_save.type", pcd_save_type, 0);
  ros_params::get(this, "image_save.img_save_en", img_save_en, false);
  ros_params::get(this, "image_save.interval", img_save_interval, 1);

  ros_params::get(this, "pcd_save.colmap_output_en", colmap_output_en, false);
  ros_params::get(this, "pcd_save.filter_size_pcd", filter_size_pcd, 0.5);
  ros_params::get(this, "extrin_calib.extrinsic_T", extrinT, vector<double>(3, 0.0));
  ros_params::get(this, "extrin_calib.extrinsic_R", extrinR, vector<double>{1, 0, 0, 0, 1, 0, 0, 0, 1});
  ros_params::get(this, "extrin_calib.Pcl", cameraextrinT, vector<double>(3, 0.0));
  ros_params::get(this, "extrin_calib.Rcl", cameraextrinR, vector<double>{1, 0, 0, 0, 1, 0, 0, 0, 1});
  ros_params::get(this, "debug.plot_time", plot_time, -10.0);
  ros_params::get(this, "debug.frame_cnt", frame_cnt, 6);

  ros_params::get(this, "publish.blind_rgb_points", blind_rgb_points, 0.01);
  ros_params::get(this, "publish.crop_bottom_px", crop_bottom_px, 0);
  ros_params::get(this, "publish.pub_scan_num", pub_scan_num, 1);
  ros_params::get(this, "publish.pub_effect_point_en", pub_effect_point_en, false);
  ros_params::get(this, "publish.dense_map_en", dense_map_en, false);
  ros_params::get(this, "publish.pub_body_frame_en", pub_body_frame_en, false);

  /* OctoMap based dynamic object removal: the launch file spawns an octomap_server
     node fed with /cloud_registered_body; its ray casting clears the voxels that a
     moving object leaves behind, and the resulting map comes back on
     /octomap_point_cloud_centers. */
  ros_params::get(this, "octomap.enable", octomap_en, false);
  ros_params::get(this, "octomap.map_pub_period", map_pub_period, 1.0);
  ros_params::get(this, "octomap.map_pub_voxel_size", map_pub_voxel_size, 0.0);
  ros_params::get(this, "octomap.temp_map_file_path", temp_map_file_path, std::string(""));
  if (temp_map_file_path.empty()) temp_map_file_path = std::string(ROOT_DIR) + "Log/pcd/octomap_map.pcd";
  if (octomap_en) pub_body_frame_en = true;

  p_pre->blind_sqr = p_pre->blind * p_pre->blind;
}

void LIVMapper::initializeComponents()
{
  downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
  extT << VEC_FROM_ARRAY(extrinT);
  extR << MAT_FROM_ARRAY(extrinR);

  voxelmap_manager->extT_ << VEC_FROM_ARRAY(extrinT);
  voxelmap_manager->extR_ << MAT_FROM_ARRAY(extrinR);

  if (!vk::camera_loader::loadFromRosParams(this, vio_manager->cam)) throw std::runtime_error("Camera model not correctly specified.");

  vio_manager->grid_size = grid_size;
  vio_manager->patch_size = patch_size;
  vio_manager->outlier_threshold = outlier_threshold;
  vio_manager->setImuToLidarExtrinsic(extT, extR);
  vio_manager->setLidarToCameraExtrinsic(cameraextrinR, cameraextrinT);
  vio_manager->state = &_state;
  vio_manager->state_propagat = &state_propagat;
  vio_manager->max_iterations = max_iterations;
  vio_manager->img_point_cov = IMG_POINT_COV;
  vio_manager->normal_en = normal_en;
  vio_manager->inverse_composition_en = inverse_composition_en;
  vio_manager->raycast_en = raycast_en;
  vio_manager->grid_n_width = grid_n_width;
  vio_manager->grid_n_height = grid_n_height;
  vio_manager->patch_pyrimid_level = patch_pyrimid_level;
  vio_manager->exposure_estimate_en = exposure_estimate_en;
  vio_manager->colmap_output_en = colmap_output_en;
  vio_manager->initializeVIO();

  p_imu->set_extrinsic(extT, extR);
  p_imu->set_gyr_cov_scale(V3D(gyr_cov, gyr_cov, gyr_cov));
  p_imu->set_acc_cov_scale(V3D(acc_cov, acc_cov, acc_cov));
  p_imu->set_inv_expo_cov(inv_expo_cov);
  p_imu->set_gyr_bias_cov(V3D(0.0001, 0.0001, 0.0001));
  p_imu->set_acc_bias_cov(V3D(0.0001, 0.0001, 0.0001));
  p_imu->set_imu_init_frame_num(imu_int_frame);

  if (!imu_en) p_imu->disable_imu();
  if (!gravity_est_en) p_imu->disable_gravity_est();
  if (!ba_bg_est_en) p_imu->disable_bias_est();
  if (!exposure_estimate_en) p_imu->disable_exposure_est();

  slam_mode_ = (img_en && lidar_en) ? LIVO : imu_en ? ONLY_LIO : ONLY_LO;
}

void LIVMapper::initializeFiles()
{
  if (pcd_save_en && colmap_output_en)
  {
      const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh";

      std::string chmodCommand = "chmod +x " + folderPath;

      int chmodRet = system(chmodCommand.c_str());
      if (chmodRet != 0) {
          std::cerr << "Failed to set execute permissions for the script." << std::endl;
          return;
      }

      int executionRet = system(folderPath.c_str());
      if (executionRet != 0) {
          std::cerr << "Failed to execute the script." << std::endl;
          return;
      }
  }
  if(colmap_output_en) fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out);
  if(pcd_save_en) fout_lidar_pos.open(std::string(ROOT_DIR) + "Log/pcd/lidar_poses.txt", std::ios::out);
  if(img_save_en) fout_visual_pos.open(std::string(ROOT_DIR) + "Log/image/image_poses.txt", std::ios::out);
  fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out);
  fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out);
}

void LIVMapper::initializeSubscribersAndPublishers()
{
  // Livox CustomMsg / IMU come from drivers publishing with the default (reliable) QoS,
  // while PointCloud2 LiDARs and cameras typically use the sensor-data QoS.
  const rclcpp::QoS reliable_qos = rclcpp::QoS(rclcpp::KeepLast(2000));
  const rclcpp::QoS sensor_qos = rclcpp::SensorDataQoS();

  if (p_pre->lidar_type == AVIA)
  {
#ifdef FAST_LIVO_LIVOX_ENABLED
    sub_pcl_livox = this->create_subscription<livox_ros_driver2::msg::CustomMsg>(
        lid_topic, reliable_qos, std::bind(&LIVMapper::livox_pcl_cbk, this, std::placeholders::_1));
#else
    RCLCPP_FATAL(this->get_logger(), "preprocess.lidar_type is AVIA but this package was built without livox_ros_driver2.");
    throw std::runtime_error("livox_ros_driver2 is not available");
#endif
  }
  else
  {
    sub_pcl = this->create_subscription<sensor_msgs::msg::PointCloud2>(lid_topic, sensor_qos,
                                                                      std::bind(&LIVMapper::standard_pcl_cbk, this, std::placeholders::_1));
  }
  sub_imu = this->create_subscription<sensor_msgs::msg::Imu>(imu_topic, reliable_qos, std::bind(&LIVMapper::imu_cbk, this, std::placeholders::_1));
  sub_img = this->create_subscription<sensor_msgs::msg::Image>(img_topic, sensor_qos, std::bind(&LIVMapper::img_cbk, this, std::placeholders::_1));

  pubLaserCloudFullRes = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered", 100);
  pubLaserCloudFullResBody = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered_body", 100);
  pubNormal = this->create_publisher<visualization_msgs::msg::MarkerArray>("visualization_marker", 100);
  pubSubVisualMap = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_visual_sub_map_before", 100);
  pubLaserCloudEffect = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_effected", 100);
  pubLaserCloudMap = this->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 100);
  pubOdomAftMapped = this->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init", 10);
  pubPath = this->create_publisher<nav_msgs::msg::Path>("/path", 10);
  plane_pub = this->create_publisher<visualization_msgs::msg::Marker>("/planner_normal", 1);
  voxel_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("/voxels", 1);
  pubLaserCloudDyn = this->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj", 100);
  pubLaserCloudDynRmed = this->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj_removed", 100);
  pubLaserCloudDynDbg = this->create_publisher<sensor_msgs::msg::PointCloud2>("/dyn_obj_dbg_hist", 100);
  mavros_pose_publisher = this->create_publisher<geometry_msgs::msg::PoseStamped>("/mavros/vision_pose/pose", 10);
  pubImage = image_transport::create_publisher(this, "/rgb_img");
  pubUndistortedImage = image_transport::create_publisher(this, "/undistorted_camera");
  pubImuPropOdom = this->create_publisher<nav_msgs::msg::Odometry>("/LIVO2/imu_propagate", 10000);
  imu_prop_timer = this->create_wall_timer(std::chrono::milliseconds(4), std::bind(&LIVMapper::imu_prop_callback, this));
  voxelmap_manager->voxel_map_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/planes", 10000);

  tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

  map_save_srv = this->create_service<std_srvs::srv::Trigger>(
      "/map_save", std::bind(&LIVMapper::map_save_cbk, this, std::placeholders::_1, std::placeholders::_2));

  if (octomap_en)
  {
    RCLCPP_INFO(this->get_logger(),
                "[OCTOMAP] Dynamic object removal enabled: feeding /cloud_registered_body to octomap_server and "
                "publishing the cleared map on /Laser_map.");
    sub_octomap_cloud = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        "/octomap_point_cloud_centers", 1, std::bind(&LIVMapper::octomap_cloud_cbk, this, std::placeholders::_1));
    map_pub_timer = this->create_wall_timer(std::chrono::duration<double>(map_pub_period), std::bind(&LIVMapper::publish_octomap_map, this));
  }
}

void LIVMapper::octomap_cloud_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
{
  PointCloudXYZI::Ptr cloud(new PointCloudXYZI());
  pcl::fromROSMsg(*msg, *cloud);
  std::lock_guard<std::mutex> lock(octomap_cloud_mutex);
  octomap_latest_cloud = cloud;
}

void LIVMapper::publish_octomap_map()
{
  PointCloudXYZI::Ptr cloud(new PointCloudXYZI());
  {
    std::lock_guard<std::mutex> lock(octomap_cloud_mutex);
    if (octomap_latest_cloud->empty()) return;
    *cloud = *octomap_latest_cloud;
  }

  if (map_pub_voxel_size > 0.0)
  {
    pcl::VoxelGrid<PointType> filter;
    filter.setLeafSize(map_pub_voxel_size, map_pub_voxel_size, map_pub_voxel_size);
    filter.setInputCloud(cloud);
    PointCloudXYZI::Ptr filtered(new PointCloudXYZI());
    filter.filter(*filtered);
    cloud = filtered;
  }

  sensor_msgs::msg::PointCloud2 map_msg;
  pcl::toROSMsg(*cloud, map_msg);
  map_msg.header.stamp = currentStamp();
  map_msg.header.frame_id = "camera_init";
  pubLaserCloudMap->publish(map_msg);
}

void LIVMapper::map_save_cbk(const std_srvs::srv::Trigger::Request::SharedPtr /*req*/, std_srvs::srv::Trigger::Response::SharedPtr res)
{
  if (octomap_en)
  {
    std::lock_guard<std::mutex> lock(octomap_cloud_mutex);
    if (octomap_latest_cloud->empty())
    {
      res->success = false;
      res->message = "OctoMap cloud not yet received. Is octomap_server running?";
      RCLCPP_WARN(this->get_logger(), "%s", res->message.c_str());
      return;
    }
    std::filesystem::path out_path(temp_map_file_path);
    if (out_path.has_parent_path()) std::filesystem::create_directories(out_path.parent_path());
    pcl::PCDWriter pcd_writer;
    pcd_writer.writeBinary(temp_map_file_path, *octomap_latest_cloud);
    RCLCPP_INFO(this->get_logger(), "OctoMap (dynamic objects removed) saved to %s (%zu pts)", temp_map_file_path.c_str(),
                octomap_latest_cloud->size());
    res->success = true;
    res->message = "OctoMap saved to " + temp_map_file_path;
    return;
  }

  if (!pcd_save_en)
  {
    res->success = false;
    res->message = "Map save disabled (set pcd_save.pcd_save_en or octomap.enable).";
    return;
  }
  savePCD();
  res->success = true;
  res->message = "Map saved to " + std::string(ROOT_DIR) + "Log/pcd/";
}

void LIVMapper::handleFirstFrame()
{
  if (!is_first_frame)
  {
    _first_lidar_time = LidarMeasures.last_lio_update_time;
    p_imu->first_lidar_time = _first_lidar_time; // Only for IMU data log
    is_first_frame = true;
    cout << "FIRST LIDAR FRAME!" << endl;
  }
}

void LIVMapper::gravityAlignment()
{
  if (!p_imu->imu_need_init && !gravity_align_finished)
  {
    std::cout << "Gravity Alignment Starts" << std::endl;
    V3D ez(0, 0, -1), gz(_state.gravity);
    Quaterniond G_q_I0 = Quaterniond::FromTwoVectors(gz, ez);
    M3D G_R_I0 = G_q_I0.toRotationMatrix();

    _state.pos_end = G_R_I0 * _state.pos_end;
    _state.rot_end = G_R_I0 * _state.rot_end;
    _state.vel_end = G_R_I0 * _state.vel_end;
    _state.gravity = G_R_I0 * _state.gravity;
    gravity_align_finished = true;
    std::cout << "Gravity Alignment Finished" << std::endl;
  }
}

void LIVMapper::processImu()
{
  // double t0 = omp_get_wtime();

  p_imu->Process2(LidarMeasures, _state, feats_undistort);

  if (gravity_align_en) gravityAlignment();

  state_propagat = _state;
  voxelmap_manager->state_ = _state;
  voxelmap_manager->feats_undistort_ = feats_undistort;

  // double t_prop = omp_get_wtime();

  // std::cout << "[ Mapping ] feats_undistort: " << feats_undistort->size() << std::endl;
  // std::cout << "[ Mapping ] predict cov: " << _state.cov.diagonal().transpose() << std::endl;
  // std::cout << "[ Mapping ] predict sta: " << state_propagat.pos_end.transpose() << state_propagat.vel_end.transpose() << std::endl;
}

void LIVMapper::stateEstimationAndMapping()
{
  switch (LidarMeasures.lio_vio_flg)
  {
    case VIO:
      handleVIO();
      break;
    case LIO:
    case LO:
      handleLIO();
      break;
  }
}

void LIVMapper::handleVIO()
{
  euler_cur = RotMtoEuler(_state.rot_end);
  fout_pre << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " "
            << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " "
            << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << std::endl;

  if (pcl_w_wait_pub->empty() || (pcl_w_wait_pub == nullptr))
  {
    std::cout << "[ VIO ] No point!!!" << std::endl;
    return;
  }

  std::cout << "[ VIO ] Raw feature num: " << pcl_w_wait_pub->points.size() << std::endl;

  if (fabs((LidarMeasures.last_lio_update_time - _first_lidar_time) - plot_time) < (frame_cnt / 2 * 0.1))
  {
    vio_manager->plot_flag = true;
  }
  else
  {
    vio_manager->plot_flag = false;
  }

  vio_manager->processFrame(LidarMeasures.measures.back().img, _pv_list, voxelmap_manager->voxel_map_, LidarMeasures.last_lio_update_time - _first_lidar_time);

  if (imu_prop_enable)
  {
    ekf_finish_once = true;
    latest_ekf_state = _state;
    latest_ekf_time = LidarMeasures.last_lio_update_time;
    state_update_flg = true;
  }

  // int size_sub_map = vio_manager->visual_sub_map_cur.size();
  // visual_sub_map->reserve(size_sub_map);
  // for (int i = 0; i < size_sub_map; i++)
  // {
  //   PointType temp_map;
  //   temp_map.x = vio_manager->visual_sub_map_cur[i]->pos_[0];
  //   temp_map.y = vio_manager->visual_sub_map_cur[i]->pos_[1];
  //   temp_map.z = vio_manager->visual_sub_map_cur[i]->pos_[2];
  //   temp_map.intensity = 0.;
  //   visual_sub_map->push_back(temp_map);
  // }

  publish_frame_world(pubLaserCloudFullRes, vio_manager);
  publish_img_rgb(pubImage, vio_manager);
  publish_undistorted_img(pubUndistortedImage, vio_manager);

  euler_cur = RotMtoEuler(_state.rot_end);
  fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " "
            << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " "
            << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl;
}

void LIVMapper::handleLIO()
{
  euler_cur = RotMtoEuler(_state.rot_end);
  fout_pre << setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " "
           << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " "
           << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << endl;

  if (feats_undistort->empty() || (feats_undistort == nullptr))
  {
    std::cout << "[ LIO ]: No point!!!" << std::endl;
    return;
  }

  double t0 = omp_get_wtime();

  downSizeFilterSurf.setInputCloud(feats_undistort);
  downSizeFilterSurf.filter(*feats_down_body);

  double t_down = omp_get_wtime();

  feats_down_size = feats_down_body->points.size();
  voxelmap_manager->feats_down_body_ = feats_down_body;
  transformLidar(_state.rot_end, _state.pos_end, feats_down_body, feats_down_world);
  voxelmap_manager->feats_down_world_ = feats_down_world;
  voxelmap_manager->feats_down_size_ = feats_down_size;

  if (!lidar_map_inited)
  {
    lidar_map_inited = true;
    voxelmap_manager->BuildVoxelMap();
  }

  double t1 = omp_get_wtime();

  voxelmap_manager->StateEstimation(state_propagat);
  _state = voxelmap_manager->state_;
  _pv_list = voxelmap_manager->pv_list_;

  double t2 = omp_get_wtime();

  if (imu_prop_enable)
  {
    ekf_finish_once = true;
    latest_ekf_state = _state;
    latest_ekf_time = LidarMeasures.last_lio_update_time;
    state_update_flg = true;
  }

  if (pose_output_en)
  {
    static bool pos_opend = false;
    static int ocount = 0;
    std::ofstream outFile, evoFile;
    if (!pos_opend)
    {
      evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::out);
      pos_opend = true;
      if (!evoFile.is_open()) RCLCPP_ERROR(rclcpp::get_logger("fast_livo"), "open fail\n");
    }
    else
    {
      evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app);
      if (!evoFile.is_open()) RCLCPP_ERROR(rclcpp::get_logger("fast_livo"), "open fail\n");
    }
    Eigen::Matrix4d outT;
    Eigen::Quaterniond q(_state.rot_end);
    evoFile << std::fixed;
    evoFile << LidarMeasures.last_lio_update_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " "
            << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
  }

  euler_cur = RotMtoEuler(_state.rot_end);
  geoQuat = createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2));
  publish_odometry(pubOdomAftMapped);
  if (pub_body_frame_en) publish_frame_body(pubLaserCloudFullResBody);

  double t3 = omp_get_wtime();

  PointCloudXYZI::Ptr world_lidar(new PointCloudXYZI());
  transformLidar(_state.rot_end, _state.pos_end, feats_down_body, world_lidar);
  for (size_t i = 0; i < world_lidar->points.size(); i++)
  {
    voxelmap_manager->pv_list_[i].point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z;
    M3D point_crossmat = voxelmap_manager->cross_mat_list_[i];
    M3D var = voxelmap_manager->body_cov_list_[i];
    var = (_state.rot_end * extR) * var * (_state.rot_end * extR).transpose() +
          (-point_crossmat) * _state.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + _state.cov.block<3, 3>(3, 3);
    voxelmap_manager->pv_list_[i].var = var;
  }
  voxelmap_manager->UpdateVoxelMap(voxelmap_manager->pv_list_);
  std::cout << "[ LIO ] Update Voxel Map" << std::endl;
  _pv_list = voxelmap_manager->pv_list_;

  double t4 = omp_get_wtime();

  if(voxelmap_manager->config_setting_.map_sliding_en)
  {
    voxelmap_manager->mapSliding();
  }

  PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body);
  int size = laserCloudFullRes->points.size();
  PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1));

  for (int i = 0; i < size; i++)
  {
    RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]);
  }
  *pcl_w_wait_pub = *laserCloudWorld;

  publish_frame_world(pubLaserCloudFullRes, vio_manager);
  if (pub_effect_point_en) publish_effect_world(pubLaserCloudEffect, voxelmap_manager->ptpl_list_);
  if (voxelmap_manager->config_setting_.is_pub_plane_map_) voxelmap_manager->pubVoxelMap();
  publish_path(pubPath);
  publish_mavros(mavros_pose_publisher);

  frame_num++;
  aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t4 - t0) / frame_num;

  // aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t2 - t1) / frame_num;
  // aver_time_map_inre = aver_time_map_inre * (frame_num - 1) / frame_num + (t4 - t3) / frame_num;
  // aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time) / frame_num;
  // aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_const_H_time / frame_num;
  // printf("[ mapping time ]: per scan: propagation %0.6f downsample: %0.6f match: %0.6f solve: %0.6f  ICP: %0.6f  map incre: %0.6f total: %0.6f \n"
  //         "[ mapping time ]: average: icp: %0.6f construct H: %0.6f, total: %0.6f \n",
  //         t_prop - t0, t1 - t_prop, match_time, solve_time, t3 - t1, t5 - t3, t5 - t0, aver_time_icp, aver_time_const_H_time, aver_time_consu);

  // printf("\033[1;36m[ LIO mapping time ]: current scan: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n"
  //         "\033[1;36m[ LIO mapping time ]: average: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n",
  //         t2 - t1, t4 - t3, t4 - t0, aver_time_icp, aver_time_map_inre, aver_time_consu);
  printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
  printf("\033[1;34m|                         LIO Mapping Time                    |\033[0m\n");
  printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
  printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)");
  printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
  printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "DownSample", t_down - t0);
  printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "ICP", t2 - t1);
  printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "updateVoxelMap", t4 - t3);
  printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
  printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Current Total Time", t4 - t0);
  printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Average Total Time", aver_time_consu);
  printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");

  euler_cur = RotMtoEuler(_state.rot_end);
  fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " "
            << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " "
            << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl;
}

void LIVMapper::savePCD()
{
  if (octomap_en)
  {
    /* In OctoMap mode the map that gets saved is the one octomap_server has ray-cast
       cleared, i.e. without the points left behind by dynamic objects. */
    std::lock_guard<std::mutex> lock(octomap_cloud_mutex);
    if (octomap_latest_cloud->empty())
    {
      RCLCPP_WARN(this->get_logger(), "OctoMap mode: no cloud received on /octomap_point_cloud_centers, nothing saved.");
      return;
    }
    std::filesystem::path out_path(temp_map_file_path);
    if (out_path.has_parent_path()) std::filesystem::create_directories(out_path.parent_path());
    pcl::PCDWriter pcd_writer;
    pcd_writer.writeBinary(temp_map_file_path, *octomap_latest_cloud);
    std::cout << GREEN << "OctoMap (dynamic objects removed) saved to: " << temp_map_file_path << " with point count: "
              << octomap_latest_cloud->size() << RESET << std::endl;
    return;
  }

  std::lock_guard<std::mutex> lock(pcl_wait_save_mutex);

  if (pcd_save_en && (pcl_wait_save->points.size() > 0 || pcl_wait_save_intensity->points.size() > 0) && pcd_save_interval < 0)
  {
    std::string raw_points_dir = std::string(ROOT_DIR) + "Log/pcd/all_raw_points.pcd";
    std::string downsampled_points_dir = std::string(ROOT_DIR) + "Log/pcd/all_downsampled_points.pcd";
    pcl::PCDWriter pcd_writer;

    if (img_en)
    {
      pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
      pcl::VoxelGrid<pcl::PointXYZRGB> voxel_filter;
      voxel_filter.setInputCloud(pcl_wait_save);
      voxel_filter.setLeafSize(filter_size_pcd, filter_size_pcd, filter_size_pcd);
      voxel_filter.filter(*downsampled_cloud);

      pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save); // Save the raw point cloud data
      std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir
                << " with point count: " << pcl_wait_save->points.size() << RESET << std::endl;

      pcd_writer.writeBinary(downsampled_points_dir, *downsampled_cloud); // Save the downsampled point cloud data
      std::cout << GREEN << "Downsampled point cloud data saved to: " << downsampled_points_dir
                << " with point count after filtering: " << downsampled_cloud->points.size() << RESET << std::endl;

      if(colmap_output_en)
      {
        fout_points << "# 3D point list with one line of data per point\n";
        fout_points << "#  POINT_ID, X, Y, Z, R, G, B, ERROR\n";
        for (size_t i = 0; i < downsampled_cloud->size(); ++i)
        {
            const auto& point = downsampled_cloud->points[i];
            fout_points << i << " "
                        << std::fixed << std::setprecision(6)
                        << point.x << " " << point.y << " " << point.z << " "
                        << static_cast<int>(point.r) << " "
                        << static_cast<int>(point.g) << " "
                        << static_cast<int>(point.b) << " "
                        << 0 << std::endl;
        }
      }
    }
    else
    {
      pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save_intensity);
      std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir
                << " with point count: " << pcl_wait_save_intensity->points.size() << RESET << std::endl;
    }
  }
}

void LIVMapper::run()
{
  rclcpp::Rate rate(5000);
  while (rclcpp::ok())
  {
    if (!sync_packages(LidarMeasures))
    {
      rate.sleep();
      continue;
    }
    handleFirstFrame();

    processImu();

    // if (!p_imu->imu_time_init) continue;

    stateEstimationAndMapping();
  }
  savePCD();
}

void LIVMapper::prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr)
{
  double mean_acc_norm = p_imu->IMU_mean_acc_norm;
  acc_avr = acc_avr * G_m_s2 / mean_acc_norm - imu_prop_state.bias_a;
  angvel_avr -= imu_prop_state.bias_g;

  M3D Exp_f = Exp(angvel_avr, dt);
  /* propogation of IMU attitude */
  imu_prop_state.rot_end = imu_prop_state.rot_end * Exp_f;

  /* Specific acceleration (global frame) of IMU */
  V3D acc_imu = imu_prop_state.rot_end * acc_avr + V3D(imu_prop_state.gravity[0], imu_prop_state.gravity[1], imu_prop_state.gravity[2]);

  /* propogation of IMU */
  imu_prop_state.pos_end = imu_prop_state.pos_end + imu_prop_state.vel_end * dt + 0.5 * acc_imu * dt * dt;

  /* velocity of IMU */
  imu_prop_state.vel_end = imu_prop_state.vel_end + acc_imu * dt;
}

void LIVMapper::imu_prop_callback()
{
  if (p_imu->imu_need_init || !new_imu || !ekf_finish_once) { return; }
  mtx_buffer_imu_prop.lock();
  new_imu = false; // 控制propagate频率和IMU频率一致
  if (imu_prop_enable && !prop_imu_buffer.empty())
  {
    static double last_t_from_lidar_end_time = 0;
    if (state_update_flg)
    {
      imu_propagate = latest_ekf_state;
      // drop all useless imu pkg
      while ((!prop_imu_buffer.empty() && get_time_sec(prop_imu_buffer.front().header.stamp) < latest_ekf_time))
      {
        prop_imu_buffer.pop_front();
      }
      last_t_from_lidar_end_time = 0;
      for (int i = 0; i < prop_imu_buffer.size(); i++)
      {
        double t_from_lidar_end_time = get_time_sec(prop_imu_buffer[i].header.stamp) - latest_ekf_time;
        double dt = t_from_lidar_end_time - last_t_from_lidar_end_time;
        // cout << "prop dt" << dt << ", " << t_from_lidar_end_time << ", " << last_t_from_lidar_end_time << endl;
        V3D acc_imu(prop_imu_buffer[i].linear_acceleration.x, prop_imu_buffer[i].linear_acceleration.y, prop_imu_buffer[i].linear_acceleration.z);
        V3D omg_imu(prop_imu_buffer[i].angular_velocity.x, prop_imu_buffer[i].angular_velocity.y, prop_imu_buffer[i].angular_velocity.z);
        prop_imu_once(imu_propagate, dt, acc_imu, omg_imu);
        last_t_from_lidar_end_time = t_from_lidar_end_time;
      }
      state_update_flg = false;
    }
    else
    {
      V3D acc_imu(newest_imu.linear_acceleration.x, newest_imu.linear_acceleration.y, newest_imu.linear_acceleration.z);
      V3D omg_imu(newest_imu.angular_velocity.x, newest_imu.angular_velocity.y, newest_imu.angular_velocity.z);
      double t_from_lidar_end_time = get_time_sec(newest_imu.header.stamp) - latest_ekf_time;
      double dt = t_from_lidar_end_time - last_t_from_lidar_end_time;
      prop_imu_once(imu_propagate, dt, acc_imu, omg_imu);
      last_t_from_lidar_end_time = t_from_lidar_end_time;
    }

    V3D posi, vel_i;
    Eigen::Quaterniond q;
    posi = imu_propagate.pos_end;
    vel_i = imu_propagate.vel_end;
    q = Eigen::Quaterniond(imu_propagate.rot_end);
    imu_prop_odom.header.frame_id = "world";
    imu_prop_odom.header.stamp = newest_imu.header.stamp;
    imu_prop_odom.pose.pose.position.x = posi.x();
    imu_prop_odom.pose.pose.position.y = posi.y();
    imu_prop_odom.pose.pose.position.z = posi.z();
    imu_prop_odom.pose.pose.orientation.w = q.w();
    imu_prop_odom.pose.pose.orientation.x = q.x();
    imu_prop_odom.pose.pose.orientation.y = q.y();
    imu_prop_odom.pose.pose.orientation.z = q.z();
    imu_prop_odom.twist.twist.linear.x = vel_i.x();
    imu_prop_odom.twist.twist.linear.y = vel_i.y();
    imu_prop_odom.twist.twist.linear.z = vel_i.z();
    pubImuPropOdom->publish(imu_prop_odom);
  }
  mtx_buffer_imu_prop.unlock();
}

void LIVMapper::transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud)
{
  PointCloudXYZI().swap(*trans_cloud);
  trans_cloud->reserve(input_cloud->size());
  for (size_t i = 0; i < input_cloud->size(); i++)
  {
    pcl::PointXYZINormal p_c = input_cloud->points[i];
    Eigen::Vector3d p(p_c.x, p_c.y, p_c.z);
    p = (rot * (extR * p + extT) + t);
    PointType pi;
    pi.x = p(0);
    pi.y = p(1);
    pi.z = p(2);
    pi.intensity = p_c.intensity;
    trans_cloud->points.push_back(pi);
  }
}

void LIVMapper::pointBodyToWorld(const PointType &pi, PointType &po)
{
  V3D p_body(pi.x, pi.y, pi.z);
  V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end);
  po.x = p_global(0);
  po.y = p_global(1);
  po.z = p_global(2);
  po.intensity = pi.intensity;
}

template <typename T> void LIVMapper::pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po)
{
  V3D p_body(pi[0], pi[1], pi[2]);
  V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end);
  po[0] = p_global(0);
  po[1] = p_global(1);
  po[2] = p_global(2);
}

template <typename T> Matrix<T, 3, 1> LIVMapper::pointBodyToWorld(const Matrix<T, 3, 1> &pi)
{
  V3D p(pi[0], pi[1], pi[2]);
  p = (_state.rot_end * (extR * p + extT) + _state.pos_end);
  Matrix<T, 3, 1> po(p[0], p[1], p[2]);
  return po;
}

void LIVMapper::RGBpointBodyToWorld(PointType const *const pi, PointType *const po)
{
  V3D p_body(pi->x, pi->y, pi->z);
  V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end);
  po->x = p_global(0);
  po->y = p_global(1);
  po->z = p_global(2);
  po->intensity = pi->intensity;
}

void LIVMapper::RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)
{
  V3D p_body_lidar(pi->x, pi->y, pi->z);
  V3D p_body_imu(extR * p_body_lidar + extT);

  po->x = p_body_imu(0);
  po->y = p_body_imu(1);
  po->z = p_body_imu(2);
  po->intensity = pi->intensity;
}

void LIVMapper::standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
{
  if (!lidar_en) return;
  mtx_buffer.lock();

  double cur_head_time = get_time_sec(msg->header.stamp) + lidar_time_offset;
  // cout<<"got feature"<<endl;
  if (cur_head_time < last_timestamp_lidar)
  {
    RCLCPP_ERROR(rclcpp::get_logger("fast_livo"), "lidar loop back, clear buffer");
    lid_raw_data_buffer.clear();
  }
  // RCLCPP_INFO(rclcpp::get_logger("fast_livo"), "get point cloud at time: %.6f", get_time_sec(msg->header.stamp));
  PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
  p_pre->process(msg, ptr);
  lid_raw_data_buffer.push_back(ptr);
  lid_header_time_buffer.push_back(cur_head_time);
  last_timestamp_lidar = cur_head_time;

  mtx_buffer.unlock();
  sig_buffer.notify_all();
}

#ifdef FAST_LIVO_LIVOX_ENABLED
void LIVMapper::livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr msg_in)
{
  if (!lidar_en) return;
  mtx_buffer.lock();
  livox_ros_driver2::msg::CustomMsg::ConstSharedPtr msg = msg_in;
  // if ((abs(get_time_sec(msg->header.stamp) - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag)
  // {
  //   RCLCPP_WARN(rclcpp::get_logger("fast_livo"), "lidar jumps %.3f\n", get_time_sec(msg->header.stamp) - last_timestamp_lidar);
  //   sync_jump_flag = true;
  //   msg->header.stamp = ros::Time().fromSec(last_timestamp_lidar + 0.1);
  // }
  if (abs(last_timestamp_imu - get_time_sec(msg->header.stamp)) > 1.0 && !imu_buffer.empty())
  {
    double timediff_imu_wrt_lidar = last_timestamp_imu - get_time_sec(msg->header.stamp);
    printf("\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100);
    // imu_time_offset = timediff_imu_wrt_lidar;
  }

  double cur_head_time = get_time_sec(msg->header.stamp);
  RCLCPP_INFO(rclcpp::get_logger("fast_livo"), "Get LiDAR, its header time: %.6f", cur_head_time);
  if (cur_head_time < last_timestamp_lidar)
  {
    RCLCPP_ERROR(rclcpp::get_logger("fast_livo"), "lidar loop back, clear buffer");
    lid_raw_data_buffer.clear();
  }
  // RCLCPP_INFO(rclcpp::get_logger("fast_livo"), "get point cloud at time: %.6f", get_time_sec(msg->header.stamp));
  PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
  p_pre->process(msg, ptr);

  if (!ptr || ptr->empty()) {
    RCLCPP_ERROR(rclcpp::get_logger("fast_livo"), "Received an empty point cloud");
    mtx_buffer.unlock();
    return;
  }

  lid_raw_data_buffer.push_back(ptr);
  lid_header_time_buffer.push_back(cur_head_time);
  last_timestamp_lidar = cur_head_time;

  mtx_buffer.unlock();
  sig_buffer.notify_all();
}

#endif // FAST_LIVO_LIVOX_ENABLED

void LIVMapper::imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr msg_in)
{
  if (!imu_en) return;

  if (last_timestamp_lidar < 0.0) return;
  // RCLCPP_INFO(rclcpp::get_logger("fast_livo"), "get imu at time: %.6f", get_time_sec(msg_in->header.stamp));
  sensor_msgs::msg::Imu::SharedPtr msg = std::make_shared<sensor_msgs::msg::Imu>(*msg_in);
  msg->header.stamp = get_ros_time(get_time_sec(msg->header.stamp) - imu_time_offset);
  double timestamp = get_time_sec(msg->header.stamp);

  if (fabs(last_timestamp_lidar - timestamp) > 0.5 && (!ros_driver_fix_en))
  {
    RCLCPP_WARN(rclcpp::get_logger("fast_livo"), "IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp);
  }

  if (ros_driver_fix_en) timestamp += std::round(last_timestamp_lidar - timestamp);
  msg->header.stamp = get_ros_time(timestamp);

  mtx_buffer.lock();

  if (last_timestamp_imu > 0.0 && timestamp < last_timestamp_imu)
  {
    mtx_buffer.unlock();
    sig_buffer.notify_all();
    RCLCPP_ERROR(rclcpp::get_logger("fast_livo"), "imu loop back, offset: %lf \n", last_timestamp_imu - timestamp);
    return;
  }

  // if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2)
  // {

  //   RCLCPP_WARN(rclcpp::get_logger("fast_livo"), "imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu);
  //   mtx_buffer.unlock();
  //   sig_buffer.notify_all();
  //   return;
  // }

  last_timestamp_imu = timestamp;

  imu_buffer.push_back(msg);
  // cout<<"got imu: "<<timestamp<<" imu size "<<imu_buffer.size()<<endl;
  mtx_buffer.unlock();
  if (imu_prop_enable)
  {
    mtx_buffer_imu_prop.lock();
    if (imu_prop_enable && !p_imu->imu_need_init) { prop_imu_buffer.push_back(*msg); }
    newest_imu = *msg;
    new_imu = true;
    mtx_buffer_imu_prop.unlock();
  }
  sig_buffer.notify_all();
}

cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg)
{
  cv::Mat img;
  img = cv_bridge::toCvCopy(img_msg, "bgr8")->image;
  return img;
}

void LIVMapper::img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr msg_in)
{
  if (!img_en) return;
  sensor_msgs::msg::Image::ConstSharedPtr msg = msg_in;
  // if ((abs(get_time_sec(msg->header.stamp) - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag)
  // {
  //   RCLCPP_WARN(rclcpp::get_logger("fast_livo"), "img jumps %.3f\n", get_time_sec(msg->header.stamp) - last_timestamp_img);
  //   sync_jump_flag = true;
  //   msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1);
  // }

  // Hiliti2022 40Hz
  if (hilti_en)
  {
    static int frame_counter = 0;
    if (++frame_counter % 4 != 0) return;
  }
  // double msg_header_time =  get_time_sec(msg->header.stamp);