app ad node

faster-lio/app/CMakeLists.txt

@@ -1,4 +1,4 @@
-add_executable(run_mapping_online run_mapping_online.cc)
+add_executable(run_mapping_online run_mapping_online.cc map_node.cpp)
 target_link_libraries(run_mapping_online
         ${PROJECT_NAME} gflags
         )

faster-lio/app/map_node.cpp

@@ -0,0 +1,423 @@
+//
+// Created by zx on 22-10-31.
+//
+
+#include "map_node.h"
+#include <tf/transform_broadcaster.h>
+#include <yaml-cpp/yaml.h>
+#include <execution>
+#include <fstream>
+#include <pcl/io/pcd_io.h>
+#include "utils.h"
+
+namespace faster_lio {
+
+bool Map_node::InitROS(ros::NodeHandle &nh) {
+    LoadParams(nh);
+    SubAndPubToROS(nh);
+
+
+    return true;
+}
+
+
+bool Map_node::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;
+}
+
+void Map_node::SubAndPubToROS(ros::NodeHandle &nh) {
+    // ROS subscribe initialization
+    std::string lidar_topic, imu_topic;
+    nh.param<std::string>("common/lid_topic", lidar_topic, "/livox/lidar");
+    nh.param<std::string>("common/imu_topic", imu_topic, "/livox/imu");
+
+    if (preprocess_->GetLidarType() == LidarType::AVIA) {
+        sub_pcl_ = nh.subscribe<livox_ros_driver::CustomMsg>(
+                lidar_topic, 200000, [this](const livox_ros_driver::CustomMsg::ConstPtr &msg) { LivoxPCLCallBack(msg); });
+    } else {
+        sub_pcl_ = nh.subscribe<sensor_msgs::PointCloud2>(
+                lidar_topic, 200000, [this](const sensor_msgs::PointCloud2::ConstPtr &msg) { StandardPCLCallBack(msg); });
+    }
+
+    sub_imu_ = nh.subscribe<sensor_msgs::Imu>(imu_topic, 200000,
+                                              [this](const sensor_msgs::Imu::ConstPtr &msg) { IMUCallBack(msg); });
+
+    // ROS publisher init
+    path_.header.stamp = ros::Time::now();
+    path_.header.frame_id = "camera_init";
+
+    pub_laser_cloud_world_ = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 100000);
+    pub_laser_cloud_body_ = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered_body", 100000);
+    pub_laser_cloud_effect_world_ = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered_effect_world", 100000);
+    pub_odom_aft_mapped_ = nh.advertise<nav_msgs::Odometry>("/Odometry", 100000);
+    pub_path_ = nh.advertise<nav_msgs::Path>("/path", 100000);
+}
+
+Map_node::Map_node() {
+    preprocess_.reset(new PointCloudPreprocess());
+
+}
+
+void Map_node::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 Map_node::StandardPCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) {
+    mapper_.mtx_buffer_.lock();
+    Timer::Evaluate(
+            [&, this]() {
+              mapper_.scan_count_++;
+              if (msg->header.stamp.toSec() < mapper_.last_timestamp_lidar_) {
+                  LOG(ERROR) << "lidar loop back, clear buffer";
+                  mapper_.lidar_buffer_.clear();
+              }
+
+              PointCloudType::Ptr ptr(new PointCloudType());
+              preprocess_->Process(msg, ptr);
+              mapper_.lidar_buffer_.push_back(ptr);
+              mapper_.time_buffer_.push_back(msg->header.stamp.toSec());
+              mapper_.last_timestamp_lidar_ = msg->header.stamp.toSec();
+            },
+            "Preprocess (Standard)");
+    mapper_.mtx_buffer_.unlock();
+}
+
+void Map_node::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
+    mapper_.mtx_buffer_.lock();
+    Timer::Evaluate(
+            [&, this]() {
+              //scan_count_++;
+              if (msg->header.stamp.toSec() < mapper_.last_timestamp_lidar_) {
+                  LOG(WARNING) << "lidar loop back, clear buffer";
+                  mapper_.lidar_buffer_.clear();
+              }
+
+              mapper_.last_timestamp_lidar_ = msg->header.stamp.toSec();
+
+              if (!mapper_.time_sync_en_ && abs(mapper_.last_timestamp_imu_ - mapper_.last_timestamp_lidar_) > 10.0 && !mapper_.imu_buffer_.empty() &&
+                      !mapper_.lidar_buffer_.empty()) {
+                  LOG(INFO) << "IMU and LiDAR not Synced, IMU time: " << mapper_.last_timestamp_imu_
+                            << ", lidar header time: " << mapper_.last_timestamp_lidar_;
+              }
+
+              if (mapper_.time_sync_en_ && !mapper_.timediff_set_flg_ && abs(mapper_.last_timestamp_lidar_ - mapper_.last_timestamp_imu_) > 1 &&
+                      !mapper_.imu_buffer_.empty()) {
+                  mapper_.timediff_set_flg_ = true;
+                  mapper_.timediff_lidar_wrt_imu_ = mapper_.last_timestamp_lidar_ + 0.1 - mapper_.last_timestamp_imu_;
+                  LOG(INFO) << "Self sync IMU and LiDAR, time diff is " << mapper_.timediff_lidar_wrt_imu_;
+              }
+
+              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)");
+
+    mapper_.mtx_buffer_.unlock();
+}
+
+void Map_node::IMUCallBack(const sensor_msgs::Imu::ConstPtr &msg_in) {
+    //publish_count_++;
+    sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
+
+    if (abs(mapper_.timediff_lidar_wrt_imu_) > 0.1 && mapper_.time_sync_en_) {
+        msg->header.stamp = ros::Time().fromSec(mapper_.timediff_lidar_wrt_imu_ + msg_in->header.stamp.toSec());
+    }
+
+    double timestamp = msg->header.stamp.toSec();
+
+
+    common::ImuData imu;
+    imu.timebase=msg_in->header.stamp.toSec();
+    imu.orientation=Eigen::Quaterniond(msg_in->orientation.w,msg_in->orientation.x,
+                                       msg_in->orientation.y,msg_in->orientation.z);
+    imu.orientation_covariance=Eigen::Matrix3d(msg_in->orientation_covariance.data());
+    imu.angular_velocity=Eigen::Vector3d(msg_in->angular_velocity.x,msg_in->angular_velocity.y,msg_in->angular_velocity.z);
+    imu.angular_velocity_covariance=Eigen::Matrix3d(msg_in->angular_velocity_covariance.data());
+    imu.linear_acceleration=Eigen::Vector3d(msg_in->linear_acceleration.x,msg_in->linear_acceleration.y,msg_in->linear_acceleration.z);
+    imu.linear_acceleration_covariance=Eigen::Matrix3d(msg_in->linear_acceleration_covariance.data());
+    mapper_.mtx_buffer_.lock();
+    mapper_.AddImu(imu,timestamp);
+    mapper_.mtx_buffer_.unlock();
+}
+
+
+
+
+/////////////////////////////////////  debug save / show /////////////////////////////////////////////////////
+
+void Map_node::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 Map_node::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 Map_node::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 Map_node::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 Map_node::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 Map_node::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 Map_node::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 Map_node::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 Map_node::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";
+}
+
+}  // namespace faster_lio

faster-lio/app/map_node.h

@@ -0,0 +1,113 @@
+//
+// Created by zx on 22-10-31.
+//
+
+#ifndef SRC_FASTER_LIO_APP_MAP_NODE_H_
+#define SRC_FASTER_LIO_APP_MAP_NODE_H_
+
+
+#include <livox_ros_driver/CustomMsg.h>
+#include <nav_msgs/Path.h>
+#include <pcl/filters/voxel_grid.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <condition_variable>
+#include <thread>
+
+#include <eigen_conversions/eigen_msg.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include "pointcloud_preprocess.h"
+#include "mapping.h"
+namespace faster_lio {
+
+
+class Map_node {
+ public:
+
+    Map_node();
+    ~Map_node() {
+    }
+
+    /// init with ros
+    bool InitROS(ros::NodeHandle &nh);
+
+
+    void Run();
+
+
+    // callbacks of lidar and imu
+    void StandardPCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg);
+    void LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg);
+    void IMUCallBack(const sensor_msgs::Imu::ConstPtr &msg_in);
+
+
+
+    ////////////////////////////// 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);
+
+
+ private:
+    std::shared_ptr<PointCloudPreprocess> preprocess_ = nullptr;  // point cloud preprocess
+
+    /// ros pub and sub stuffs
+    ros::Subscriber sub_pcl_;
+    ros::Subscriber sub_imu_;
+    ros::Publisher pub_laser_cloud_world_;
+    ros::Publisher pub_laser_cloud_body_;
+    ros::Publisher pub_laser_cloud_effect_world_;
+    ros::Publisher pub_odom_aft_mapped_;
+    ros::Publisher pub_path_;
+
+
+    nav_msgs::Odometry odom_aft_mapped_;
+
+
+
+    /////////////////////////  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
+    nav_msgs::Path path_;
+    geometry_msgs::PoseStamped msg_body_pose_;
+
+
+    TTMapping  mapper_;
+};
+
+
+}  // namespace faster_lio
+
+
+#endif //SRC_FASTER_LIO_APP_MAP_NODE_H_

faster-lio/app/run_mapping_online.cc

@@ -5,7 +5,7 @@
 #include <unistd.h>
 #include <csignal>
 
-#include "laser_mapping.h"
+#include "map_node.h"
 
 /// run the lidar mapping in online mode
 
@@ -23,7 +23,7 @@ int main(int argc, char **argv) {
     ros::init(argc, argv, "faster_lio");
     ros::NodeHandle nh;
 
-    auto laser_mapping = std::make_shared<faster_lio::LaserMapping>();
+    auto laser_mapping = std::make_shared<faster_lio::Map_node>();
     laser_mapping->InitROS(nh);
 
     signal(SIGINT, SigHandle);

faster-lio/include/laser_mapping.h

@@ -13,14 +13,11 @@
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/PointCloud2.h>
-
+#include "pointcloud_preprocess.h"
 #include "mapping.h"
 namespace faster_lio {
 
 
-
-
-
 class LaserMapping {
    public:
 

faster-lio/include/mapping.h

@@ -8,8 +8,9 @@
 #include "imu_processing.h"
 #include "ivox3d/ivox3d.h"
 #include "options.h"
-#include "pointcloud_preprocess.h"
 #include <Eigen/Eigen>
+#include <pcl/filters/voxel_grid.h>
+#include <mutex>
 namespace faster_lio
 {
 

faster-lio/include/pointcloud_preprocess.h

@@ -4,9 +4,6 @@
 #include <livox_ros_driver/CustomMsg.h>
 #include <pcl_conversions/pcl_conversions.h>
 
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
 #include "common_lib.h"
 
 namespace velodyne_ros {

faster-lio/src/CMakeLists.txt

@@ -1,4 +1,5 @@
 add_library(${PROJECT_NAME}
+        mapping.cpp
         laser_mapping.cc
         use-ikfom.cc
         pointcloud_preprocess.cc
@@ -7,7 +8,7 @@ add_library(${PROJECT_NAME}
         utils.cc
         )
 
-#add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp livox_ros_driver_gencpp)
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp livox_ros_driver_gencpp)
 
 target_link_libraries(${PROJECT_NAME}
         ${catkin_LIBRARIES}

faster-lio/src/laser_mapping.cc

@@ -2,7 +2,7 @@
 #include <yaml-cpp/yaml.h>
 #include <execution>
 #include <fstream>
-
+#include <pcl/io/pcd_io.h>
 #include "laser_mapping.h"
 #include "utils.h"
 
@@ -536,30 +536,36 @@ void LaserMapping::Finish() {
     LOG(INFO) << "finish done";
 }
 
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 TTMapping::TTMapping() {
     p_imu_.reset(new ImuProcess());
 }
 
-
-
-bool TTMapping::Init(int ivox_nearby_type,double gyr_cov, double acc_cov,
-                     double b_gyr_cov, double b_acc_cov,double filter_size_surf_min)
+bool TTMapping::Init(int ivox_nearby_type, double gyr_cov, double acc_cov,
+                     double b_gyr_cov, double b_acc_cov, double filter_size_surf_min)
 {
 
     common::V3D lidar_T_wrt_IMU;
     common::M3D lidar_R_wrt_IMU;
 
-    if (ivox_nearby_type == 0) {
+    if (ivox_nearby_type == 0)
+    {
         ivox_options_.nearby_type_ = IVoxType::NearbyType::CENTER;
-    } else if (ivox_nearby_type == 6) {
+    }
+    else if (ivox_nearby_type == 6)
+    {
         ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY6;
-    } else if (ivox_nearby_type == 18) {
+    }
+    else if (ivox_nearby_type == 18)
+    {
         ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
-    } else if (ivox_nearby_type == 26) {
+    }
+    else if (ivox_nearby_type == 26)
+    {
         ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY26;
-    } else {
+    }
+    else
+    {
         LOG(WARNING) << "unknown ivox_nearby_type, use NEARBY18";
         ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
     }
@@ -583,26 +589,32 @@ bool TTMapping::Init(int ivox_nearby_type,double gyr_cov, double acc_cov,
     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); },
+            [this](state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data) {
+              ObsModel(s, ekfom_data);
+            },
             options::NUM_MAX_ITERATIONS, epsi.data());
     return true;
 
 }
+
 bool TTMapping::Run()
 {
-    if (!SyncPackages()) {
+    if (!SyncPackages())
+    {
         return false;
     }
 
     /// IMU process, kf prediction, undistortion
     p_imu_->Process(measures_, kf_, scan_undistort_);
-    if (scan_undistort_->empty() || (scan_undistort_ == nullptr)) {
+    if (scan_undistort_->empty() || (scan_undistort_ == nullptr))
+    {
         LOG(WARNING) << "No point, skip this scan!";
         return false;
     }
 
     /// the first scan
-    if (flg_first_scan_) {
+    if (flg_first_scan_)
+    {
         ivox_->AddPoints(scan_undistort_->points);
         first_lidar_time_ = measures_.lidar_bag_time_;
         flg_first_scan_ = false;
@@ -619,7 +631,8 @@ bool TTMapping::Run()
             "Downsample PointCloud");
 
     int cur_pts = scan_down_body_->size();
-    if (cur_pts < 5) {
+    if (cur_pts < 5)
+    {
         LOG(WARNING) << "Too few points, skip this scan!" << scan_undistort_->size() << ", " << scan_down_body_->size();
         return false;
     }
@@ -644,7 +657,9 @@ bool TTMapping::Run()
             "IEKF Solve and Update");
 
     // update local map
-    Timer::Evaluate([&, this]() { MapIncremental(); }, "    Incremental Mapping");
+    Timer::Evaluate([&, this]() {
+      MapIncremental();
+    }, "    Incremental Mapping");
 
     LOG(INFO) << "[ mapping ]: In num: " << scan_undistort_->points.size() << " downsamp " << cur_pts
               << " Map grid num: " << ivox_->NumValidGrids() << " effect num : " << effect_feat_num_;
@@ -656,22 +671,30 @@ bool TTMapping::Run()
     return true;
 }
 
-bool TTMapping::SyncPackages() {
-    if (lidar_buffer_.empty() || imu_buffer_.empty()) {
+bool TTMapping::SyncPackages()
+{
+    if (lidar_buffer_.empty() || imu_buffer_.empty())
+    {
         return false;
     }
 
     /*** push a lidar scan ***/
-    if (!lidar_pushed_) {
+    if (!lidar_pushed_)
+    {
         measures_.lidar_ = lidar_buffer_.front();
         measures_.lidar_bag_time_ = time_buffer_.front();
 
-        if (measures_.lidar_->points.size() <= 1) {
+        if (measures_.lidar_->points.size() <= 1)
+        {
             LOG(WARNING) << "Too few input point cloud!";
             lidar_end_time_ = measures_.lidar_bag_time_ + lidar_mean_scantime_;
-        } else if (measures_.lidar_->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime_) {
+        }
+        else if (measures_.lidar_->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime_)
+        {
             lidar_end_time_ = measures_.lidar_bag_time_ + lidar_mean_scantime_;
-        } else {
+        }
+        else
+        {
             scan_num_++;
             lidar_end_time_ = measures_.lidar_bag_time_ + measures_.lidar_->points.back().curvature / double(1000);
             lidar_mean_scantime_ +=
@@ -682,14 +705,16 @@ bool TTMapping::SyncPackages() {
         lidar_pushed_ = true;
     }
 
-    if (last_timestamp_imu_ < lidar_end_time_) {
+    if (last_timestamp_imu_ < lidar_end_time_)
+    {
         return false;
     }
 
     /*** push imu_ data, and pop from imu_ buffer ***/
     double imu_time = imu_buffer_.front().timebase;
     measures_.imu_.clear();
-    while ((!imu_buffer_.empty()) && (imu_time < lidar_end_time_)) {
+    while ((!imu_buffer_.empty()) && (imu_time < lidar_end_time_))
+    {
         imu_time = imu_buffer_.front().timebase;
         if (imu_time > lidar_end_time_) break;
         measures_.imu_.push_back(imu_buffer_.front());
@@ -702,7 +727,8 @@ bool TTMapping::SyncPackages() {
     return true;
 }
 
-void TTMapping::MapIncremental() {
+void TTMapping::MapIncremental()
+{
     PointVector points_to_add;
     PointVector point_no_need_downsample;
 
@@ -711,7 +737,8 @@ void TTMapping::MapIncremental() {
     point_no_need_downsample.reserve(cur_pts);
 
     std::vector<size_t> index(cur_pts);
-    for (size_t i = 0; i < cur_pts; ++i) {
+    for (size_t i = 0; i < cur_pts; ++i)
+    {
         index[i] = i;
     }
 
@@ -721,7 +748,8 @@ void TTMapping::MapIncremental() {
 
       /* decide if need add to map */
       PointType &point_world = scan_down_world_->points[i];
-      if (!nearest_points_[i].empty() && flg_EKF_inited_) {
+      if (!nearest_points_[i].empty() && flg_EKF_inited_)
+      {
           const PointVector &points_near = nearest_points_[i];
 
           Eigen::Vector3f center =
@@ -731,25 +759,32 @@ void TTMapping::MapIncremental() {
 
           if (fabs(dis_2_center.x()) > 0.5 * filter_size_map_min_ &&
                   fabs(dis_2_center.y()) > 0.5 * filter_size_map_min_ &&
-                  fabs(dis_2_center.z()) > 0.5 * filter_size_map_min_) {
+                  fabs(dis_2_center.z()) > 0.5 * filter_size_map_min_)
+          {
               point_no_need_downsample.emplace_back(point_world);
               return;
           }
 
           bool need_add = true;
           float dist = common::calc_dist(point_world.getVector3fMap(), center);
-          if (points_near.size() >= options::NUM_MATCH_POINTS) {
-              for (int readd_i = 0; readd_i < options::NUM_MATCH_POINTS; readd_i++) {
-                  if (common::calc_dist(points_near[readd_i].getVector3fMap(), center) < dist + 1e-6) {
+          if (points_near.size() >= options::NUM_MATCH_POINTS)
+          {
+              for (int readd_i = 0; readd_i < options::NUM_MATCH_POINTS; readd_i++)
+              {
+                  if (common::calc_dist(points_near[readd_i].getVector3fMap(), center) < dist + 1e-6)
+                  {
                       need_add = false;
                       break;
                   }
               }
           }
-          if (need_add) {
+          if (need_add)
+          {
               points_to_add.emplace_back(point_world);
           }
-      } else {
+      }
+      else
+      {
           points_to_add.emplace_back(point_world);
       }
     });
@@ -763,7 +798,8 @@ void TTMapping::MapIncremental() {
 }
 
 
-void TTMapping::PointBodyToWorld(const PointType *pi, PointType *const po) {
+void TTMapping::PointBodyToWorld(const PointType *pi, PointType *const po)
+{
     common::V3D p_body(pi->x, pi->y, pi->z);
     common::V3D p_global(state_point_.rot * (state_point_.offset_R_L_I * p_body + state_point_.offset_T_L_I) +
                                  state_point_.pos);
@@ -774,7 +810,8 @@ void TTMapping::PointBodyToWorld(const PointType *pi, PointType *const po) {
     po->intensity = pi->intensity;
 }
 
-void TTMapping::PointBodyToWorld(const common::V3F &pi, PointType *const po) {
+void TTMapping::PointBodyToWorld(const common::V3F &pi, PointType *const po)
+{
     common::V3D p_body(pi.x(), pi.y(), pi.z());
     common::V3D p_global(state_point_.rot * (state_point_.offset_R_L_I * p_body + state_point_.offset_T_L_I) +
                                  state_point_.pos);
@@ -792,11 +829,13 @@ void TTMapping::PointBodyToWorld(const common::V3F &pi, PointType *const po) {
  * @param s kf state
  * @param ekfom_data H matrix
  */
-void TTMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data) {
+void TTMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data)
+{
     int cnt_pts = scan_down_body_->size();
 
     std::vector<size_t> index(cnt_pts);
-    for (size_t i = 0; i < index.size(); ++i) {
+    for (size_t i = 0; i < index.size(); ++i)
+    {
         index[i] = i;
     }
 
@@ -816,23 +855,27 @@ void TTMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &
                 point_world.intensity = point_body.intensity;
 
                 auto &points_near = nearest_points_[i];
-                if (ekfom_data.converge) {
+                if (ekfom_data.converge)
+                {
                     /** Find the closest surfaces in the map **/
                     ivox_->GetClosestPoint(point_world, points_near, options::NUM_MATCH_POINTS);
                     point_selected_surf_[i] = points_near.size() >= options::MIN_NUM_MATCH_POINTS;
-                    if (point_selected_surf_[i]) {
+                    if (point_selected_surf_[i])
+                    {
                         point_selected_surf_[i] =
                                 common::esti_plane(plane_coef_[i], points_near, options::ESTI_PLANE_THRESHOLD);
                     }
                 }
 
-                if (point_selected_surf_[i]) {
+                if (point_selected_surf_[i])
+                {
                     auto temp = point_world.getVector4fMap();
                     temp[3] = 1.0;
                     float pd2 = plane_coef_[i].dot(temp);
 
                     bool valid_corr = p_body.norm() > 81 * pd2 * pd2;
-                    if (valid_corr) {
+                    if (valid_corr)
+                    {
                         point_selected_surf_[i] = true;
                         residuals_[i] = pd2;
                     }
@@ -845,8 +888,10 @@ void TTMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &
 
     corr_pts_.resize(cnt_pts);
     corr_norm_.resize(cnt_pts);
-    for (int i = 0; i < cnt_pts; i++) {
-        if (point_selected_surf_[i]) {
+    for (int i = 0; i < cnt_pts; i++)
+    {
+        if (point_selected_surf_[i])
+        {
             corr_norm_[effect_feat_num_] = plane_coef_[i];
             corr_pts_[effect_feat_num_] = scan_down_body_->points[i].getVector4fMap();
             corr_pts_[effect_feat_num_][3] = residuals_[i];
@@ -857,7 +902,8 @@ void TTMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &
     corr_pts_.resize(effect_feat_num_);
     corr_norm_.resize(effect_feat_num_);
 
-    if (effect_feat_num_ < 1) {
+    if (effect_feat_num_ < 1)
+    {
         ekfom_data.valid = false;
         LOG(WARNING) << "No Effective Points!";
         return;
@@ -887,11 +933,14 @@ void TTMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &
                 common::V3F C(Rt * norm_vec);
                 common::V3F A(point_crossmat * C);
 
-                if (extrinsic_est_en_) {
+                if (extrinsic_est_en_)
+                {
                     common::V3F B(point_be_crossmat * off_R.transpose() * C);
                     ekfom_data.h_x.block<1, 12>(i, 0) << norm_vec[0], norm_vec[1], norm_vec[2], A[0], A[1], A[2], B[0],
                             B[1], B[2], C[0], C[1], C[2];
-                } else {
+                }
+                else
+                {
                     ekfom_data.h_x.block<1, 12>(i, 0) << norm_vec[0], norm_vec[1], norm_vec[2], A[0], A[1], A[2], 0.0,
                             0.0, 0.0, 0.0, 0.0, 0.0;
                 }
@@ -903,16 +952,18 @@ void TTMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &
             "    ObsModel (IEKF Build Jacobian)");
 }
 
-void TTMapping::AddPointCloud(PointCloudType::Ptr cloud,double timebase)
+void TTMapping::AddPointCloud(PointCloudType::Ptr cloud, double timebase)
 {
     scan_count_++;
     lidar_buffer_.emplace_back(cloud);
     time_buffer_.emplace_back(timebase);
 }
-void TTMapping::AddImu(const common::ImuData &imu,double timebase)
+
+void TTMapping::AddImu(const common::ImuData &imu, double timebase)
 {
     publish_count_++;
-    if (timebase < last_timestamp_imu_) {
+    if (timebase < last_timestamp_imu_)
+    {
         LOG(WARNING) << "imu loop back, clear buffer";
         imu_buffer_.clear();
     }
@@ -921,4 +972,7 @@ void TTMapping::AddImu(const common::ImuData &imu,double timebase)
     imu_buffer_.emplace_back(imu);
 }
 
+
+
+
 }  // namespace faster_lio

faster-lio/src/mapping.cpp

@@ -0,0 +1,13 @@
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+#include "mapping.h"
+
+
+namespace faster_lio
+{
+
+
+}