save odometry poses for lidar scans

FAST-LIO/config/hesai32.yaml

@@ -25,4 +25,9 @@ mapping:
 publish:     
     scan_publish_en:  true       # 'false' will close all the point cloud output
     dense_publish_en: false       # false will low down the points number in a global-frame point clouds scan.
-    scan_bodyframe_pub_en: true  # output the point cloud scans in IMU-body-frame
+    scan_bodyframe_pub_en: true  # output the point cloud scans in IMU-body-frame
+
+pcd_save:
+    pcd_save_en: false
+    interval: -1                 # how many LiDAR frames saved in each pcd file; 
+                                 # -1 : all frames will be saved in ONE pcd file.

FAST-LIO/launch/mapping_hesai32.launch

@@ -2,7 +2,7 @@
   <!-- Launch file for hesai32 LiDAR -->
 
     <arg name="rviz" default="true" />
-
+    <arg name="save_directory" default="$(find fast_lio)/Log" />
     <rosparam command="load" file="$(find fast_lio)/config/hesai32.yaml" />
 
     <param name="feature_extract_enable" type="bool" value="0"/>
@@ -15,6 +15,7 @@
     <param name="cube_side_length" type="double" value="1000" />
     <param name="runtime_pos_log_enable" type="bool" value="0" />
     <param name="pcd_save_enable" type="bool" value="0" />
+    <param name="save_directory" type="string" value="$(arg save_directory)" />
     <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
 
     <group if="$(arg rviz)">

FAST-LIO/launch/mapping_ouster64_mulran.launch

@@ -2,7 +2,7 @@
 <!-- Launch file for ouster OS2-64 LiDAR -->
 
     <arg name="rviz" default="true" />
-
+    <arg name="save_directory" default="$(find fast_lio)/Log" />
     <rosparam command="load" file="$(find fast_lio)/config/ouster64_mulran.yaml" />
 
     <param name="feature_extract_enable" type="bool" value="0"/>
@@ -16,6 +16,7 @@
     <param name="cube_side_length" type="double" value="1000" />
     <param name="runtime_pos_log_enable" type="bool" value="0" />
     <param name="pcd_save_enable" type="bool" value="0" />
+    <param name="save_directory" type="string" value="$(arg save_directory)" />
     <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
 
     <group if="$(arg rviz)">

FAST-LIO/src/laserMapping.cpp

@@ -83,6 +83,7 @@ condition_variable sig_buffer;
 
 string root_dir = ROOT_DIR;
 string map_file_path, lid_topic, imu_topic;
+string state_log_dir;
 
 double res_mean_last = 0.05, total_residual = 0.0;
 double last_timestamp_lidar = 0, last_timestamp_imu = -1.0;
@@ -146,20 +147,20 @@ void SigHandle(int sig)
     sig_buffer.notify_all();
 }
 
-inline void dump_lio_state_to_log(FILE *fp)  
+inline void dump_lio_state_to_log(double lidar_end_time, const state_ikfom& state_point, FILE *fp)  
 {
-    V3D rot_ang(Log(state_point.rot.toRotationMatrix()));
-    fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time);
-    fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2));                   // Angle
-    fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos  
-    fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // omega  
-    fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel  
-    fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // Acc  
-    fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2));    // Bias_g  
-    fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2));    // Bias_a  
-    fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a  
-    fprintf(fp, "\r\n");  
-    fflush(fp);
+    // V3D rot_ang(Log(state_point.rot.toRotationMatrix()));
+    fprintf(fp, "%.9lf ", lidar_end_time);
+    // fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2));                   // Angle
+    fprintf(fp, "%.6lf %.6lf %.6lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos  
+    fprintf(fp, "%.9lf %.9lf %.9lf %.9lf ", state_point.rot.coeffs()[0], state_point.rot.coeffs()[1], state_point.rot.coeffs()[2], state_point.rot.coeffs()[3]);                                        // omega  
+    fprintf(fp, "%.6lf %.6lf %.6lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel  
+    // fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // Acc  
+    fprintf(fp, "%.6lf %.6lf %.6lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2));    // Bias_g  
+    fprintf(fp, "%.6lf %.6lf %.6lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2));    // Bias_a  
+    fprintf(fp, "%.6lf %.6lf %.6lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // gravity in world 
+    fprintf(fp, "\n");
+    // fflush(fp);
 }
 
 void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s)
@@ -527,7 +528,6 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
 
 void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
 {
-    ROS_INFO_STREAM("Publishing undistorted scans at " << lidar_end_time);
     int size = feats_undistort->points.size();
     PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
 
@@ -788,8 +788,9 @@ int main(int argc, char** argv)
     nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
     nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());
     nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());
+    nh.param<string>("save_directory", state_log_dir, "");
     cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
-
+    cout << "state log dir: " << state_log_dir << endl;
     path.header.stamp    = ros::Time::now();
     path.header.frame_id ="camera_init";
 
@@ -824,8 +825,8 @@ int main(int argc, char** argv)
 
     /*** debug record ***/
     FILE *fp;
-    string pos_log_dir = root_dir + "/Log/pos_log.txt";
-    fp = fopen(pos_log_dir.c_str(),"w");
+    string pos_log_filename = state_log_dir + "/pos_log.txt";
+    fp = fopen(pos_log_filename.c_str(),"w");
 
     ofstream fout_pre, fout_out, fout_dbg;
     fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out);
@@ -1005,8 +1006,8 @@ int main(int argc, char** argv)
                 ext_euler = SO3ToEuler(state_point.offset_R_L_I);
                 fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<<" "<< state_point.vel.transpose() \
                 <<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<<" "<<feats_undistort->points.size()<<endl;
-                dump_lio_state_to_log(fp);
             }
+            dump_lio_state_to_log(lidar_end_time, state_point, fp);
         }
 
         status = ros::ok();
@@ -1027,7 +1028,7 @@ int main(int argc, char** argv)
 
     fout_out.close();
     fout_pre.close();
-
+    fclose(fp);
     if (runtime_pos_log)
     {
         vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;    

SC-PGO/launch/fastlio_hesai32.launch

@@ -1,7 +1,9 @@
 <launch>
    <arg name="bagname" default="/path/to/ros.bag" />
    <arg name="save_directory" default="$(env HOME)/Desktop/temp/sc_pgo_data/" />
-   <include file="$(find fast_lio)/launch/mapping_hesai32.launch"/>
+   <include file="$(find fast_lio)/launch/mapping_hesai32.launch">
+        <arg name="save_directory" value="$(arg save_directory)"/>
+   </include>
    <include file="$(find aloam_velodyne)/launch/aloam_hesai32.launch">
      <arg name="save_directory" value="$(arg save_directory)"/>
    </include>

SC-PGO/src/laserPosegraphOptimization.cpp

@@ -513,7 +513,6 @@ void process_pg()
             pcl::PointCloud<PointType>::Ptr thisKeyFrame(new pcl::PointCloud<PointType>());
             pcl::fromROSMsg(*fullResBuf.front(), *thisKeyFrame);
             fullResBuf.pop();
-            ROS_INFO_STREAM("Pose graph optimization received scan at " << timeLaser);
             Pose6D pose_curr = getOdom(odometryBuf.front());
             odometryBuf.pop();
 
@@ -626,7 +625,6 @@ void process_pg()
                     cout << "posegraph odom node " << curr_node_idx << " added." << endl;
             }
             // if want to print the current graph, use gtSAMgraph.print("\nFactor Graph:\n");
-            ROS_INFO_STREAM("Saving scan at " << timeLaser);
             // save utility 
             std::string curr_node_idx_str = padZeros(curr_node_idx);
             pcl::io::savePCDFileBinary(pgScansDirectory + curr_node_idx_str + ".pcd", *thisKeyFrame); // scan