update the pcd saving to one map

PCD/1

@@ -0,0 +1 @@
+1

README.md

@@ -101,11 +101,11 @@ Connect to your PC to Livox Avia LiDAR by following  [Livox-ros-driver installat
 
 ### 3.2 For Livox serials with external IMU
 
-mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial lidar, but need to setup some parameters befor run:
+mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:
 
 Edit ``` config/avia.yaml ``` to set the below parameters:
 
-1. lidar point cloud topic name: ``` lid_topic ```
+1. LiDAR point cloud topic name: ``` lid_topic ```
 2. IMU topic name: ``` imu_topic ```
 3. Translational extrinsic: ``` extrinsic_T ```
 4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
@@ -117,7 +117,7 @@ Step A: Setup before run
 
 Edit ``` config/velodyne.yaml ``` to set the below parameters:
 
-1. lidar point cloud topic name: ``` lid_topic ```
+1. LiDAR point cloud topic name: ``` lid_topic ```
 2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
 3. Line number (we tested 16 and 32 line, but not tested 64 or above): ``` scan_line ```
 4. Translational extrinsic: ``` extrinsic_T ```
@@ -138,7 +138,7 @@ Step C: Run LiDAR's ros driver or play rosbag.
 
 ### 3.4 PCD file save
 
-Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```, every frame will be saved as an independent .PCD file in ``` FAST_LIO/PCD/ ``` directory.
+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.
 
 ## 4. Rosbag Example
 ### 4.1 Indoor rosbag (Livox Avia LiDAR)

src/laserMapping.cpp

@@ -420,7 +420,7 @@ void map_incremental()
 }
 
 PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
-int ind = 0;
+PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
 void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
 {
     PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body);
@@ -434,16 +434,16 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
                             &laserCloudWorld->points[i]);
     }
 
-    *pcl_wait_pub += *laserCloudWorld;
+    // *pcl_wait_pub += *laserCloudWorld;
     if(1)
     {
         sensor_msgs::PointCloud2 laserCloudmsg;
-        pcl::toROSMsg(*pcl_wait_pub, laserCloudmsg);
+        pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
         laserCloudmsg.header.stamp = ros::Time::now();
         laserCloudmsg.header.frame_id = "camera_init";
         pubLaserCloudFullRes.publish(laserCloudmsg);
         publish_count -= PUBFRAME_PERIOD;
-        pcl_wait_pub->clear();
+        // pcl_wait_pub->clear();
     }
 
     /**************** save map ****************/
@@ -451,13 +451,8 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
     /* 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<<endl;
-        pcd_writer.writeBinary(all_points_dir, *laserCloudWorld);
+        *pcl_wait_save += *laserCloudWorld;
     }
-    ind ++;
 }
 
 void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
@@ -898,6 +893,18 @@ int main(int argc, char** argv)
         rate.sleep();
     }
 
+    /**************** 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)
+    {
+        string file_name = string("scans.pcd");
+        string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
+        pcl::PCDWriter pcd_writer;
+        cout << "current scan saved to /PCD/" << file_name<<endl;
+        pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
+    }
+
     fout_out.close();
     fout_pre.close();
 

src/preprocess.cpp

@@ -724,6 +724,7 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
         ap.x = pl[j].x;
         ap.y = pl[j].y;
         ap.z = pl[j].z;
+        ap.intensity = pl[j].intensity;
         ap.curvature = pl[j].curvature;
         pl_surf.push_back(ap);
 
@@ -744,11 +745,13 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
           ap.x += pl[k].x;
           ap.y += pl[k].y;
           ap.z += pl[k].z;
+          ap.intensity += pl[k].intensity;
           ap.curvature += pl[k].curvature;
         }
         ap.x /= (j-last_surface);
         ap.y /= (j-last_surface);
         ap.z /= (j-last_surface);
+        ap.intensity /= (j-last_surface);
         ap.curvature /= (j-last_surface);
         pl_surf.push_back(ap);
       }