diff --git a/README.md b/README.md index 85271c2..4fe06c6 100644 --- a/README.md +++ b/README.md @@ -204,7 +204,22 @@ Step B: Run below Step C: Run LiDAR's ros driver or play rosbag. -### 3.4 PCD file save +### 3.3 For MARSIM Simulator + +Install MARSIM: https://github.com/hku-mars/MARSIM and run MARSIM as below + +``` +cd ~/$MARSIM_ROS_DIR$ +roslaunch test_interface single_drone_avia.launch +``` + +Then Run FAST-LIO: + +``` +roslaunch fast_lio mapping_marsim.launch +``` + +### 3.5 PCD file save Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds. diff --git a/config/marsim.yaml b/config/marsim.yaml new file mode 100644 index 0000000..bb9a08f --- /dev/null +++ b/config/marsim.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/quad0_pcl_render_node/sensor_cloud" + imu_topic: "/quad_0/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 4 + blind: 0.5 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 90 + det_range: 50.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.0, -0.0, 0.0 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/launch/mapping_marsim.launch b/launch/mapping_marsim.launch new file mode 100644 index 0000000..e08a79c --- /dev/null +++ b/launch/mapping_marsim.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp index 0774911..344d07f 100644 --- a/src/IMU_Processing.hpp +++ b/src/IMU_Processing.hpp @@ -23,6 +23,7 @@ #include #include #include "use-ikfom.hpp" +#include "preprocess.h" /// *************Preconfiguration @@ -59,6 +60,7 @@ class ImuProcess V3D cov_bias_gyr; V3D cov_bias_acc; double first_lidar_time; + int lidar_type; private: void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); @@ -218,10 +220,16 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekfheader.stamp.toSec(); const double &imu_end_time = v_imu.back()->header.stamp.toSec(); - const double &pcl_beg_time = meas.lidar_beg_time; - const double &pcl_end_time = meas.lidar_end_time; - - /*** sort point clouds by offset time ***/ + + double pcl_beg_time = meas.lidar_beg_time; + double pcl_end_time = meas.lidar_end_time; + + if (lidar_type == MARSIM) { + pcl_beg_time = last_lidar_end_time_; + pcl_end_time = meas.lidar_beg_time; + } + + /*** sort point clouds by offset time ***/ pcl_out = *(meas.lidar); sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); // cout<<"[ IMU Process ]: Process lidar from "<rot); - // cout<<"head imu acc: "<vel); - pos_imu<pos); - acc_imu<acc); - angvel_avr<gyr); - for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) - { - dt = it_pcl->curvature / double(1000) - head->offset_time; + if(lidar_type != MARSIM){ + auto it_pcl = pcl_out.points.end() - 1; + for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) + { + auto head = it_kp - 1; + auto tail = it_kp; + R_imu<rot); + // cout<<"head imu acc: "<vel); + pos_imu<pos); + acc_imu<acc); + angvel_avr<gyr); - /* Transform to the 'end' frame, using only the rotation - * Note: Compensation direction is INVERSE of Frame's moving direction - * So if we want to compensate a point at timestamp-i to the frame-e - * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ - M3D R_i(R_imu * Exp(angvel_avr, dt)); - - V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); - V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); - V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate! - - // save Undistorted points and their rotation - it_pcl->x = P_compensate(0); - it_pcl->y = P_compensate(1); - it_pcl->z = P_compensate(2); + for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) + { + dt = it_pcl->curvature / double(1000) - head->offset_time; - if (it_pcl == pcl_out.points.begin()) break; - } + /* Transform to the 'end' frame, using only the rotation + * Note: Compensation direction is INVERSE of Frame's moving direction + * So if we want to compensate a point at timestamp-i to the frame-e + * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ + M3D R_i(R_imu * Exp(angvel_avr, dt)); + + V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); + V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); + V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate! + + // save Undistorted points and their rotation + it_pcl->x = P_compensate(0); + it_pcl->y = P_compensate(1); + it_pcl->z = P_compensate(2); + + if (it_pcl == pcl_out.points.begin()) break; + } + } } } diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 44bc280..fc41408 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -94,6 +94,7 @@ int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudVal bool point_selected_surf[100000] = {0}; bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited; bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false; +int lidar_type; vector> pointSearchInd_surf; vector cub_needrm; @@ -375,6 +376,8 @@ bool sync_packages(MeasureGroup &meas) { meas.lidar = lidar_buffer.front(); meas.lidar_beg_time = time_buffer.front(); + + if (meas.lidar->points.size() <= 1) // time too little { lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; @@ -390,6 +393,8 @@ bool sync_packages(MeasureGroup &meas) lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num; } + if(lidar_type == MARSIM) + lidar_end_time = meas.lidar_beg_time; meas.lidar_end_time = lidar_end_time; @@ -774,7 +779,7 @@ int main(int argc, char** argv) nh.param("mapping/b_gyr_cov",b_gyr_cov,0.0001); nh.param("mapping/b_acc_cov",b_acc_cov,0.0001); nh.param("preprocess/blind", p_pre->blind, 0.01); - nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); + nh.param("preprocess/lidar_type", lidar_type, AVIA); nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); nh.param("preprocess/timestamp_unit", p_pre->time_unit, US); nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); @@ -786,6 +791,8 @@ int main(int argc, char** argv) nh.param("pcd_save/interval", pcd_save_interval, -1); nh.param>("mapping/extrinsic_T", extrinT, vector()); nh.param>("mapping/extrinsic_R", extrinR, vector()); + + p_pre->lidar_type = lidar_type; cout<<"p_pre->lidar_type "<lidar_type<set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); - + p_imu->lidar_type = lidar_type; double epsi[23] = {0.001}; fill(epsi, epsi+23, 0.001); kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi); diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 2bce047..09a3959 100644 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -77,6 +77,10 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo case VELO16: velodyne_handler(msg); break; + + case MARSIM: + sim_handler(msg); + break; default: printf("Error LiDAR Type"); @@ -451,6 +455,31 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) } } +void Preprocess::sim_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { + pl_surf.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_surf.reserve(plsize); + for (int i = 0; i < pl_orig.points.size(); i++) { + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + + pl_orig.points[i].z * pl_orig.points[i].z; + if (range < blind * blind) continue; + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.curvature = 0.0; + pl_surf.points.push_back(added_pt); + } +} + void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) { int plsize = pl.size(); diff --git a/src/preprocess.h b/src/preprocess.h index 45b3eb9..35f3833 100644 --- a/src/preprocess.h +++ b/src/preprocess.h @@ -1,3 +1,7 @@ +#ifndef PREPROCESS_H +#define PREPROCESS_H + + #include #include #include @@ -10,7 +14,7 @@ using namespace std; typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; -enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3} +enum LID_TYPE{AVIA = 1, VELO16, OUST64, MARSIM}; //{1, 2, 3} enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; enum Surround{Prev, Next}; @@ -106,6 +110,7 @@ class Preprocess void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void sim_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void give_feature(PointCloudXYZI &pl, vector &types); void pub_func(PointCloudXYZI &pl, const ros::Time &ct); int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); @@ -122,3 +127,4 @@ class Preprocess double smallp_intersect, smallp_ratio; double vx, vy, vz; }; +#endif \ No newline at end of file