delete ...

faster-lio/app/map_node.h

@@ -19,7 +19,7 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/PointCloud2.h>
 #include "pointcloud_preprocess.h"
-#include "mapping.h"
+#include "laser_mapping.h"
 namespace faster_lio {
 
 

faster-lio/app/run_mapping_offline.cc

@@ -8,7 +8,7 @@
 #include <unistd.h>
 #include <csignal>
 
-#include "laser_mapping.h"
+#include "map_node.h"
 #include "utils.h"
 
 /// run faster-LIO in offline mode
@@ -24,7 +24,7 @@ void SigHandle(int sig) {
 }
 
 int main(int argc, char **argv) {
-    gflags::ParseCommandLineFlags(&argc, &argv, true);
+    /*gflags::ParseCommandLineFlags(&argc, &argv, true);
 
     FLAGS_stderrthreshold = google::INFO;
     FLAGS_colorlogtostderr = true;
@@ -33,7 +33,7 @@ int main(int argc, char **argv) {
     const std::string bag_file = FLAGS_bag_file;
     const std::string config_file = FLAGS_config_file;
 
-    auto laser_mapping = std::make_shared<faster_lio::LaserMapping>();
+    auto laser_mapping = std::make_shared<faster_lio::Map_node>();
     if (!laser_mapping->InitWithoutROS(FLAGS_config_file)) {
         LOG(ERROR) << "laser mapping init failed.";
         return -1;
@@ -93,6 +93,6 @@ int main(int argc, char **argv) {
 
     faster_lio::Timer::PrintAll();
     faster_lio::Timer::DumpIntoFile(FLAGS_time_log_file);
-
+*/
     return 0;
 }

faster-lio/include/laser_mapping.h

@@ -14,10 +14,105 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/PointCloud2.h>
 #include "pointcloud_preprocess.h"
-#include "mapping.h"
+#include "imu_processing.h"
+#include "ivox3d/ivox3d.h"
+#include "options.h"
+
+
 namespace faster_lio {
 
 
+class TTMapping
+{
+#ifdef IVOX_NODE_TYPE_PHC
+    using IVoxType = IVox<3, IVoxNodeType::PHC, PointType>;
+#else
+    using IVoxType = IVox<3, IVoxNodeType::DEFAULT, PointType>;
+#endif
+ public:
+    TTMapping();
+    ~TTMapping(){
+        scan_down_body_ = nullptr;
+        scan_undistort_ = nullptr;
+        scan_down_world_ = nullptr;
+    }
+    bool Init(int ivox_nearby_type,double gyr_cov, double acc_cov,
+              double b_gyr_cov, double b_acc_cov,double filter_size_surf_min);
+    void AddPointCloud(PointCloudType::Ptr cloud,double timebase);
+    void AddImu(const common::ImuData &msg_in,double timebase);
+    bool Run();
+    bool SyncPackages();
+    void MapIncremental();
+
+    void PointBodyToWorld(PointType const *pi, PointType *const po);
+    void PointBodyToWorld(const common::V3F &pi, PointType *const po);
+
+    /// interface of mtk, customized obseravtion model
+    void ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data);
+
+
+    /// modules
+    IVoxType::Options ivox_options_;
+    std::shared_ptr<IVoxType> ivox_ = nullptr;                    // localmap in ivox
+    std::shared_ptr<ImuProcess> p_imu_ = nullptr;                 // imu process
+
+    ///data bufer
+    std::mutex mtx_buffer_;
+    std::deque<double> time_buffer_;
+    std::deque<PointCloudType::Ptr> lidar_buffer_;
+    std::deque<common::ImuData> imu_buffer_;
+
+    ///////////////////////// EKF inputs and output ///////////////////////////////////////////////////////
+    common::MeasureGroup measures_;                    // sync IMU and lidar scan
+    esekfom::esekf<state_ikfom, 12, input_ikfom> kf_;  // esekf
+    state_ikfom state_point_;                          // ekf current state
+    vect3 pos_lidar_;                                  // lidar position after eskf update
+    common::V3D euler_cur_ = common::V3D::Zero();      // rotation in euler angles
+    bool extrinsic_est_en_ = true;
+
+    /// local map related
+    float det_range_ = 300.0f;
+    double cube_len_ = 0;
+    double filter_size_map_min_ = 0;
+
+    /// options
+    bool time_sync_en_ = false;
+    double timediff_lidar_wrt_imu_ = 0.0;
+    double last_timestamp_lidar_ = 0;
+    double lidar_end_time_ = 0;
+    double last_timestamp_imu_ = -1.0;
+    double first_lidar_time_ = 0.0;
+    bool lidar_pushed_ = false;
+
+    /// statistics and flags ///
+    int scan_count_ = 0;
+    int publish_count_ = 0;
+    bool flg_first_scan_ = true;
+    bool flg_EKF_inited_ = false;
+    int pcd_index_ = 0;
+    double lidar_mean_scantime_ = 0.0;
+    int scan_num_ = 0;
+    bool timediff_set_flg_ = false;
+    int effect_feat_num_ = 0, frame_num_ = 0;
+
+
+    /// params
+    std::vector<double> extrinT_{3, 0.0};  // lidar-imu translation
+    std::vector<double> extrinR_{9, 0.0};  // lidar-imu rotation
+
+    /// point clouds data
+    CloudPtr scan_undistort_{new PointCloudType()};   // scan after undistortion
+    CloudPtr scan_down_body_{new PointCloudType()};   // downsampled scan in body
+    CloudPtr scan_down_world_{new PointCloudType()};  // downsampled scan in world
+    std::vector<PointVector> nearest_points_;         // nearest points of current scan
+    common::VV4F corr_pts_;                           // inlier pts
+    common::VV4F corr_norm_;                          // inlier plane norms
+    pcl::VoxelGrid<PointType> voxel_scan_;            // voxel filter for current scan
+    std::vector<float> residuals_;                    // point-to-plane residuals
+    std::vector<bool> point_selected_surf_;           // selected points
+    common::VV4F plane_coef_;                         // plane coeffs
+};
+
 class LaserMapping {
    public:
 
@@ -25,13 +120,8 @@ class LaserMapping {
     ~LaserMapping() {
     }
 
-    /// init with ros
-    bool InitROS(ros::NodeHandle &nh);
 
-    /// init without ros
-    bool InitWithoutROS(const std::string &config_yaml);
 
-    void Run();
 
 
     // callbacks of lidar and imu
@@ -41,31 +131,11 @@ class LaserMapping {
 
 
 
-    ////////////////////////////// debug save / show ////////////////////////////////////////////////////////////////
-    void PublishPath(const ros::Publisher pub_path);
-    void PublishOdometry(const ros::Publisher &pub_odom_aft_mapped);
-    void PublishFrameWorld();
-    void PublishFrameBody(const ros::Publisher &pub_laser_cloud_body);
-    void PublishFrameEffectWorld(const ros::Publisher &pub_laser_cloud_effect_world);
-
-    void Savetrajectory(const std::string &traj_file);
-
-    void Finish();
 
    private:
-    template <typename T>
-    void SetPosestamp(T &out);
-
-
-    void PointBodyLidarToIMU(PointType const *const pi, PointType *const po);
-
-
     void SubAndPubToROS(ros::NodeHandle &nh);
 
-    bool LoadParams(ros::NodeHandle &nh);
-    bool LoadParamsFromYAML(const std::string &yaml);
 
-    void PrintState(const state_ikfom &s);
 
    private:
     std::shared_ptr<PointCloudPreprocess> preprocess_ = nullptr;  // point cloud preprocess
@@ -85,21 +155,11 @@ class LaserMapping {
 
 
     /////////////////////////  debug show / save /////////////////////////////////////////////////////////
-    bool run_in_offline_ = false;
-    bool path_pub_en_ = true;
-    bool scan_pub_en_ = false;
-    bool dense_pub_en_ = false;
-    bool scan_body_pub_en_ = false;
-    bool scan_effect_pub_en_ = false;
-    bool pcd_save_en_ = false;
-    bool runtime_pos_log_ = true;
-    int pcd_save_interval_ = -1;
-    bool path_save_en_ = false;
-    std::string dataset_;
-
-    PointCloudType::Ptr pcl_wait_save_{new PointCloudType()};  // debug save
+
+
+    PointCloudType::Ptr pcl_wait_save_{new PointCloudType()};  // debug save  铭感
     nav_msgs::Path path_;
-    geometry_msgs::PoseStamped msg_body_pose_;
+
 
 
     TTMapping  mapper_;

faster-lio/include/mapping.h

@@ -5,105 +5,11 @@
 #ifndef ROBOT_CONTROL_FASTER_LIO_INCLUDE_MAPPING_H_
 #define ROBOT_CONTROL_FASTER_LIO_INCLUDE_MAPPING_H_
 
-#include "imu_processing.h"
-#include "ivox3d/ivox3d.h"
-#include "options.h"
-#include <Eigen/Eigen>
-#include <pcl/filters/voxel_grid.h>
-#include <mutex>
-namespace faster_lio
-{
 
-class TTMapping
+namespace faster_lio
 {
-#ifdef IVOX_NODE_TYPE_PHC
-    using IVoxType = IVox<3, IVoxNodeType::PHC, PointType>;
-#else
-    using IVoxType = IVox<3, IVoxNodeType::DEFAULT, PointType>;
-#endif
- public:
-    TTMapping();
-    ~TTMapping(){
-        scan_down_body_ = nullptr;
-        scan_undistort_ = nullptr;
-        scan_down_world_ = nullptr;
-    }
-    bool Init(int ivox_nearby_type,double gyr_cov, double acc_cov,
-              double b_gyr_cov, double b_acc_cov,double filter_size_surf_min);
-    void AddPointCloud(PointCloudType::Ptr cloud,double timebase);
-    void AddImu(const common::ImuData &msg_in,double timebase);
-    bool Run();
-    bool SyncPackages();
-    void MapIncremental();
-
-    void PointBodyToWorld(PointType const *pi, PointType *const po);
-    void PointBodyToWorld(const common::V3F &pi, PointType *const po);
-
-    /// interface of mtk, customized obseravtion model
-    void ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data);
-
-
-    /// modules
-    IVoxType::Options ivox_options_;
-    std::shared_ptr<IVoxType> ivox_ = nullptr;                    // localmap in ivox
-    std::shared_ptr<ImuProcess> p_imu_ = nullptr;                 // imu process
-
-    ///data bufer
-    std::mutex mtx_buffer_;
-    std::deque<double> time_buffer_;
-    std::deque<PointCloudType::Ptr> lidar_buffer_;
-    std::deque<common::ImuData> imu_buffer_;
-
-    ///////////////////////// EKF inputs and output ///////////////////////////////////////////////////////
-    common::MeasureGroup measures_;                    // sync IMU and lidar scan
-    esekfom::esekf<state_ikfom, 12, input_ikfom> kf_;  // esekf
-    state_ikfom state_point_;                          // ekf current state
-    vect3 pos_lidar_;                                  // lidar position after eskf update
-    common::V3D euler_cur_ = common::V3D::Zero();      // rotation in euler angles
-    bool extrinsic_est_en_ = true;
-
-    /// local map related
-    float det_range_ = 300.0f;
-    double cube_len_ = 0;
-    double filter_size_map_min_ = 0;
-
-    /// options
-    bool time_sync_en_ = false;
-    double timediff_lidar_wrt_imu_ = 0.0;
-    double last_timestamp_lidar_ = 0;
-    double lidar_end_time_ = 0;
-    double last_timestamp_imu_ = -1.0;
-    double first_lidar_time_ = 0.0;
-    bool lidar_pushed_ = false;
-
-    /// statistics and flags ///
-    int scan_count_ = 0;
-    int publish_count_ = 0;
-    bool flg_first_scan_ = true;
-    bool flg_EKF_inited_ = false;
-    int pcd_index_ = 0;
-    double lidar_mean_scantime_ = 0.0;
-    int scan_num_ = 0;
-    bool timediff_set_flg_ = false;
-    int effect_feat_num_ = 0, frame_num_ = 0;
-
 
-    /// params
-    std::vector<double> extrinT_{3, 0.0};  // lidar-imu translation
-    std::vector<double> extrinR_{9, 0.0};  // lidar-imu rotation
 
-    /// point clouds data
-    CloudPtr scan_undistort_{new PointCloudType()};   // scan after undistortion
-    CloudPtr scan_down_body_{new PointCloudType()};   // downsampled scan in body
-    CloudPtr scan_down_world_{new PointCloudType()};  // downsampled scan in world
-    std::vector<PointVector> nearest_points_;         // nearest points of current scan
-    common::VV4F corr_pts_;                           // inlier pts
-    common::VV4F corr_norm_;                          // inlier plane norms
-    pcl::VoxelGrid<PointType> voxel_scan_;            // voxel filter for current scan
-    std::vector<float> residuals_;                    // point-to-plane residuals
-    std::vector<bool> point_selected_surf_;           // selected points
-    common::VV4F plane_coef_;                         // plane coeffs
-};
 
 }
 

faster-lio/src/laser_mapping.cc

@@ -8,199 +8,8 @@
 
 namespace faster_lio {
 
-bool LaserMapping::InitROS(ros::NodeHandle &nh) {
-    LoadParams(nh);
-    SubAndPubToROS(nh);
 
 
-    return true;
-}
-
-bool LaserMapping::InitWithoutROS(const std::string &config_yaml) {
-    /*LOG(INFO) << "init laser mapping from " << config_yaml;
-    if (!LoadParamsFromYAML(config_yaml)) {
-        return false;
-    }
-
-    // localmap init (after LoadParams)
-    ivox_ = std::make_shared<IVoxType>(ivox_options_);
-
-    // esekf init
-    std::vector<double> epsi(23, 0.001);
-    kf_.init_dyn_share(
-        get_f, df_dx, df_dw,
-        [this](state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data) { ObsModel(s, ekfom_data); },
-        options::NUM_MAX_ITERATIONS, epsi.data());
-
-    if (std::is_same<IVoxType, IVox<3, IVoxNodeType::PHC, pcl::PointXYZI>>::value == true) {
-        LOG(INFO) << "using phc ivox";
-    } else if (std::is_same<IVoxType, IVox<3, IVoxNodeType::DEFAULT, pcl::PointXYZI>>::value == true) {
-        LOG(INFO) << "using default ivox";
-    }
-*/
-    return true;
-}
-
-bool LaserMapping::LoadParams(ros::NodeHandle &nh) {
-    // get params from param server
-    int lidar_type, ivox_nearby_type;
-    double gyr_cov, acc_cov, b_gyr_cov, b_acc_cov;
-    double filter_size_surf_min;
-    common::V3D lidar_T_wrt_IMU;
-    common::M3D lidar_R_wrt_IMU;
-
-    nh.param<bool>("path_save_en", path_save_en_, true);
-    nh.param<bool>("publish/path_publish_en", path_pub_en_, true);
-    nh.param<bool>("publish/scan_publish_en", scan_pub_en_, true);
-    nh.param<bool>("publish/dense_publish_en", dense_pub_en_, false);
-    nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en_, true);
-    nh.param<bool>("publish/scan_effect_pub_en", scan_effect_pub_en_, false);
-
-    nh.param<int>("max_iteration", options::NUM_MAX_ITERATIONS, 4);
-    nh.param<float>("esti_plane_threshold", options::ESTI_PLANE_THRESHOLD, 0.1);
-    //nh.param<std::string>("map_file_path", map_file_path_, "");
-    nh.param<bool>("common/time_sync_en", mapper_.time_sync_en_, false);
-    nh.param<double>("filter_size_surf", filter_size_surf_min, 0.5);
-    nh.param<double>("filter_size_map", mapper_.filter_size_map_min_, 0.0);
-    nh.param<double>("cube_side_length", mapper_.cube_len_, 200);
-    nh.param<float>("mapping/det_range", mapper_.det_range_, 300.f);
-    nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1);
-    nh.param<double>("mapping/acc_cov", acc_cov, 0.1);
-    nh.param<double>("mapping/b_gyr_cov", b_gyr_cov, 0.0001);
-    nh.param<double>("mapping/b_acc_cov", b_acc_cov, 0.0001);
-    nh.param<double>("preprocess/blind", preprocess_->Blind(), 0.01);
-    nh.param<float>("preprocess/time_scale", preprocess_->TimeScale(), 1e-3);
-    nh.param<int>("preprocess/lidar_type", lidar_type, 1);
-    nh.param<int>("preprocess/scan_line", preprocess_->NumScans(), 16);
-    nh.param<int>("point_filter_num", preprocess_->PointFilterNum(), 2);
-    nh.param<bool>("feature_extract_enable", preprocess_->FeatureEnabled(), false);
-    nh.param<bool>("runtime_pos_log_enable", runtime_pos_log_, true);
-    nh.param<bool>("mapping/extrinsic_est_en", mapper_.extrinsic_est_en_, true);
-    nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en_, false);
-    nh.param<int>("pcd_save/interval", pcd_save_interval_, -1);
-    nh.param<std::vector<double>>("mapping/extrinsic_T", mapper_.extrinT_, std::vector<double>());
-    nh.param<std::vector<double>>("mapping/extrinsic_R", mapper_.extrinR_, std::vector<double>());
-
-    nh.param<float>("ivox_grid_resolution", mapper_.ivox_options_.resolution_, 0.2);
-    nh.param<int>("ivox_nearby_type", ivox_nearby_type, 18);
-
-
-    LOG(INFO) << "lidar_type " << lidar_type;
-    if (lidar_type == 1) {
-        preprocess_->SetLidarType(LidarType::AVIA);
-        LOG(INFO) << "Using AVIA Lidar";
-    } else if (lidar_type == 2) {
-        preprocess_->SetLidarType(LidarType::VELO32);
-        LOG(INFO) << "Using Velodyne 32 Lidar";
-    } else if (lidar_type == 3) {
-        preprocess_->SetLidarType(LidarType::OUST64);
-        LOG(INFO) << "Using OUST 64 Lidar";
-    } else {
-        LOG(WARNING) << "unknown lidar_type";
-        return false;
-    }
-
-    mapper_.Init(ivox_nearby_type,gyr_cov,
-                 acc_cov, b_gyr_cov, b_acc_cov,filter_size_surf_min);
-
-    path_.header.stamp = ros::Time::now();
-    path_.header.frame_id = "camera_init";
-
-
-    return true;
-}
-
-bool LaserMapping::LoadParamsFromYAML(const std::string &yaml_file) {
-    // get params from yaml
-    /*int lidar_type, ivox_nearby_type;
-    double gyr_cov, acc_cov, b_gyr_cov, b_acc_cov;
-    double filter_size_surf_min;
-    common::V3D lidar_T_wrt_IMU;
-    common::M3D lidar_R_wrt_IMU;
-
-    auto yaml = YAML::LoadFile(yaml_file);
-    try {
-        path_pub_en_ = yaml["publish"]["path_publish_en"].as<bool>();
-        scan_pub_en_ = yaml["publish"]["scan_publish_en"].as<bool>();
-        dense_pub_en_ = yaml["publish"]["dense_publish_en"].as<bool>();
-        scan_body_pub_en_ = yaml["publish"]["scan_bodyframe_pub_en"].as<bool>();
-        scan_effect_pub_en_ = yaml["publish"]["scan_effect_pub_en"].as<bool>();
-        path_save_en_ = yaml["path_save_en"].as<bool>();
-
-        options::NUM_MAX_ITERATIONS = yaml["max_iteration"].as<int>();
-        options::ESTI_PLANE_THRESHOLD = yaml["esti_plane_threshold"].as<float>();
-        time_sync_en_ = yaml["common"]["time_sync_en"].as<bool>();
-
-        filter_size_surf_min = yaml["filter_size_surf"].as<float>();
-        filter_size_map_min_ = yaml["filter_size_map"].as<float>();
-        cube_len_ = yaml["cube_side_length"].as<int>();
-        det_range_ = yaml["mapping"]["det_range"].as<float>();
-        gyr_cov = yaml["mapping"]["gyr_cov"].as<float>();
-        acc_cov = yaml["mapping"]["acc_cov"].as<float>();
-        b_gyr_cov = yaml["mapping"]["b_gyr_cov"].as<float>();
-        b_acc_cov = yaml["mapping"]["b_acc_cov"].as<float>();
-        preprocess_->Blind() = yaml["preprocess"]["blind"].as<double>();
-        preprocess_->TimeScale() = yaml["preprocess"]["time_scale"].as<double>();
-        lidar_type = yaml["preprocess"]["lidar_type"].as<int>();
-        preprocess_->NumScans() = yaml["preprocess"]["scan_line"].as<int>();
-        preprocess_->PointFilterNum() = yaml["point_filter_num"].as<int>();
-        preprocess_->FeatureEnabled() = yaml["feature_extract_enable"].as<bool>();
-        extrinsic_est_en_ = yaml["mapping"]["extrinsic_est_en"].as<bool>();
-        pcd_save_en_ = yaml["pcd_save"]["pcd_save_en"].as<bool>();
-        pcd_save_interval_ = yaml["pcd_save"]["interval"].as<int>();
-        extrinT_ = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
-        extrinR_ = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
-
-        ivox_options_.resolution_ = yaml["ivox_grid_resolution"].as<float>();
-        ivox_nearby_type = yaml["ivox_nearby_type"].as<int>();
-    } catch (...) {
-        LOG(ERROR) << "bad conversion";
-        return false;
-    }
-
-    LOG(INFO) << "lidar_type " << lidar_type;
-    if (lidar_type == 1) {
-        preprocess_->SetLidarType(LidarType::AVIA);
-        LOG(INFO) << "Using AVIA Lidar";
-    } else if (lidar_type == 2) {
-        preprocess_->SetLidarType(LidarType::VELO32);
-        LOG(INFO) << "Using Velodyne 32 Lidar";
-    } else if (lidar_type == 3) {
-        preprocess_->SetLidarType(LidarType::OUST64);
-        LOG(INFO) << "Using OUST 64 Lidar";
-    } else {
-        LOG(WARNING) << "unknown lidar_type";
-        return false;
-    }
-
-    if (ivox_nearby_type == 0) {
-        ivox_options_.nearby_type_ = IVoxType::NearbyType::CENTER;
-    } else if (ivox_nearby_type == 6) {
-        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY6;
-    } else if (ivox_nearby_type == 18) {
-        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
-    } else if (ivox_nearby_type == 26) {
-        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY26;
-    } else {
-        LOG(WARNING) << "unknown ivox_nearby_type, use NEARBY18";
-        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
-    }
-
-    voxel_scan_.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
-
-    lidar_T_wrt_IMU = common::VecFromArray<double>(extrinT_);
-    lidar_R_wrt_IMU = common::MatFromArray<double>(extrinR_);
-
-    p_imu_->SetExtrinsic(lidar_T_wrt_IMU, lidar_R_wrt_IMU);
-    p_imu_->SetGyrCov(common::V3D(gyr_cov, gyr_cov, gyr_cov));
-    p_imu_->SetAccCov(common::V3D(acc_cov, acc_cov, acc_cov));
-    p_imu_->SetGyrBiasCov(common::V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov));
-    p_imu_->SetAccBiasCov(common::V3D(b_acc_cov, b_acc_cov, b_acc_cov));
-
-    run_in_offline_ = true;*/
-    return true;
-}
-
 void LaserMapping::SubAndPubToROS(ros::NodeHandle &nh) {
     // ROS subscribe initialization
     std::string lidar_topic, imu_topic;
@@ -234,43 +43,11 @@ LaserMapping::LaserMapping() {
 
 }
 
-void LaserMapping::Run() {
-
-    if (!mapper_.Run())
-        return;
-    // publish or save map pcd
-    if (run_in_offline_) {
-        if (pcd_save_en_) {
-            PublishFrameWorld();
-        }
-        if (path_save_en_) {
-            PublishPath(pub_path_);
-        }
-    } else {
-        if (pub_odom_aft_mapped_) {
-            PublishOdometry(pub_odom_aft_mapped_);
-        }
-        if (path_pub_en_ || path_save_en_) {
-            PublishPath(pub_path_);
-        }
-        if (scan_pub_en_ || pcd_save_en_) {
-            PublishFrameWorld();
-        }
-        if (scan_pub_en_ && scan_body_pub_en_) {
-            PublishFrameBody(pub_laser_cloud_body_);
-        }
-        if (scan_pub_en_ && scan_effect_pub_en_) {
-            PublishFrameEffectWorld(pub_laser_cloud_effect_world_);
-        }
-    }
-
-    // Debug variables
-    mapper_.frame_num_++;
-}
-
 
 
 void LaserMapping::StandardPCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) {
+
+    printf("  LaserMapping::StandardPCLCallBack\n ");
     mapper_.mtx_buffer_.lock();
     Timer::Evaluate(
         [&, this]() {
@@ -291,6 +68,8 @@ void LaserMapping::StandardPCLCallBack(const sensor_msgs::PointCloud2::ConstPtr
 }
 
 void LaserMapping::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
+
+    printf("  LaserMapping::LivoxPCLCallBack\n ");
     mapper_.mtx_buffer_.lock();
     Timer::Evaluate(
         [&, this]() {
@@ -318,8 +97,6 @@ void LaserMapping::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr
             PointCloudType::Ptr ptr(new PointCloudType());
             preprocess_->Process(msg, ptr);
             mapper_.AddPointCloud(ptr,mapper_.last_timestamp_lidar_);
-            /*lidar_buffer_.emplace_back(ptr);
-            time_buffer_.emplace_back(last_timestamp_lidar_);*/
         },
         "Preprocess (Livox)");
 
@@ -327,7 +104,7 @@ void LaserMapping::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr
 }
 
 void LaserMapping::IMUCallBack(const sensor_msgs::Imu::ConstPtr &msg_in) {
-    //publish_count_++;
+    printf("  LaserMapping::IMUCallBack\n ");
     sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
 
     if (abs(mapper_.timediff_lidar_wrt_imu_) > 0.1 && mapper_.time_sync_en_) {
@@ -352,190 +129,6 @@ void LaserMapping::IMUCallBack(const sensor_msgs::Imu::ConstPtr &msg_in) {
 }
 
 
-void LaserMapping::PrintState(const state_ikfom &s) {
-    LOG(INFO) << "state r: " << s.rot.coeffs().transpose() << ", t: " << s.pos.transpose()
-              << ", off r: " << s.offset_R_L_I.coeffs().transpose() << ", t: " << s.offset_T_L_I.transpose();
-}
-
-
-
-/////////////////////////////////////  debug save / show /////////////////////////////////////////////////////
-
-void LaserMapping::PublishPath(const ros::Publisher pub_path) {
-    SetPosestamp(msg_body_pose_);
-    msg_body_pose_.header.stamp = ros::Time().fromSec(mapper_.lidar_end_time_);
-    msg_body_pose_.header.frame_id = "camera_init";
-
-    /*** if path is too large, the rvis will crash ***/
-    path_.poses.push_back(msg_body_pose_);
-    if (run_in_offline_ == false) {
-        pub_path.publish(path_);
-    }
-}
-
-void LaserMapping::PublishOdometry(const ros::Publisher &pub_odom_aft_mapped) {
-    odom_aft_mapped_.header.frame_id = "camera_init";
-    odom_aft_mapped_.child_frame_id = "body";
-    odom_aft_mapped_.header.stamp = ros::Time().fromSec(mapper_.lidar_end_time_);  // ros::Time().fromSec(lidar_end_time_);
-    SetPosestamp(odom_aft_mapped_.pose);
-    pub_odom_aft_mapped.publish(odom_aft_mapped_);
-    auto P = mapper_.kf_.get_P();
-    for (int i = 0; i < 6; i++) {
-        int k = i < 3 ? i + 3 : i - 3;
-        odom_aft_mapped_.pose.covariance[i * 6 + 0] = P(k, 3);
-        odom_aft_mapped_.pose.covariance[i * 6 + 1] = P(k, 4);
-        odom_aft_mapped_.pose.covariance[i * 6 + 2] = P(k, 5);
-        odom_aft_mapped_.pose.covariance[i * 6 + 3] = P(k, 0);
-        odom_aft_mapped_.pose.covariance[i * 6 + 4] = P(k, 1);
-        odom_aft_mapped_.pose.covariance[i * 6 + 5] = P(k, 2);
-    }
-
-    static tf::TransformBroadcaster br;
-    tf::Transform transform;
-    tf::Quaternion q;
-    transform.setOrigin(tf::Vector3(odom_aft_mapped_.pose.pose.position.x, odom_aft_mapped_.pose.pose.position.y,
-                                    odom_aft_mapped_.pose.pose.position.z));
-    q.setW(odom_aft_mapped_.pose.pose.orientation.w);
-    q.setX(odom_aft_mapped_.pose.pose.orientation.x);
-    q.setY(odom_aft_mapped_.pose.pose.orientation.y);
-    q.setZ(odom_aft_mapped_.pose.pose.orientation.z);
-    transform.setRotation(q);
-    br.sendTransform(tf::StampedTransform(transform, odom_aft_mapped_.header.stamp, "camera_init", "body"));
-}
-
-void LaserMapping::PublishFrameWorld() {
-    if (!(run_in_offline_ == false && scan_pub_en_) && !pcd_save_en_) {
-        return;
-    }
-
-    PointCloudType::Ptr laserCloudWorld;
-    if (dense_pub_en_) {
-        PointCloudType::Ptr laserCloudFullRes(mapper_.scan_undistort_);
-        int size = laserCloudFullRes->points.size();
-        laserCloudWorld.reset(new PointCloudType(size, 1));
-        for (int i = 0; i < size; i++) {
-            mapper_.PointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]);
-        }
-    } else {
-        laserCloudWorld = mapper_.scan_down_world_;
-    }
-
-    if (run_in_offline_ == false && scan_pub_en_) {
-        sensor_msgs::PointCloud2 laserCloudmsg;
-        pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
-        laserCloudmsg.header.stamp = ros::Time().fromSec(mapper_.lidar_end_time_);
-        laserCloudmsg.header.frame_id = "camera_init";
-        pub_laser_cloud_world_.publish(laserCloudmsg);
-        mapper_.publish_count_ -= options::PUBFRAME_PERIOD;
-    }
-
-    /**************** save map ****************/
-    /* 1. make sure you have enough memories
-    /* 2. noted that pcd save will influence the real-time performences **/
-    if (pcd_save_en_) {
-        *pcl_wait_save_ += *laserCloudWorld;
-
-        static int scan_wait_num = 0;
-        scan_wait_num++;
-        if (pcl_wait_save_->size() > 0 && pcd_save_interval_ > 0 && scan_wait_num >= pcd_save_interval_) {
-            mapper_.pcd_index_++;
-            std::string all_points_dir(std::string(std::string(ROOT_DIR) + "PCD/scans_") + std::to_string(mapper_.pcd_index_) +
-                                       std::string(".pcd"));
-            pcl::PCDWriter pcd_writer;
-            LOG(INFO) << "current scan saved to /PCD/" << all_points_dir;
-            pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_);
-            pcl_wait_save_->clear();
-            scan_wait_num = 0;
-        }
-    }
-}
-
-void LaserMapping::PublishFrameBody(const ros::Publisher &pub_laser_cloud_body) {
-    int size = mapper_.scan_undistort_->points.size();
-    PointCloudType::Ptr laser_cloud_imu_body(new PointCloudType(size, 1));
-
-    for (int i = 0; i < size; i++) {
-        PointBodyLidarToIMU(&mapper_.scan_undistort_->points[i], &laser_cloud_imu_body->points[i]);
-    }
-
-    sensor_msgs::PointCloud2 laserCloudmsg;
-    pcl::toROSMsg(*laser_cloud_imu_body, laserCloudmsg);
-    laserCloudmsg.header.stamp = ros::Time().fromSec(mapper_.lidar_end_time_);
-    laserCloudmsg.header.frame_id = "body";
-    pub_laser_cloud_body.publish(laserCloudmsg);
-    mapper_.publish_count_ -= options::PUBFRAME_PERIOD;
-}
-
-void LaserMapping::PublishFrameEffectWorld(const ros::Publisher &pub_laser_cloud_effect_world) {
-    int size = mapper_.corr_pts_.size();
-    PointCloudType::Ptr laser_cloud(new PointCloudType(size, 1));
-
-    for (int i = 0; i < size; i++) {
-        mapper_.PointBodyToWorld(mapper_.corr_pts_[i].head<3>(), &laser_cloud->points[i]);
-    }
-    sensor_msgs::PointCloud2 laserCloudmsg;
-    pcl::toROSMsg(*laser_cloud, laserCloudmsg);
-    laserCloudmsg.header.stamp = ros::Time().fromSec(mapper_.lidar_end_time_);
-    laserCloudmsg.header.frame_id = "camera_init";
-    pub_laser_cloud_effect_world.publish(laserCloudmsg);
-    mapper_.publish_count_ -= options::PUBFRAME_PERIOD;
-}
-
-void LaserMapping::Savetrajectory(const std::string &traj_file) {
-    std::ofstream ofs;
-    ofs.open(traj_file, std::ios::out);
-    if (!ofs.is_open()) {
-        LOG(ERROR) << "Failed to open traj_file: " << traj_file;
-        return;
-    }
-
-    ofs << "#timestamp x y z q_x q_y q_z q_w" << std::endl;
-    for (const auto &p : path_.poses) {
-        ofs << std::fixed << std::setprecision(6) << p.header.stamp.toSec() << " " << std::setprecision(15)
-            << p.pose.position.x << " " << p.pose.position.y << " " << p.pose.position.z << " " << p.pose.orientation.x
-            << " " << p.pose.orientation.y << " " << p.pose.orientation.z << " " << p.pose.orientation.w << std::endl;
-    }
-
-    ofs.close();
-}
-
-///////////////////////////  private method /////////////////////////////////////////////////////////////////////
-template <typename T>
-void LaserMapping::SetPosestamp(T &out) {
-    out.pose.position.x = mapper_.state_point_.pos(0);
-    out.pose.position.y = mapper_.state_point_.pos(1);
-    out.pose.position.z = mapper_.state_point_.pos(2);
-    out.pose.orientation.x = mapper_.state_point_.rot.coeffs()[0];
-    out.pose.orientation.y = mapper_.state_point_.rot.coeffs()[1];
-    out.pose.orientation.z = mapper_.state_point_.rot.coeffs()[2];
-    out.pose.orientation.w = mapper_.state_point_.rot.coeffs()[3];
-}
-
-void LaserMapping::PointBodyLidarToIMU(PointType const *const pi, PointType *const po) {
-    common::V3D p_body_lidar(pi->x, pi->y, pi->z);
-    common::V3D p_body_imu(mapper_.state_point_.offset_R_L_I * p_body_lidar + mapper_.state_point_.offset_T_L_I);
-
-    po->x = p_body_imu(0);
-    po->y = p_body_imu(1);
-    po->z = p_body_imu(2);
-    po->intensity = pi->intensity;
-}
-
-void LaserMapping::Finish() {
-    /**************** save map ****************/
-    /* 1. make sure you have enough memories
-    /* 2. pcd save will largely influence the real-time performences **/
-    if (pcl_wait_save_->size() > 0 && pcd_save_en_) {
-        std::string file_name = std::string("scans.pcd");
-        std::string all_points_dir(std::string(std::string(ROOT_DIR) + "PCD/") + file_name);
-        pcl::PCDWriter pcd_writer;
-        LOG(INFO) << "current scan saved to /PCD/" << file_name;
-        pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_);
-    }
-
-    LOG(INFO) << "finish done";
-}
-
 
 TTMapping::TTMapping() {
     p_imu_.reset(new ImuProcess());