diff --git a/.gitignore b/.gitignore index 09ff613b..eb7be626 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ Log/*.csv Log/*.pdf .vscode/c_cpp_properties.json .vscode/settings.json +PCD/*.pcd diff --git a/config/ouster64.yaml b/config/ouster64.yaml index aef4b7ff..b8e79570 100644 --- a/config/ouster64.yaml +++ b/config/ouster64.yaml @@ -5,7 +5,7 @@ common: preprocess: lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 64 - blind: 1 + blind: 4 mapping: acc_cov: 0.1 diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch index 76667ee5..5b61e6b4 100644 --- a/launch/mapping_avia.launch +++ b/launch/mapping_avia.launch @@ -13,6 +13,7 @@ + diff --git a/launch/mapping_horizon.launch b/launch/mapping_horizon.launch index a61845d5..ed0276c0 100644 --- a/launch/mapping_horizon.launch +++ b/launch/mapping_horizon.launch @@ -13,6 +13,7 @@ + diff --git a/launch/mapping_ouster64.launch b/launch/mapping_ouster64.launch index 598cbbd1..1b4c85ef 100644 --- a/launch/mapping_ouster64.launch +++ b/launch/mapping_ouster64.launch @@ -13,6 +13,7 @@ + diff --git a/launch/mapping_velodyne.launch b/launch/mapping_velodyne.launch index f2183456..819253eb 100644 --- a/launch/mapping_velodyne.launch +++ b/launch/mapping_velodyne.launch @@ -13,6 +13,7 @@ + diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 5a5fa1af..75e2cc7d 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -70,7 +70,7 @@ double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_ti double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN]; double match_time = 0, solve_time = 0, solve_const_H_time = 0; int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0; -bool runtime_pos_log = false; +bool runtime_pos_log = false, pcd_save_en = false; /**************************/ float res_last[100000] = {0.0}; @@ -420,6 +420,7 @@ void map_incremental() } PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); +int ind = 0; void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) { PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); @@ -444,6 +445,19 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) publish_count -= PUBFRAME_PERIOD; pcl_wait_pub->clear(); } + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. pcd save will largely influence the real-time performences **/ + if (laserCloudWorld->size() > 0 && pcd_save_en) + { + string file_name = string("scans_"+to_string(ind)+".pcd"); + string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << file_name<("point_filter_num", p_pre->point_filter_num, 2); nh.param("feature_extract_enable", p_pre->feature_enabled, 0); nh.param("runtime_pos_log_enable", runtime_pos_log, 0); + nh.param("pcd_save_enable", pcd_save_en, 0); nh.param>("mapping/extrinsic_T", extrinT, vector()); nh.param>("mapping/extrinsic_R", extrinR, vector()); cout<<"p_pre->lidar_type "<lidar_type< 0 && corner_points.size() > 0) - { - pcl::PCDWriter pcd_writer; - cout << "saving..."; - pcd_writer.writeBinary(surf_filename, surf_points); - pcd_writer.writeBinary(corner_filename, corner_points); - } - *******************************************/ + if (runtime_pos_log) { vector t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;