diff --git a/FAST-LIO/config/hesai32.yaml b/FAST-LIO/config/hesai32.yaml
index 5f129837..f335f84a 100644
--- a/FAST-LIO/config/hesai32.yaml
+++ b/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
\ No newline at end of file
+ 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.
\ No newline at end of file
diff --git a/FAST-LIO/launch/mapping_hesai32.launch b/FAST-LIO/launch/mapping_hesai32.launch
index 5667976b..1596cf79 100644
--- a/FAST-LIO/launch/mapping_hesai32.launch
+++ b/FAST-LIO/launch/mapping_hesai32.launch
@@ -2,7 +2,7 @@
-
+
@@ -15,6 +15,7 @@
+
diff --git a/FAST-LIO/launch/mapping_ouster64_mulran.launch b/FAST-LIO/launch/mapping_ouster64_mulran.launch
index f319f594..a213da2f 100644
--- a/FAST-LIO/launch/mapping_ouster64_mulran.launch
+++ b/FAST-LIO/launch/mapping_ouster64_mulran.launch
@@ -2,7 +2,7 @@
-
+
@@ -16,6 +16,7 @@
+
diff --git a/FAST-LIO/src/laserMapping.cpp b/FAST-LIO/src/laserMapping.cpp
index d976d275..b3ab88e2 100644
--- a/FAST-LIO/src/laserMapping.cpp
+++ b/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("pcd_save/interval", pcd_save_interval, -1);
nh.param>("mapping/extrinsic_T", extrinT, vector());
nh.param>("mapping/extrinsic_R", extrinR, vector());
+ nh.param("save_directory", state_log_dir, "");
cout<<"p_pre->lidar_type "<lidar_type<points.size()< t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
diff --git a/SC-PGO/launch/fastlio_hesai32.launch b/SC-PGO/launch/fastlio_hesai32.launch
index 517b4ba0..9bbd51ed 100644
--- a/SC-PGO/launch/fastlio_hesai32.launch
+++ b/SC-PGO/launch/fastlio_hesai32.launch
@@ -1,7 +1,9 @@
-
+
+
+
diff --git a/SC-PGO/src/laserPosegraphOptimization.cpp b/SC-PGO/src/laserPosegraphOptimization.cpp
index b9a6b631..b625376a 100644
--- a/SC-PGO/src/laserPosegraphOptimization.cpp
+++ b/SC-PGO/src/laserPosegraphOptimization.cpp
@@ -513,7 +513,6 @@ void process_pg()
pcl::PointCloud::Ptr thisKeyFrame(new pcl::PointCloud());
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