From 845e20355bf0558cef7f40b281a2371297450fe9 Mon Sep 17 00:00:00 2001 From: xw Date: Tue, 6 Jul 2021 02:38:05 -0400 Subject: [PATCH] add pcd saving function --- .gitignore | 1 + config/ouster64.yaml | 2 +- launch/mapping_avia.launch | 1 + launch/mapping_horizon.launch | 1 + launch/mapping_ouster64.launch | 1 + launch/mapping_velodyne.launch | 1 + src/laserMapping.cpp | 33 ++++++++++++++++++--------------- 7 files changed, 24 insertions(+), 16 deletions(-) diff --git a/.gitignore b/.gitignore index 09ff613..eb7be62 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 aef4b7f..b8e7957 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 76667ee..5b61e6b 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 a61845d..ed0276c 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 598cbbd..1b4c85e 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 f218345..819253e 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 5a5fa1a..75e2cc7 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;